How to create Gazebo plugins in ROS, Part 2

[Gazebo Q&A] 002 - How to create Gazebo plugins in ROS #Part 2

Written by Bayode Aderinola

22/03/2023

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!

Gazebo world with EarthQuake plugin

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.

Masterclass 2023 batch2 blog banner

Check Out These Related Posts

0 Comments

Submit a Comment

Your email address will not be published.

This site uses Akismet to reduce spam. Learn how your comment data is processed.

Pin It on Pinterest

Share This