How to create Gazebo plugins in ROS, Part 2

In this post, you will learn how to create Gazebo plugins by creating a more complex one integrated with ROS subscribers, to make an EarthQuake plugin. This is the second part of the response to the question posted on Gazebo Answers.

Before following this post, please ensure you have read and implemented the previous post, which created a simple plugin that merely printed “Hello World!” This post assumes you have read the first post. Let’s go!

Step 1: Add the source code for the new Gazebo plugin: EarthQuake

Create a new file in the src folder of the example_plugins_gazebo package.

cd ~/catkin_ws/src/example_plugins_gazebo/src
touch earthquake.cc

Now paste the following source code into the earthquake.cc file. Please read the explanations within the code.

#include <functional>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ignition/math/Vector3.hh>
#include <thread>
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>

namespace gazebo
{
  class ModelQuake : 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(
          std::bind(&amp;ModelQuake::OnUpdate, this));
      
      this->old_secs =ros::Time::now().toSec();
      
      // Create a topic name
      std::string earthquake_freq_topicName = "/earthquake_freq";
      std::string earthquake_magnitud_topicName = "/earthquake_magnitud";

      // Initialize ros, if it has not already bee initialized.
      if (!ros::isInitialized())
      {
        int argc = 0;
        char **argv = NULL;
        ros::init(argc, argv, "earthquake_rosnode",
            ros::init_options::NoSigintHandler);
      }
         
      // Create our ROS node. This acts in a similar manner to
      // the Gazebo node
      this->rosNode.reset(new ros::NodeHandle("earthquake_rosnode"));
      
      // Freq
      ros::SubscribeOptions so =
        ros::SubscribeOptions::create<std_msgs::Float32>(
            earthquake_freq_topicName,
            1,
            boost::bind(&amp;ModelQuake::OnRosMsg, this, _1),
            ros::VoidPtr(), &amp;this->rosQueue);
      this->rosSub = this->rosNode->subscribe(so);
      
      // Spin up the queue helper thread.
      this->rosQueueThread =
        std::thread(std::bind(&amp;ModelQuake::QueueThread, this));
        
        
      // Magnitude
      ros::SubscribeOptions so2 =
        ros::SubscribeOptions::create<std_msgs::Float32>(
            earthquake_magnitud_topicName,
            1,
            boost::bind(&amp;ModelQuake::OnRosMsg_Magn, this, _1),
            ros::VoidPtr(), &amp;this->rosQueue2);
      this->rosSub2 = this->rosNode->subscribe(so2);
      
      // Spin up the queue helper thread.
      this->rosQueueThread2 =
        std::thread(std::bind(&amp;ModelQuake::QueueThread2, this));
         
      ROS_WARN("Loaded ModelQuake Plugin with parent...%s, only X Axis Freq Supported in this V-1.0", this->model->GetName().c_str());
      
    }

    // Called by the world update start event
    public: void OnUpdate()
    {
      double new_secs =ros::Time::now().toSec();
      double delta = new_secs - this->old_secs;
      
      double max_delta = 0.0;
      
      if (this->x_axis_freq != 0.0)
      {
        max_delta = 1.0 / this->x_axis_freq;
      }
      
      double magnitude_speed = this->x_axis_magn;
      
      
      if (delta > max_delta &amp;&amp; delta != 0.0)
      {
        // We change Direction
        this->direction = this->direction * -1;
        this->old_secs = new_secs;
        ROS_DEBUG("Change Direction...");
      }
      
      double speed = magnitude_speed * this->direction;
      
      // Apply a small linear velocity to the model.
      this->model->SetLinearVel(ignition::math::Vector3d(speed, 0, 0));
      this->model->SetAngularVel(ignition::math::Vector3d(0, 0, 0));
    }
    
    
    public: void SetFrequency(const double &amp;_freq)
    {
      this->x_axis_freq = _freq;
      ROS_WARN("x_axis_freq >> %f", this->x_axis_freq);
    }
    
    public: void SetMagnitude(const double &amp;_magn)
    {
      this->x_axis_magn = _magn;
      ROS_WARN("x_axis_magn >> %f", this->x_axis_magn);
    }
    
    
    public: void OnRosMsg(const std_msgs::Float32ConstPtr &amp;_msg)
    {
      this->SetFrequency(_msg->data);
    }
    
    /// \brief ROS helper function that processes messages
    private: void QueueThread()
    {
      static const double timeout = 0.01;
      while (this->rosNode->ok())
      {
        this->rosQueue.callAvailable(ros::WallDuration(timeout));
      }
    }
    
    public: void OnRosMsg_Magn(const std_msgs::Float32ConstPtr &amp;_msg)
    {
      this->SetMagnitude(_msg->data);
    }
    
    /// \brief ROS helper function that processes messages
    private: void QueueThread2()
    {
      static const double timeout = 0.01;
      while (this->rosNode->ok())
      {
        this->rosQueue2.callAvailable(ros::WallDuration(timeout));
      }
    }
    

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

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;
    
    // Time Memory
    double old_secs;
    
    // Direction Value
    int direction = 1;
    // Frequency of earthquake
    double x_axis_freq = 1.0;
    // Magnitude of the Oscilations
    double x_axis_magn = 1.0;
    
    /// \brief A node use for ROS transport
    private: std::unique_ptr<ros::NodeHandle> rosNode;
    
    /// \brief A ROS subscriber
    private: ros::Subscriber rosSub;
    /// \brief A ROS callbackqueue that helps process messages
    private: ros::CallbackQueue rosQueue;
    /// \brief A thread the keeps running the rosQueue
    private: std::thread rosQueueThread;
    
    
    /// \brief A ROS subscriber
    private: ros::Subscriber rosSub2;
    /// \brief A ROS callbackqueue that helps process messages
    private: ros::CallbackQueue rosQueue2;
    /// \brief A thread the keeps running the rosQueue
    private: std::thread rosQueueThread2;
    
  };

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

