SetWorldPose() of several models: very slow updates
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!
Asked by ROS_GAZEBO_USER on 2021-01-14 00:06:00 UTC
Comments