How to create a ROS Sensor Plugin for Gazebo
There are magnificent tutorials about how to create plugins for Gazebo in the GazeboSim webpage. There are even some tutorials about how to create plugins for Gazebo + ROS. Those tutorials show that there are several types of plugins (world, model, sensor, system, visual), and indicate how to create a plugin for a world type plugin.
Recently I need to create a plugin for a light detector. Reading the tutorials, I missed a concrete example about how to create a sensor plugin. Hence, I had to investigate a little bit about it. The result is the content of this post.
How to: light sensor plugin in Gazebo
Following the indications provided at the answers forum of Gazebo, I decided to build a very simple light detector sensor based on a camera. Instead of using a raytracing algorithm from lights, the idea is to use a camera to capture an image, then use the image to calculate the illuminance of the image, and then publish that illuminance value through a ROS topic.
Since the plugin is for its use with ROS, the whole plugin should be compilable using a ROS environment. Hence, be sure that you have installed the following packages in your Linux system:
- ros-<your_ros_version>-<your_gazebo_version>-ros. (in my case it is ros-jade-gazebo6-ros)
- ros-<your_ros_version>-<your_gazebo_version>-plugins (in my case it is ros-jade-gazebo6-plugins)
This tutorial, has two parts: on the first one we will explain how to create the plugin, and on the second, how to test that it works
Creating the plugin
Creating a ROS package for the plugin
First thing, is to create the package in our catkin workspace that will allow us to compile the plugin without a problem.
cd ~/catkin_ws/src catkin_create_pkg gazebo_light_sensor_plugin gazebo_ros gazebo_plugins roscpp
Creating the plugin code
For this purpose, since we are using a camera to capture the light, we are going to create a plugin class that inherits from the CameraPlugin. The code that follows has been created taking as guideline the code of the authentic gazebo ROS camera plugin.
Create a file called light_sensor_plugin.h inside the include directory of your package, including the following code:
#ifndef GAZEBO_ROS_LIGHT_SENSOR_HH
#define GAZEBO_ROS_LIGHT_SENSOR_HH
#include <string>
// library for processing camera data for gazebo / ros conversions
#include <gazebo/plugins/CameraPlugin.hh>
#include <gazebo_plugins/gazebo_ros_camera_utils.h>
namespace gazebo
{
class GazeboRosLight : public CameraPlugin, GazeboRosCameraUtils
{
/// \brief Constructor
/// \param parent The parent entity, must be a Model or a Sensor
public: GazeboRosLight();
/// \brief Destructor
public: ~GazeboRosLight();
/// \brief Load the plugin
/// \param take in SDF root element
public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
/// \brief Update the controller
protected: virtual void OnNewFrame(const unsigned char *_image,
unsigned int _width, unsigned int _height,
unsigned int _depth, const std::string &_format);
ros::NodeHandle _nh;
ros::Publisher _sensorPublisher;
double _fov;
double _range;
};
}
#endif
As you can see, the code includes a node handler to connect to the roscore. It also defines a publisher that will publish messages containing the illuminance value. Two parameters have been defined: fov (field of view) and range. At present only fov is used to indicate the amount of pixels around the center of the image that will be taken into account to calculate the illuminance.
Next step is to create a file named light_sensor_plugin.cpp containing the following code in the src directory of your package:
#include <gazebo/common/Plugin.hh>
#include <ros/ros.h>
#include "gazebo_light_sensor_plugin/light_sensor_plugin.h"
#include "gazebo_plugins/gazebo_ros_camera.h"
#include <string>
#include <gazebo/sensors/Sensor.hh>
#include <gazebo/sensors/CameraSensor.hh>
#include <gazebo/sensors/SensorTypes.hh>
#include <sensor_msgs/Illuminance.h>
namespace gazebo
{
// Register this plugin with the simulator
GZ_REGISTER_SENSOR_PLUGIN(GazeboRosLight)
////////////////////////////////////////////////////////////////////////////////
// Constructor
GazeboRosLight::GazeboRosLight():
_nh("light_sensor_plugin"),
_fov(6),
_range(10)
{
_sensorPublisher = _nh.advertise<sensor_msgs::Illuminance>("lightSensor", 1);
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
GazeboRosLight::~GazeboRosLight()
{
ROS_DEBUG_STREAM_NAMED("camera","Unloaded");
}
void GazeboRosLight::Load(sensors::SensorPtr _parent, 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;
}
CameraPlugin::Load(_parent, _sdf);
// copying from CameraPlugin into GazeboRosCameraUtils
this->parentSensor_ = this->parentSensor;
this->width_ = this->width;
this->height_ = this->height;
this->depth_ = this->depth;
this->format_ = this->format;
this->camera_ = this->camera;
GazeboRosCameraUtils::Load(_parent, _sdf);
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosLight::OnNewFrame(const unsigned char *_image,
unsigned int _width, unsigned int _height, unsigned int _depth,
const std::string &_format)
{
static int seq=0;
this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
if (!this->parentSensor->IsActive())
{
if ((*this->image_connect_count_) > 0)
// do this first so there's chance for sensor to run once after activated
this->parentSensor->SetActive(true);
}
else
{
if ((*this->image_connect_count_) > 0)
{
common::Time cur_time = this->world_->GetSimTime();
if (cur_time - this->last_update_time_ >= this->update_period_)
{
this->PutCameraData(_image);
this->PublishCameraInfo();
this->last_update_time_ = cur_time;
sensor_msgs::Illuminance msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = "";
msg.header.seq = seq;
int startingPix = _width * ( (int)(_height/2) - (int)( _fov/2)) - (int)(_fov/2);
double illum = 0;
for (int i=0; i<_fov ; ++i)
{
int index = startingPix + i*_width;
for (int j=0; j<_fov ; ++j)
illum += _image[index+j];
}
msg.illuminance = illum/(_fov*_fov);
msg.variance = 0.0;
_sensorPublisher.publish(msg);
seq++;
}
}
}
}
}
That is the code that calculates the illuminance in a very simple way. Basically, it just adds the values of all the pixels in the fov of the camera and then divides by the total number of pixels.
Create a proper CMakeLists.txt
Substitute the code of the automatically created CMakeLists.txt by the code below:
cmake_minimum_required(VERSION 2.8.3)
project(gazebo_light_sensor_plugin)
find_package(catkin REQUIRED COMPONENTS
gazebo_plugins
gazebo_ros
roscpp
)
find_package (gazebo REQUIRED)
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS gazebo_plugins gazebo_ros roscpp
)
###########
## Build ##
###########
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
link_directories(${GAZEBO_LIBRARY_DIRS})
include_directories(include)
include_directories( ${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIR}
${GAZEBO_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME} src/light_sensor_plugin.cpp)
## Specify libraries to link a library or executable target against
target_link_libraries( ${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES} CameraPlugin )
Update the package.xml and compile
Now you need to include the following line in your package.xml, between the tags <export></export>
<gazebo_ros plugin_path="${prefix}/lib" gazebo_media_path="${prefix}" />
Now you are ready to compile the plugin. Compilation should generate the library containing the plugin inside your building directory.
> roscd > cd .. > catkin_make
Testing the Plugin
Let’s create a world file containing the plugin and launch it to see how it works
Create a world file
You need a world file that includes the plugin. Here it is an example. Create a worlds directory inside your plugin package, and save the following code in a file entitled light.world. This world file just loads the camera with its plugin so it may be a bit ugly but enough for your tests. Feel free to add more elements and models in the world file (like for example, in the picture at the top of this post).
<?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 -->
<model name='camera'>
<pose>0 -1 0.05 0 -0 0</pose>
<link name='link'>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name='camera' type='camera'>
<camera name='__default__'>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
<plugin name="gazebo_light_sensor_plugin" filename="libgazebo_light_sensor_plugin.so">
<cameraName>camera</cameraName>
<alwaysOn>true</alwaysOn>
<updateRate>10</updateRate>
<imageTopicName>rgb/image_raw</imageTopicName>
<depthImageTopicName>depth/image_raw</depthImageTopicName>
<pointCloudTopicName>depth/points</pointCloudTopicName>
<cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
<depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
<frameName>camera_depth_optical_frame</frameName>
<baseline>0.1</baseline>
<distortion_k1>0.0</distortion_k1>
<distortion_k2>0.0</distortion_k2>
<distortion_k3>0.0</distortion_k3>
<distortion_t1>0.0</distortion_t1>
<distortion_t2>0.0</distortion_t2>
<pointCloudCutoff>0.4</pointCloudCutoff>
<robotNamespace>/</robotNamespace>
</plugin>
</sensor>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<gravity>1</gravity>
</link>
</model>
</world>
</sdf>
Create a launch file
Now the final step, to create a launch that will upload everything for you. Save the following code as main.launch inside the launch directory of you package.
<launch>
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="verbose" value="true"/>
<arg name="world_name" value="$(find gazebo_light_sensor_plugin)/worlds/light.world"/>
<!-- more default parameters can be changed here -->
</include>
</launch>
Ready to run!
Now launch the world. Be sure that a roscore is running or your machine, and that the GAZEBO_PLUGIN_PATH environment var includes the path to the new plugin.
Now execute the following command:
roslaunch gazebo_light_sensor_plugin main.launchYou can see what the camera is observing by running the following command:
rosrun image_view image_view image:=/camera/rgb/image_raw
After running that command, a small window will appear in your screen showing what the camera is capturing. Of course, since your world is completely empty, you will only see something like as ugly as this:
Try to add some stuff in front of the camera and see how it is actually working.
Now it is time to check the value of illuminance by watching the published topic (/light_sensor_plugin/lightSensor). Just type the following and you are done:
rostopic echo /light_sensor_plugin/lightSensor
You should see the topic messages been published in your screen, something like this:
Conclusion
Now you have a plugin for your Gazebo simulations that can measure (very roughly) the light detected. It can be improved in many ways, but it serves as a starting point for understanding the complex world of plugins within Gazebo.
You can use it in you desktop Gazebo or even inside the ROS Development Studio. It is also independent of the ROS version you are using (just install the proper packages).