In this post, I will show the reader how to control our RP2W from the previous post titled “Model Robots for Gazebo Robotics Simulator: Tutorial” with C++ code with the Gazebo API. Using Gazebo 5.0.1 and Ubuntu 14.04

This tutorial is based largely on the model plugin tutorial from the Gazebo website.

This post assumes that you have followed the previous post and you have an SDF file that models the custom RP2W robot. By the end of this tutorial, you will have a simple C++ code that can applies a constant force to every single joint of the SDF model.

First, ensure that you have the Gazebo development files. Launch terminal (shortcut CTRL+ALT+T) and type:

sudo apt-get install libgazebo5-dev

Next, create a directory (/gazebo_control) and C++ file (rp2w.cc) for your plugin:

mkdir ~/gazebo_control
cd ~/gazebo_control
gedit rp2w.cc

In your rp2w.cc file, copy+paste the following:

#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>

namespace gazebo
{
  class rp2w : public ModelPlugin
  {
    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
    {
      // Store the pointer to the model
      this->model = _parent;

      // Listen to the update event. This event is broadcast every
      // simulation iteration.
      this->updateConnection = event::Events::ConnectWorldUpdateBegin(
          boost::bind(&rp2w::OnUpdate, this, _1));
    }

    // Called by the world update start event
    public: void OnUpdate(const common::UpdateInfo & /*_info*/)
    {
    	physics::JointPtr chassis_right_wheel=model->GetJoint("chassis_right_wheel");
			physics::JointPtr chassis_left_wheel=model->GetJoint("chassis_left_wheel");
			physics::JointPtr camera_base_camera=model->GetJoint("camera_base_camera");
			physics::JointPtr body_camera_base=model->GetJoint("body_camera_base");
			physics::JointPtr body_front_wheel_y=model->GetJoint("body_front_wheel_y");
			
			chassis_right_wheel->SetForce(0,1.00);
			chassis_left_wheel->SetForce(0,1.00);	
			body_front_wheel_y->SetForce(0,1.00);	
			body_camera_base->SetForce(0,.1);	
			camera_base_camera->SetForce(0,.1);	
		}
    // Pointer to the model
    private: physics::ModelPtr model;

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

  // Register this plugin with the simulator
  GZ_REGISTER_MODEL_PLUGIN(rp2w)
}

The Load() function is called once when the plugin is loaded by Gazebo. TheOnUpdate()
function is continuously called until interrupted. You will control the SDF model’s joints inOnUpdate(). First, you need aphysics::JointPtrpointer object to access each joint in the SDF model:
physics::JointPtr /*NAME_OF_JOINT*/ = model->GetJoint("NAME_OF_JOINT")
Then, you will set the force of each joint to 1.00:
/*NAME_OF_JOINT*/->SetForce(0,1.00)

For a full list of things you can do with a Gazebo plugin, reference the Gazebo API.

Now, let’s compile the plugin. You will need to create a CMakeLists.txt file:

gedit ~/gazebo_control/CMakeLists.txt

Copy and paste the following into CMakeLists.txt:

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
find_package(Boost REQUIRED COMPONENTS system)
include_directories(${Boost_INCLUDE_DIRS})
link_directories(${Boost_LIBRARY_DIRS})
include (FindPkgConfig)
if (PKG_CONFIG_FOUND)
pkg_check_modules(GAZEBO gazebo)
endif()
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
add_library(rp2w SHARED rp2w.cc)
target_link_libraries(rp2w ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES})
list( APPEND CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS} -g -ftest-coverage -fprofile-arcs")

Now, create the build directory:

mkdir ~/gazebo_control/build

Compile:

cmake .
make

You will now have a ~/gazebo_control/build/librp2w.so that you will insert into your SDF file in order to run it in Gazebo.

Now, you will need to add your plugin’s path to GAZEBO_PLUGIN_PATH:

export GAZEBO_PLUGIN_PATH=$(GAZEBO_PLUGIN_PATH):~/gazebo_control/build

Lastly, you need to insert a tag into your SDF file in order to link your plugin to your RP2W model. If you followed the previous tutorial, you should have

<plugin name="rp2w" filename="/home/e-motion/gazebo_control/librp2w.so"/>

nested inside the <model> tag.

Start up Gazebo and insert your RP2W model into the world. You should see all of its joints in action.