Gazebo Simulated Time. Control step by step

asked 2018-04-13 07:57:10 -0500

iAba gravatar image

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 <gazebo/gazebo.hh>
#include <gazebo/gazebo_client.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>

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<gazebo::msgs::WorldControl>("~/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()
edit retag flag offensive close merge delete

Comments

Did you manage to solve this ? I want to also control each step of the simulation from ros code.

gpldecha gravatar imagegpldecha ( 2018-08-09 07:45:02 -0500 )edit