Hi everybody, I am currently working on a Gazebo Plugin. It must measure the speed of the wheels of the robot and print the values at a given frequency.
This is the core of the code I wrote until now:
public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
{
this->model = _parent;
this->updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&encoder_plugin::OnUpdate, this));
}
public: void OnUpdate()
{
this->joint1_ = this->model->GetJoint("Left_wheel"); //measurement of the speed
this->joint2_ = this->model->GetJoint("Right_wheel");
speed_dx = this->joint1_->GetVelocity(0);
speed_sx = this->joint2_->GetVelocity(0);
usleep(5000); //Sleep in order to print the values at the given frequency
printf ("Left wheel speed %f rad/s\n",speed_sx);
printf ("Right wheel speed %f rad/s\n", speed_dx);
printf("------");
}
This code slows down the simulation, because it decreases the Real Time Factor down to 0.15. I think that this is probably due to the sleep on the OnUpdate function. Is there a way to do this without using sleep functions and without slowing down Gazebo too much? I tried to measure the Simulation Time but I couldn't get results.
Thank you very much.
↧