SetWorldPose() of several models: very slow updates

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

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
  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
  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;


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

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

// Register this plugin with the simulator
}  // 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?


edit retag flag offensive close merge delete