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(&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(&ModelQuake::OnRosMsg, this, _1),
ros::VoidPtr(), &this->rosQueue);
this->rosSub = this->rosNode->subscribe(so);
// Spin up the queue helper thread.
this->rosQueueThread =
std::thread(std::bind(&ModelQuake::QueueThread, this));
// Magnitude
ros::SubscribeOptions so2 =
ros::SubscribeOptions::create<std_msgs::Float32>(
earthquake_magnitud_topicName,
1,
boost::bind(&ModelQuake::OnRosMsg_Magn, this, _1),
ros::VoidPtr(), &this->rosQueue2);
this->rosSub2 = this->rosNode->subscribe(so2);
// Spin up the queue helper thread.
this->rosQueueThread2 =
std::thread(std::bind(&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 && 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 &_freq)
{
this->x_axis_freq = _freq;
ROS_WARN("x_axis_freq >> %f", this->x_axis_freq);
}
public: void SetMagnitude(const double &_magn)
{
this->x_axis_magn = _magn;
ROS_WARN("x_axis_magn >> %f", this->x_axis_magn);
}
public: void OnRosMsg(const std_msgs::Float32ConstPtr &_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 &_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.
