The Robot Operating System, commonly known as ROS, (http://www.ros.org) is a framework for building robotics software.  Rather than build each robot control system from the ground up, ROS provides a rich software environment with code submitted by many research labs around the world.

Gazebo (http://gazebosim.org/) is a physics simulation environment intended for use in robotics applications.  In addition to dynamics solvers, Gazebo includes functionality to visualize a robot or many robots while also interacting with the simulation through a GUI.

Together, the two software frameworks allow for building robust robotic controllers and simulating their operation while freeing the experimenter from needing to build controller code from the ground up.  The ability to add common controllers such as PIDs and prebuilt sensor packages are very intriguing, but getting started in this world can be a challenge.

Evolutionary robotics adds another layer of complexity to the equation by requiring some perhaps not so common operations while running a simulation.  From what I have seen, ROS is intended to act as a real-time controller for physical robots, and hence works asynchronously with either real or simulated robotics systems.  That is, the controller does not necessarily update at well defined time intervals.  This can be a challenge when doing evolutionary experiments as we want our work to be reproducible.  The next two blog posts describe how to control the update frequency through a Gazebo plugin and then how to use that plugin in a ROS-Gazebo project.

Code for the tutorials/snippets that I describe can be found at the following Github ROS-Gazebo-Examples.

So, without further ado, I will now present the first snippet of test code that I put together to begin working with the two software frameworks.

Stepping the World Programmatically

ROS and Gazebo operate as two separate programs running on a computer and communicate with each other through TCP/IP sockets and message passing.  If left to run on their own, Gazebo will update at whatever rate the system allows given the hardware specifications and ROS will communicate through the sockets.  However, there is no guarantee that communication will occur at any steady rate.  This can make reproducing functionality difficult, if not impossible.

The solution to this problem is to throttle the physics simulation.  Within Gazebo is the concept of plugins that allow a user to insert their own code in a simulation to do many different things.  An overview of plugins can be found here.  I highly recommend reading through that before going through the rest of this post.

Although plugins allow a user to write custom code for a simulation, I could not find a tutorial out there to actually write one that controls the stepping of a simulation.  What’s needed for this is to write a publisher that throttles the rate of an update.  While trying to search for an answer, the closest I found was a tutorial on changing the gravity of a simulation (here) and one for writing custom messages (here).  With these two tutorials, I was able to come up with the basic strategy needed to make a stepping plugin.  First, we need to pause the simulation from the start.  Second, we need to step the simulation after delaying for some set amount of wall clock time.

Code Breakdown

The code for this example is found in world_step.cc

#include <sdf/sdf.hh>

#include <boost/bind.hpp>

#include "gazebo/gazebo.hh"
#include "gazebo/common/Plugin.hh"
#include "gazebo/msgs/msgs.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"

/// This example creates a WorldPlugin, initializes the Transport system by
/// creating a new Node, and publishes messages to alter gravity.
namespace gazebo
{
  class WorldEdit : public WorldPlugin
  {
    public: void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
    {
      // Create a new transport node
      this->node.reset(new transport::Node());

      // Initialize the node with the world name
      this->node->Init(_parent->GetName());

      // Create a publisher
      this->pub = this->node->Advertise<msgs::WorldControl>("~/world_control");

      // Listen to the update event. Event is broadcast every simulation 
      // iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateEnd(
      boost::bind(&WorldEdit::OnUpdate, this));


      // Configure the initial message to the system
      msgs::WorldControl worldControlMsg;

      // Set the world to paused
      worldControlMsg.set_pause(0);

      // Set the step flag to true
      worldControlMsg.set_step(1);

      // Publish the initial message. 
      this->pub->Publish(worldControlMsg);
      std::cout << "Publishing Load." << std::endl;
    }

    // Called by the world update start event.
    public: void OnUpdate() 
    {
      // Throttle Publication
      gazebo::common::Time::MSleep(1000);

      msgs::WorldControl msg;
      msg.set_step(1);
      this->pub->Publish(msg);
      std::cout << "Publishing OnUpdate." << std::endl;
    }

    // Pointer to the world_controller
    private: transport::NodePtr node;
    private: transport::PublisherPtr pub;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;
  };

  // Register this plugin with the simulator
  GZ_REGISTER_WORLD_PLUGIN(WorldEdit);
}

From the above code, we first establish the plugin specifics in the Load function.  There, we initialize a node and publisher for communicating with Gazebo in lines 20-27.  We add an event listener (lines 31-32) connected to the world update end event to trigger our OnUpdate function each time the simulation updates.  Finally, in lines 50-59, we setup the initial message to publish which pauses the simulation and sends a single step command.

Thereafter, the OnUpdate function is called at the end of an update where we wait 1 second and then step the simulation again.  By changing the value in the sleep function you can change how long between simulation updates.

Future Improvements

For now, the code simply steps the simulation after a given time delay.  It may however be advantageous to connect the update to some predetermined events and shut the simulation down after a set amount of time.  Look for those tutorials in future posts.

My next post will cover integrating this plugin within the ROS-Gazebo framework.