Gazebo | Ignition | Community
Ask Your Question
0

Gazebo Simulated Time. Control step by step

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

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 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-02-18 20:41:13 -0600

This code worked for me:

#include <gazebo/gazebo.hh>
#include <gazebo/gazebo_client.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <functional>  // To use std::bind
#include "plen_ros/Iterate.h"


bool iterateCallback(plen_ros::Iterate::Request& req, plen_ros::Iterate::Response& res, const gazebo::transport::PublisherPtr& pub)
{
  // Create Iterate msg
  gazebo::msgs::WorldControl stepper;
  // Set multi-step to requested iterations
  stepper.set_multi_step(req.iterations);
  pub->Publish(stepper);

  res.result = true;

  return true;
}

int main(int argc, char **argv)
{
gazebo::client::setup(argc,argv);

gazebo::transport::NodePtr node(new gazebo::transport::Node());
node->Init();

ros::init(argc, argv, "gazebo_iterator"); // register the node on ROS
ros::NodeHandle nh; // get a handle to ROS

//Create publisher for topic ~/world_control
gazebo::transport::PublisherPtr pub = node->Advertise<gazebo::msgs::WorldControl>("~/world_control");
//Publish to topic ~/world_control
pub->WaitForConnection();

// Create Iterate Service Server
ros::ServiceServer iter_server = nh.advertiseService<plen_ros::Iterate::Request,
                                    plen_ros::Iterate::Response>("/iterate", std::bind(&iterateCallback,
                                                               std::placeholders::_1, 
                                                               std::placeholders::_2,
                                                               pub));

//Publish 1st step

// pub->Publish(stepper, true);

while (ros::ok())
  {
    ros::spinOnce();
  }
return 0;
} // end main()

Using this code, you can call my "/iterate" service and specify the number of iterations.

However, note that you cannot (to my knowledge) issue joint commands using the ros_control package or read from any topics while the sim is paused. Iterating does not change this.

edit flag offensive delete link more

Comments

Could I know what is in your srv file? you use the Iterate.h but I have no idea what should be include

Donghao gravatar imageDonghao ( 2020-06-29 20:46:49 -0600 )edit

I am not sure I understand. Is gazebo paused when this service is called?

devjo gravatar imagedevjo ( 2020-09-08 02:49:15 -0600 )edit
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2018-04-13 07:57:10 -0600

Seen: 1,266 times

Last updated: Apr 13 '18