Tutorials/1.5/ros enabled model plugin


 * 1) Tutorial: Building a ROS-Enabled Gazebo Model Plugin

In this tutorial, we'll demonstrate how to create a simple model plugin with dependencies on ROS libraries.


 * This tutorial may not work with Gazebo Quantal and ROS-Groovy. New documentation is available for ROS integration under the Gazebo 1.9 Tutorials.**


 * 1) Install ROS

See the ROS installation page for more details. Setup ROS environments by sourcing the ROS setup script: source /opt/ros/fuerte/setup.bash


 * 1) Setup Workspace

Make sure Gazebo is installed and working on your system. Setup Gazebo environment variables by sourcing: source /usr/share/gazebo/setup.sh

Create a local workspace for this tutorial: mkdir -p ~/gazebo_ros_plugin/models/rosbot


 * 1) Simple Model

For this exercise, we'll use a very simple box shaped rosbot: ~ gedit ~/gazebo_ros_plugin/models/rosbot/model.sdf ~

Copy and save the follow SDF into the `model.sdf` file: ~  0 0 0.5 0 0 0  true 1.0        1.0         0.0         0.0         1.0         0.0         1.0           1 1 1           1 1 1    ~

Create a corresponding model config file:

~ gedit ~/gazebo_ros_plugin/models/rosbot/model.config ~

Copy and save the following XML into `model.config`

~  Rosbot 0.0.1  model.sdf Me    Me@me.org A robot with a ROS model plugin. ~

For more information on Gazebo models, see the Model Database page.


 * 1) Model Plugin

Create a model plugin that subscribes to a ROS topic:

~ gedit ~/gazebo_ros_plugin/ros_model_plugin.cc ~

Copy and save the following into `ros_model_plugin.cc`: ~
 * 1) include 
 * 2) include 
 * 3) include 
 * 4) include 
 * 5) include 


 * 1) include "ros/ros.h"
 * 2) include "std_msgs/Float64.h"

namespace gazebo {    class ROSModelPlugin : public ModelPlugin {

public: ROSModelPlugin {

// Start up ROS std::string name = "ros_model_plugin_node"; int argc = 0; ros::init(argc, NULL, name);

}   public: ~ROSModelPlugin {     delete this->node; }

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

// ROS Nodehandle this->node = new ros::NodeHandle("~");

// ROS Subscriber this->sub = this->node->subscribe("x", 1000, &ROSModelPlugin::ROSCallback, this );

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

// Called by the world update start event public: void OnUpdate {     ros::spinOnce; }

void ROSCallback(const std_msgs::Float64::ConstPtr& msg) {     ROS_INFO("subscriber got: [%f]", msg->data); }

// Pointer to the model private: physics::ModelPtr model;

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

// ROS Nodehandle private: ros::NodeHandle* node;

// ROS Subscriber ros::Subscriber sub; };

// Register this plugin with the simulator GZ_REGISTER_MODEL_PLUGIN(ROSModelPlugin) } ~


 * 1) World File

Create a world file and save it as `~/gazebo_ros_plugin/my.world` for convenience,

~ gedit ~/gazebo_ros_plugin/my.world ~

Copy the following into the file and save:

~   0 0 0          quick 0.001 40          1.0           0.0           0.2           100.0 0.0</contact_surface_layer> model://sun model://ground_plane model://rosbot 0 0 1 0 0 0 ~


 * 1) Setup Cmake

Create a stripped down version of rosbuild's public.cmake:

~ mkdir ~/gazebo_ros_plugin/cmake/ gedit ~/gazebo_ros_plugin/cmake/rospack.cmake ~

Copy and save the following into this file:

~ macro(_list_to_string _string _list) set(${_string}) foreach(_item ${_list}) string(LENGTH "${${_string}}" _len) if(${_len} GREATER 0) set(${_string} "${${_string}} ${_item}") else(${_len} GREATER 0) set(${_string} "${_item}") endif(${_len} GREATER 0) endforeach(_item) endmacro

