Category Archives: ROS-Gazebo

Genetic Algorithm with Multiple ROS Instances

Note: Source code discussed in this post is available here.  Specifically, we’ll be working with the adding_service ROS package.

Genetic algorithms are “pleasantly parallel” in that the evaluation phase can be distributed across multiple cores of a single machine or a cluster to better utilize hardware and lower running time of the algorithm.  When working with ROS, however, there are some platform specific considerations that need to be made in order to fully exploit parallel evaluations.

In this blog post, we will explore a genetic algorithm that evolves a genome of floats to maximize their sum.  ROS will perform the addition while a Python implementation of a GA will be run outside the scope of ROS.  The GA will communicate with ROS instances through the use of ZeroMQ messages.  This code is intended to provide a minimal working example of how to pass a genome into ROS and retrieve fitness values out.  It can be extended by a user as needed to implement more complex evaluations or a more advanced GA.

Communicating between External GA and ROS Instance

During an evaluation, information is passed between the external GA and the ROS instance through a combination of ZeroMQ messages (external) and ROS parameters and topics (internal).  The flow of information can be seen in the following diagram for a single evaluation.  Adder_Transporter handles the message passing and Adder_Worker handles the actual evaluation.

This is the information flow between a GA and ROS instance for a single evaluation. The GA sends the genome over a ZeroMQ message. A transport node handles the message, loads the genome into a ROS parameter and then alerts the worker that there is an evaluation to be performed. Once the evaluation is complete, the fitness value is sent back to the transporter, is converted to a ZeroMQ message and delivered back to the GA.

This is the information flow between a GA and ROS instance for a single evaluation. The GA sends the genome over a ZeroMQ message. A transport node handles the message, loads the genome into a ROS parameter and then alerts the worker that there is an evaluation to be performed. Once the evaluation is complete, the fitness value is sent back to the transporter, is converted to a ZeroMQ message and delivered back to the GA.

Parallelizing ROS

Alone, an individual ROS instance typically handles all facets of a controller, physics simulation, and any other nodes that are needed to evaluate a problem.  Typically, an instance is launched from within a launch file that specifies the individual ROS nodes along with any additional configuration needed.  However, an individual ROS instance cannot perform multiple evaluations simultaneously.  Thankfully, there is a mechanism within ROS to allow for many parallel ROS instances to be run using the group tag in the launch file along with a namespace argument specified with ns.

For the example GA discussed in this post, a single ROS instance contains two nodes.  The first, adder_transporter, handles communication with the GA and passes information onto an adding node.  adder_worker handles the actual summation of the genome and sends the value back to adder_transporter.  Fitness and the genome ID are then returned to the GA.

<node name="adder_transporter" pkg="adding_service" type="adder_transporter.py" output="screen"></node>
<node name="adder_worker" pkg="adding_service" type="adder.py" output="screen"></node>

Alone, these two nodes are sufficient to allow the GA to evaluate the complete population, but it does not harness all cores available on a machine.  Instead, multiple instances are needed to parallelize the evaluation step.  Wrapping the node setup code with the group tag allows us to create multiple ROS instances.  Thus, the full launch script for this example is available in adding_service.launch

<launch>

<group ns="adder0">
<node name="adder_transporter" pkg="adding_service" type="adder_transporter.py" output="screen"></node>
<node name="adder_worker" pkg="adding_service" type="adder.py" output="screen"></node>
</group>

<group ns="adder1">
<node name="adder_transporter" pkg="adding_service" type="adder_transporter.py" output="screen"></node>
<node name="adder_worker" pkg="adding_service" type="adder.py" output="screen"></node>
</group>

<group ns="adder2">
<node name="adder_transporter" pkg="adding_service" type="adder_transporter.py" output="screen"></node>
<node name="adder_worker" pkg="adding_service" type="adder.py" output="screen"></node>
</group>

</launch>

More instances could be added by copy/pasting the code within the group tags and changing the ns attribute accordingly.

Running the Example

The genetic algorithm can be seen in /src/adding_service/test/ga_server.py while a single addition can be seen in /src/adding_service/test/server.py.  To run the example, open two terminal tabs and change to the ros_gazebo_python directory.  Run the command:

source devel/setup.sh

in each terminal.  Then, in one terminal, start the ROS instances with the command:

roslaunch adding_service adding_service.launch

The GA can then be launched in the other terminal with:

python src/adding_service/test/ga_server.py

Outstanding Issues:

There is a possibility that if ROS isn’t up and running before the GA is launched the GA will not work.  I am currently trying to determine what causes this and what a potential resolution is.  I plan to edit this post once I figure that out.

ROS-Gazebo for Evolutionary Robotics: Stepping the World Pt. 2.5

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.

ROS-Gazebo for Evolutionary Robotics: Stepping the World Pt. 2

After setting up controlled simulation stepping in a pure Gazebo environment (here), we’ll now move towards integrating the plugin in a ROS-Gazebo simulation.  For this post, the codebase for the plugin is unchanged and only a few lines of code are needed to get this working.  You can find the source for this post here.

There are two tutorials I recommend reading through.  The first covers the basics of using Gazebo plugins with ROS while the second provides a better overview of integrating Gazebo plugins with ROS.

Step the World with ROS

The code needed to integrate our plugin from the previous post requires the following insertion/modifications.

Insert the following line into the package.xml file located in src/world_step/ between the export tags:

<gazebo_ros plugin_path="${prefix}/lib" gazebo_media_path="${prefix}"/>

Add/modify the following lines to your CMakelists.txt located in src/world_step/:

find_package(catkin REQUIRED COMPONENTS
  roscpp
  gazebo_ros
)

find_package(gazebo REQUIRED)

include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})

link_directories(${GAZEBO_LIBRARY_DIRS})

add_library(world_step src/plugins/world_step.cc)
target_link_libraries(world_step ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

These two changes should be all that’s needed to build the plugin from the previous example, provided you correctly move the world_step.cc file into this project of course.

Launching the environment requires you to build the project using the following:

catkin_make
source devel/setup.sh

The enter the following command to start ROS-Gazebo:

roslaunch world_step world_step.launch

If all goes well, you should see something like the image below with the iterations and simulation time corresponding to each other and updating roughly every second of real world time.  In the terminal where you called roslaunch you should see the repeating message “Publishing OnUpdate.” which is printed each time the simulation is stepped.

Adding the OnUpdate() call allows for stepping a Gazebo simulation while using ROS as well. The iterations, simulation time, and real time show that the simulation is being stepped once every second of real time.

Adding the OnUpdate() call allows for stepping a Gazebo simulation while using ROS as well. The iterations, simulation time, and real time show that the simulation is being stepped once every second of real time.

Future Extensions

Now that we can step the world programmatically, the next steps are to integrate logic to start and terminate a simulation after a set amount of time and begin writing the evolutionary framework to launch many simulations.

ROS-Gazebo for Evolutionary Robotics: Stepping the World Pt. 1

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.