Add the new source to your CMakeLists.txt file so that it now looks like this:

cmake_minimum_required(VERSION 2.8.3)
project(example_plugins_gazebo)

## Add support for C++11, supported in ROS Kinetic and newer
add_definitions(-std=c++11)

# Load catkin and all dependencies required for this package
find_package(catkin REQUIRED COMPONENTS 
  roscpp 
  gazebo_ros 
)

# Depend on system install of Gazebo
find_package(gazebo REQUIRED)

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

# For tc_simple_world_plugin plugin
add_library(simple_world_plugin src/simple_world_plugin.cpp)
target_link_libraries(simple_world_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

# For earthquake_plugin plugin
add_library(earthquake src/earthquake.cc)
target_link_libraries(earthquake ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

catkin_package(
  DEPENDS 
    roscpp 
    gazebo_ros 
)

Step 2: Create a new world that will use the Gazebo plugin file we just created

Open a web shell and run the following commands to create the package. When creating Gazebo plugins, you need, at a minimum, the roscpp and gazebo_ros dependencies.

cd ~/catkin_ws/src/example_plugins_gazebo/worlds
touch earthquake.world

Open earthquake.world and paste in the following:

<?xml version="1.0"?> 
<sdf version="1.4">
  <world name="default">
  
    <include>
      <uri>model://sun</uri>
    </include>
    <include>
      <uri>model://ground_plane</uri>
    </include>


    <!-- CUSTOM Ground Plane -->
    <model name="custom_ground_plane_box">
      <pose>0 0 0.1 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>20 20 0.1</size>
            </box>
          </geometry>
          <surface>
              <friction>
                <ode>
                  <mu>10000000000</mu>
                  <mu2>10000000000</mu2>
                </ode>
              </friction>
            </surface>
        </collision>

        <visual name="visual">
          <geometry>
            <box>
              <size>20 20 0.1</size>
            </box>
          </geometry>
          <material>
              <script>
                <uri>file://media/materials/scripts/gazebo.material</uri>
                <name>Gazebo/Green</name>
              </script>
            </material>
        </visual>
        <inertial>
          <pose>0 0 0 0 0 0</pose>
          <mass>100</mass>
          <inertia>
            <ixx>3.33341666667e+3</ixx>
            <ixy>0.0</ixy>
            <ixz>0.0</ixz>
            <iyy>3.33341666667e+3</iyy>
            <iyz>0.0</iyz>
            <izz>6.66666666667e+3</izz>
          </inertia>
        </inertial>
      </link>
      <plugin name="earthquake_plugin" filename="libearthquake.so"/>
    </model>    
 
    <model name="box">
      <pose>0 0 1.5 0 0 0</pose>
      <link name="link">
        <collision name="collision">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
          <surface>
              <friction>
                <ode>
                  <mu>10000000000</mu>
                  <mu2>10000000000</mu2>
                </ode>
              </friction>
            </surface>
        </collision>

        <visual name="visual">
          <geometry>
            <box>
              <size>1 1 1</size>
            </box>
          </geometry>
        </visual>
      </link>
    </model>        
  </world>
</sdf>

Finally, create a launch file that launches the new world

cd ~/catkin_ws/src/example_plugins_gazebo/launch
touch earthquake.launch

Open earthquake.launch in the code editor and paste the following text:

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find example_plugins_gazebo)/worlds/earthquake.world"/>
  </include>
</launch>

So much for all the hard work – now is the time to see if it works. Time to compile the code. In the same web shell, run the following commands:

cd ~/catkin_ws
rm -rf build/ devel/
catkin_make
source devel/setup.bash

Success! We have now created a new plugin. Next, we will use it!

PS: if your code did not compile correctly, please go over the instructions and ensure you have created the files in the exact locations specified.

Step 3: Launch the new world with the Gazebo plugin we just created

Time to see the result of our labor:

roslaunch example_plugins_gazebo earthquake.launch

Now you should see some yellow warning printed in the terminal and a green plane with an ash-colored box on it. What – is the plane moving? Yes, it’s having an earthquake!

But that’s not all! Do you remember the two topics defined in the plugin code? We can publish to them and change the “course of history”. Try these on two new terminals and see what happens (keep the roslaunch command running).

cd ~/catkin_ws
source devel/setup.bash
rostopic pub /earthquake_freq std_msgs/Float32 "data: 2.0"

cd ~/catkin_ws
source devel/setup.bash
rostopic pub /earthquake_magnitud std_msgs/Float32 "data: 3.0"

What happened there? Can you save the situation and stop the earthquake?

Step 4: Check your learning

Do you understand how to create Gazebo plugins? If you don’t know it yet, please go over the post again, more carefully this time.

Here are the links to the resources used in this tutorial:

• http://gazebosim.org/tutorials/?tut=plugins_hello_world
• http://gazebosim.org/tutorials?tut=ros_gzplugins
• http://gazebosim.org/tutorials?tut=guided_i5
• http://gazebosim.org/tutorials/?tut=ros_advanced
• http://gazebosim.org/tutorials?tut=plugins_model

(Extra) Step 5: Watch the video to understand how to create Gazebo plugins

Here you go:

Feedback

Did you like this post? Do you have any questions about how to create Gazebo plugins? Please leave a comment in the comments section below, so we can interact and learn from each other.

If you want to learn about other ROS topics, please let us know in the comments area and we will do a video or post about it.

How to create Gazebo plugins in ROS, Part 1

In this post, you will learn how to create Gazebo plugins by making the Gazebo plugin tutorial, Hello World, compile and work in a ROS package. This is a response to the question posted on Gazebo Answers.

Step 1: Fire up a system with ROS installation

“Hey, do you mean I have to install ros2 first?” Absolutely not! Just log in to The Construct to get access to virtual machines pre-installed with ROS.

Once logged in, click on My Rosjects, then Create a New Rosject, supply the information as shown in the image below, and click Create. Then RUN the rosject.

You might also want to try this on a local PC if you have ROS installed. However, please note that we cannot support local PCs and you will have to fix any errors you run into on your own. The rest of the instruction assumes that you are working on The Construct; please adapt them to your local PC and ROS installation.

PS: We are using ROS Kinetic here, but you should be able to use any ROS 1 distro.

Step 2: Create a new package that we’ll use to demonstrate creating Gazebo plugins

Open a web shell and run the following commands to create the package. When creating Gazebo plugins, you need, at a minimum, the roscpp and gazebo_ros dependencies.

cd catkin_ws/src
source /opt/ros/kinetic/setup.bash
catkin_create_pkg example_plugins_gazebo roscpp gazebo_ros

Create a new C++ file in the package you just created:

cd example_plugins_gazebo/src
touch simple_world_plugin.cpp

Now head over to the Code Editor to make changes to the C++ file. Check the image below for how to open the Code Editor.

Locate the C++ file in the code editor: catkin_ws > src > example_plugins_gazebo > src > simple_world_plugin.cpp and paste in the following code.

#include <gazebo/common/Plugin.hh>
#include <ros/ros.h>

namespace gazebo
{
class WorldPluginTutorial : public WorldPlugin
{
public:
  WorldPluginTutorial() : WorldPlugin()
  {
  }

  void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
  {
    // Make sure the ROS node for Gazebo has already been initialized                                                                                    
    if (!ros::isInitialized())
    {
      ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
        << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
      return;
    }

    ROS_WARN("Hello World!");
  }

};
GZ_REGISTER_WORLD_PLUGIN(WorldPluginTutorial)
}

Next, replace the package’s CMakeLists.txt with the following:

cmake_minimum_required(VERSION 2.8.3)
project(example_plugins_gazebo)

## Add support for C++11, supported in ROS Kinetic and newer
add_definitions(-std=c++11)

# Load catkin and all dependencies required for this package
find_package(catkin REQUIRED COMPONENTS 
  roscpp 
  gazebo_ros 
)

# Depend on system install of Gazebo
find_package(gazebo REQUIRED)

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

# For tc_simple_world_plugin plugin
add_library(simple_world_plugin src/simple_world_plugin.cpp)
target_link_libraries(simple_world_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

catkin_package(
  DEPENDS 
    roscpp 
    gazebo_ros 
)

Also, replace the package.xml file content with the following:

<?xml version="1.0"?>
<package>
  <name>example_plugins_gazebo</name>
  <version>0.0.0</version>
  <description>The example_plugins_gazebo package</description>
  <maintainer email="name@email.com">user</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>gazebo_ros</build_depend>
  <build_depend>roscpp</build_depend>
  <run_depend>gazebo_ros</run_depend>
  <run_depend>roscpp</run_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <gazebo_ros plugin_path="${prefix}/lib" gazebo_media_path="${prefix}" />
  </export>
</package>

So much for all the hard work – now is the time to see if it works. Time to compile the code. In the same web shell, run the following commands:

cd ~/catkin_ws
catkin_make
source devel/setup.bash

Success! We have now created a new plugin. Next, we will use it!

PS: if your code did not compile correctly, please go over the instructions and ensure you have created the files in the exact locations specified.

Step 3: Use the new Gazebo plugin we just created

Create a new folder in the package. We’ll create Gazebo worlds in it, so it’s so named!

cd ~/catkin_ws/src/example_plugins_gazebo
mkdir -p worlds
cd worlds
touch simple_world.world

Open the simple_world.world file in the code editor and paste in the following:

<?xml version="1.0" ?>
<sdf version="1.4">
  <world name="default">
    <include>
      <uri>model://ground_plane</uri>
    </include>

    <include>
      <uri>model://sun</uri>
    </include>

    <!-- reference to your plugin -->
    <plugin name="simple_world_plugin" filename="libsimple_world_plugin.so"/>
  </world>
</sdf>

But, wait…how did we know the right filename to use in the line highlighted in the world file? It is by convention named lib{name_of_cpp_executable}.so, and we should find it in ~/catkin_ws/src/devel/lib/

cd ~/catkin_ws/devel/lib/
ls

That covered, we move. Now we’ll create a launch file to launch our world with the Gazebo plugin.

cd ~/catkin_ws/src/example_plugins_gazebo
mkdir -p launch
cd launch
touch simple_world.launch

Open the simple_world.launch file in the code editor and paste in the following content.

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find example_plugins_gazebo)/worlds/simple_world.world"/>
  </include>
</launch>

Perfect! Time to launch the package.

roslaunch example_plugins_gazebo simple_world.launch

Now you should see something like this: Hello World! printed on the terminal and an empty Gazebo world. We are done here!

Step 4: Check your learning

Do you understand how to create Gazebo plugins? If you don’t know it yet, please go over the post again, more carefully this time.

Here are the links to the resources used in this tutorial:

• http://gazebosim.org/tutorials/?tut=plugins_hello_world
• http://gazebosim.org/tutorials?tut=ros_gzplugins
• http://gazebosim.org/tutorials?tut=guided_i5
• http://gazebosim.org/tutorials/?tut=ros_advanced
• http://gazebosim.org/tutorials?tut=plugins_model

(Extra) Step 5: Watch the video to understand how to create Gazebo plugins

Here you go:

Feedback

Did you like this post? Do you have any questions about how to create Gazebo plugins? Please leave a comment in the comments section below, so we can interact and learn from each other.

If you want to learn about other ROS topics, please let us know in the comments area and we will do a video or post about it.