macro(invoke_rospack pkgname _prefix _varname) # Check that our cached location of rospack is valid. It can be invalid # if rospack has moved since last time we ran, #1154. If it's invalid, # search again. if(NOT EXISTS ${ROSPACK_EXE}) message("Cached location of rospack is invalid; searching for rospack...") set(ROSPACK_EXE ROSPACK_EXE-NOTFOUND) # Only look in PATH for rospack, #3831 find_program(ROSPACK_EXE NAMES rospack DOC "rospack executable" NO_CMAKE_PATH NO_CMAKE_ENVIRONMENT_PATH NO_CMAKE_SYSTEM_PATH) if (NOT ROSPACK_EXE) message(FATAL_ERROR "Couldn't find rospack. Please source the appropriate ROS configuration file (e.g., /opt/ros/fuerte/setup.sh)") endif(NOT ROSPACK_EXE) endif(NOT EXISTS ${ROSPACK_EXE}) set(_rospack_invoke_result) execute_process(   COMMAND ${ROSPACK_EXE} ${ARGN} ${pkgname}    OUTPUT_VARIABLE _rospack_invoke_result    ERROR_VARIABLE _rospack_err_ignore    RESULT_VARIABLE _rospack_failed    OUTPUT_STRIP_TRAILING_WHITESPACE  ) if (_rospack_failed) #set(_rospack_${_varname} "") #set(${_prefix}_${_varname} "" CACHE INTERNAL "") message("Failed to invoke ${ROSPACK_EXE} ${ARGN} ${pkgname}") message("${_rospack_err_ignore}") message("${_rospack_invoke_result}") message(FATAL_ERROR "\nFailed to invoke rospack to get compile flags for package '${pkgname}'. Look above for errors from rospack itself.  Aborting.  Please fix the broken dependency!\n") else(_rospack_failed) separate_arguments(_rospack_invoke_result) set(_rospack_${_varname} ${_rospack_invoke_result}) # We don't cache results that contain newlines, because # they make CMake's cache unhappy. This check should only affect calls # to `rospack plugins`, which we don't need to cache. if(_rospack_invoke_result MATCHES ".*\n.*") set(${_prefix}_${_varname} "${_rospack_invoke_result}") else(_rospack_invoke_result MATCHES ".*\n.*") set(${_prefix}_${_varname} "${_rospack_invoke_result}" CACHE INTERNAL "") endif(_rospack_invoke_result MATCHES ".*\n.*") endif(_rospack_failed) endmacro

macro(get_rospack_flags pkgname) # Get the include dirs set(_prefix ${pkgname}) invoke_rospack(${pkgname} ${_prefix} "INCLUDE_DIRS" cflags-only-I) #message("${pkgname} include dirs: ${${_prefix}_INCLUDE_DIRS}") #set(${_prefix}_INCLUDE_DIRS ${${_prefix}_INCLUDE_DIRS} CACHE INTERNAL "")

# Get the other cflags invoke_rospack(${pkgname} ${_prefix} temp cflags-only-other) _list_to_string(${_prefix}_CFLAGS_OTHER "${${_prefix}_temp}") #message("${pkgname} other cflags: ${${_prefix}_CFLAGS_OTHER}") set(${_prefix}_CFLAGS_OTHER ${${_prefix}_CFLAGS_OTHER} CACHE INTERNAL "")

# Get the lib dirs invoke_rospack(${pkgname} ${_prefix} LIBRARY_DIRS libs-only-L) #message("${pkgname} library dirs: ${${_prefix}_LIBRARY_DIRS}") set(${_prefix}_LIBRARY_DIRS ${${_prefix}_LIBRARY_DIRS} CACHE INTERNAL "")

# Get the libs invoke_rospack(${pkgname} ${_prefix} LIBRARIES libs-only-l) # # The following code removes duplicate libraries from the link line, # saving only the last one. # list(REVERSE ${_prefix}_LIBRARIES) list(REMOVE_DUPLICATES ${_prefix}_LIBRARIES) list(REVERSE ${_prefix}_LIBRARIES)

# Get the other lflags invoke_rospack(${pkgname} ${_prefix} temp libs-only-other) _list_to_string(${_prefix}_LDFLAGS_OTHER "${${_prefix}_temp}") #message("${pkgname} other ldflags: ${${_prefix}_LDFLAGS_OTHER}") set(${_prefix}_LDFLAGS_OTHER ${${_prefix}_LDFLAGS_OTHER} CACHE INTERNAL "") endmacro ~

