This post is meant as a follow-on for part 2 and documents some of the differences that I encountered trying to give control of simulation stepping to ROS-based code itself.  Since ROS and Gazebo are technically separate, I was trying to find a way to create a ROS piece of code to communicate with the Gazebo plugin that I’d been using to step the simulation.  After quite a bit of reading, it appears that it may not be possible, or simply prohibitively difficult.  Therefore, this blog post documents an alternate solution that I plan to use in the blog posts and code going forward.  Code for this blog post can be found here

General Approach:

Rather than employ the gazebo plugin from previous entries, all code now resides in one C++ file.  We’ll declare both a ROS node to get the simulation time and report it to the terminal and also a gazebo node to actually handle the stepping of the simulation.  Note: There are a few changes to the launch file in this example too so please use the accompanying world_step.launch rather than the version from previous posts.

Code Breakdown:

The primary file to look at is /src/world_step/ros_get_world_time.cpp:

#include <math.h>

#include <boost/thread.hpp>
#include <boost/algorithm/string.hpp>

#include <gazebo/gazebo.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>

#include <ros/ros.h>
#include <ros/subscribe_options.h>

int main(int argc, char** argv)
{
 gazebo::setupClient(argc,argv);
 ros::init(argc, argv, "get_world_time_test");

 // Gazebo WorldControl Topic
 gazebo::transport::NodePtr node(new gazebo::transport::Node());
 node->Init();

 // Initialize the ROSNode
 ros::NodeHandle* rosnode = new ros::NodeHandle();
 
 gazebo::transport::PublisherPtr pub = node->Advertise<gazebo::msgs::WorldControl>("~/world_control");
 
 // Waits for simulation time update.
 ros::Time last_ros_time_;
 bool wait = true;
 while (wait)
 {
 last_ros_time_ = ros::Time::now();
 std::cout << "\t\t\t Attempted getting sim time: " << last_ros_time_ << std::endl;

 if (last_ros_time_.toSec() > 0)
 //wait = false;
 std::cout << "Current Simulation Time: " << last_ros_time_ << std::endl;
 
 // Publish the step message for the simulation.
 gazebo::msgs::WorldControl msg;
 msg.set_step(1);
 pub->Publish(msg);
 
 // Wait for 1 second and allow ROS to complete as well.
 gazebo::common::Time::MSleep(1000);
 ros::spinOnce();
 }

 gazebo::shutdown();

 return 0;
}

As you can see from the code, we initially setup the gazebo and ROS nodes in lines 19-26.  Lines 28-38 are similar to those of previous examples simply getting the current simulation time and printing it to the terminal.  The main change from previous examples are in lines 40-47.  Instead of waiting on a Gazebo plugin, this code implements the world stepping code directly in the while loop.  The next two lines then simply wait one second before proceeding with stepping.

Moving Forward:

With this setup, I now plan to use this loop to setup the basic framework for conducting simulations for a set amount of time.