Hi,
I am having problems trying to control Gazebo's simulated time. I want to command Gazebo to simulate one step after some processing is done.
I have created a publisher for gazebo's topic ~/world_control in order to publish a WorldControl message with "step = true" whenever I want Gazebo to move forward one step.
However, those messages sometimes are effective (i.e. Gazebo moves forward one step) and other times they have no effect.
As an example, in the attached code I publish 3 times the message to advance the simulation one step, so I expect Gazebo to advance 3 steps in the simulation. However, it only moves forward one step and I can't understand why.
Any explanation why this is happening? It seems to me that Gazebo is not receiving all the published messages, those messages are getting lost in the way or something like that. Any help will be very much appreciated.
Thanks,
Ignacio
#include
#include
#include
#include
#include
static bool stop_simulation;
void rosShutdownHandler(int sig)
{
stop_simulation = true;
}
int main(int argc, char **argv)
{
gazebo::client::setup(argc,argv);
gazebo::transport::NodePtr node(new gazebo::transport::Node());
node->Init();
//Create publisher for topic ~/world_control
gazebo::transport::PublisherPtr pub = node->Advertise("~/world_control");
//Create message
gazebo::msgs::WorldControl msg_step;
stop_simulation = false;
//Publish to topic ~/world_control
pub->WaitForConnection();
//Set step to true
msg_step.set_step(1);
//Publish 1st step
pub->Publish(msg_step, true);
//Publish 2nd step
pub->Publish(msg_step, true);
//Publish 3rd step
pub->Publish(msg_step, true);
while (!stop_simulation){
std::cout << "Running \n";
}
gazebo::shutdown();
return 0;
} // end main()
↧