Next, create a corresponding `~/gazebo_ros_plugin/CMakeLists.txt`

~ gedit ~/gazebo_ros_plugin/CMakeLists.txt ~

Copy and save the following into this file:

~ cmake_minimum_required(VERSION 2.8)

include (FindPkgConfig) if (PKG_CONFIG_FOUND) pkg_check_modules(GAZEBO gazebo REQUIRED) else message(FATAL_ERROR "pkg-config is required; please install it") endif

include_directories(${GAZEBO_INCLUDE_DIRS}) link_directories(${GAZEBO_LIBRARY_DIRS})

include(cmake/rospack.cmake)

get_rospack_flags(roscpp) include_directories(${roscpp_INCLUDE_DIRS}) link_directories(${roscpp_LIBRARY_DIRS}) set(CMAKE_INSTALL_RPATH "${roscpp_LIBRARY_DIRS}") add_library(ros_model_plugin SHARED ros_model_plugin.cc) set_target_properties(ros_model_plugin PROPERTIES COMPILE_FLAGS "${roscpp_CFLAGS_OTHER}") set_target_properties(ros_model_plugin PROPERTIES LINK_FLAGS "${roscpp_LDFLAGS_OTHER}") target_link_libraries(ros_model_plugin ${roscpp_LIBRARIES}) install (TARGETS ros_model_plugin DESTINATION ${CMAKE_INSTALL_PREFIX}/lib/gazebo_plugins/) ~


 * 1) Compiling the plugin

Compile normally as with any cmake project:

~ mkdir ~/gazebo_ros_plugin/build cd ~/gazebo_ros_plugin/build cmake -DCMAKE_INSTALL_PREFIX=~/local .. make make install ~


 * 1) Running Simulation with Custom Plugin

Start a ROS master:

~ source /opt/ros/fuerte/setup.bash roscore ~

In another terminal, add Gazebo plugin and model paths and start Gazebo with your world file:In a second terminal, start roscore:

~

roscore ~

~ source /opt/ros/fuerte/setup.bash export GAZEBO_PLUGIN_PATH=~/local/lib/gazebo_plugins:${GAZEBO_PLUGIN_PATH} export GAZEBO_MODEL_PATH=~/gazebo_ros_plugin/models:${GAZEBO_MODEL_PATH} gazebo ~/gazebo_ros_plugin/my.world ~

In yet another terminal, publish over ROS topic to test current plugin:

~ source /opt/ros/fuerte/setup.bash rostopic pub -r 10 /ros_model_plugin_node/x std_msgs/Float64 '{data: 1 }' ~

You should see in the terminal where gazebo was started:

~ $ gazebo ~/gazebo_ros_plugin/my.world Gazebo multi-robot simulator, version 1.4.0 Copyright (C) 2011 Nate Koenig, John Hsu, and contributors. Released under the Apache 2 License. http://gazebosim.org

Gazebo multi-robot simulator, version 1.4.0 Copyright (C) 2011 Nate Koenig, John Hsu, and contributors. Released under the Apache 2 License. http://gazebosim.org

Msg Waiting for master Msg Connected to gazebo master @ http://localhost:11345 Msg Waiting for master Msg Connected to gazebo master @ http://localhost:11345 Warning [ODEPhysics.cc:193] Gravity vector is (0, 0, 0). Objects will float. [ERROR] [1349937977.417217009]: [registerPublisher] Failed to contact master at [localhost:11311]. Retrying... [ INFO] [1349937986.303731570]: Connected to master at [localhost:11311] [ INFO] [1349938017.054007279]: subscriber got: [1.000000] [ INFO] [1349938026.456682890]: subscriber got: [1.000000] [ INFO] [1349938051.841361135]: subscriber got: [1.000000] [ INFO] [1349938051.946408395]: subscriber got: [1.000000] [ INFO] [1349938052.040262375]: subscriber got: [1.000000] [ INFO] [1349938052.141355543]: subscriber got: [1.000000] [ INFO] [1349938052.243411125]: subscriber got: [1.000000] [ INFO] [1349938052.340168439]: subscriber got: [1.000000] [ INFO] [1349938052.440160713]: subscriber got: [1.000000] ... ~

From here, you are ready to modify your model and model plugin to mimic various I/O from the robot hardware.