SetWorldPose() of several models: very slow updates

asked 2021-01-13 23:06:00 -0600

ROS_GAZEBO_USER gravatar image

I'm using this simple plugin to move a model through a sinusoidal trajectory:

#include <functional>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math.hh>

namespace gazebo
{
class ModelPush : public ModelPlugin
{
public:
  void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
  {
    this->model = _parent;
    this->world = _parent->GetWorld();  // physics::get_world(worldName);
    this->updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&ModelPush::OnUpdate, this));
  }

  // Called by the world update start event
public:
  void OnUpdate()
  {
    double time = this->world->SimTime().Double();
    ignition::math::Pose3d pose(0.0, 0.0, sin(time), 0.0, 0.0, 0.0);  // = orig_pose;
    this->model->SetWorldPose(pose);

  }

  // Pointer to the model
private:
  physics::ModelPtr model;
  physics::WorldPtr world;

  // Pointer to the update event connection
private:
  event::ConnectionPtr updateConnection;
};

// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(ModelPush)
}  // namespace gazebo

It works perfectly when I have only one model in the world. However, as soon as I spawn several models (around 5), each one equipped with this plugin, Gazebo goes extremely slow, the models move super clumsily and the real-time-factor goes down to literally 0.0. I tried both with the physics enabled and disabled, and got the same result. I'm using Gazebo 9.0, and each model is simply a box.

Is there anything I am doing wrong? How could I speed this up?

Thanks!

edit retag flag offensive close merge delete