What is the tf2 Library?

In this section, we will work with the ROS tf2 (Transform) library. The tf2 library was created to provide a way to keep track of the different coordinate frames in a robotic system. 

A typical robotic system has multiple 3D coordinate frames. Consider a robotic arm for example. You have the world coordinate frame, and then each joint of the arm has its own coordinate frame.

In this 2D robotic car example, you can see we have two coordinate frames. There is the global (i.e. world) coordinate frame. There is also the local coordinate frame (i.e. the car’s perspective).

Why are coordinate frames important in robotics? Well imagine you have a robot on an assembly line picking up objects as they pass by. A camera that is overhead identifies an object that needs to be picked up. How does the robot know where to move its gripper so that it can pick up the object?

The system needs to translate the object’s position (i.e. coordinates) in the camera coordinate frame to a position in the gripper coordinate frame. 

You can see now how the tf library in ROS can really come in handy when you have lots of sensors and actuators that need to work together to perform a task. 

And that’s not all. The coordinate frames change with time as the robot moves in the world. The tf library can help with this too.

Let’s complete the official ROS tutorials on the tf2 library.

First, let’s check out the demo to see the tf2 library in action.

Start by downloading the required ROS packages from the ROS repository. Open a new terminal window, and type these commands:

cd ~/catkin_ws/src

sudo apt-get update

sudo apt-get install ros-noetic-turtle-tf2 ros-noetic-tf

sudo apt-get install ros-noetic-tf2-tools

To run the demo, open a new terminal, and type this command:

roslaunch turtle_tf2 turtle_tf2_demo.launch

You should see a turtle on your screen.

Click the terminal window where you executed the roslaunch command, and then use the arrow keys to move the turtle around the screen.

You should see a turtle that is trailing the turtle that you are driving around the screen.

This demo uses the tf2 library to generate three coordinate frames:

1. World frame
2. Turtle1 frame
3. Turtle2 frame

A tf2 broadcaster is publishing the coordinate frames of the lead turtle. A tf2 listener calculates the difference between the coordinate frames of the lead turtle and the follower turtle. It then moves the follower turtle so that it follows the lead turtle.

Let’s take a look at what is going on behind the scenes.

In a new terminal window, type the following commands:

rosrun tf2_tools view_frames.py

evince frames.pdf

Evince is a document viewer for pdf files.

You can see below the tree of how the coordinate frames are connected:

The world coordinate frame is the parent of the turtle1 and turtle1 coordinate frames.

To see real-time information of the tree of coordinate frames, you can type this command:

rosrun rqt_tf_tree rqt_tf_tree

Now, open a new terminal window, and type this command:

rosrun tf tf_echo [reference_frame] [target_frame]

To see the transformation between any two frames, you can use this syntax:

rosrun tf tf_echo [reference_frame] [target_frame]

Type this command:

rosrun tf tf_echo turtle1 turtle2

Now, let’s look at the relationship between the coordinate frames using rviz. rviz is a 3D visualizer for ROS

rosrun rviz rviz -d `rospack find turtle_tf2`/rviz/turtle_rviz.rviz

On the left panel, click the arrow next to TF, and check all the boxes.

Click Select and highlight all the coordinate frames in the image. You should see boxes that define the different frames.

As you drive the turtle around, you will see those coordinate frames move.

Press CTRL+C on all terminal windows to shut everything down.

Now that we’ve looked at the demo, let’s take a few steps back and see how we can write the tf2 broadcaster (that publishes/broadcasts the coordinate frames of a robot) and tf2 listener (that receives and uses the coordinate frame information) that made this demo possible.

First, we’ll go through the process for C++. Then we’ll do the process for Python.

How to Write a tf2 Static Broadcaster in C++

The tf2 static broadcaster is used when you have a robotic system that is static (i.e. doesn’t change position and orientation that frequently). We won’t try to reproduce the turtle simulation just yet. We’ll begin to do that in the next section.

The first thing you want to do is open up a new terminal window and create a new catkin package.

cd ~/catkin_ws/src

catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim

Move into the src folder of the package we just made.

roscd learning_tf2/src

Open up a new C++ file named static_turtle_tf2_broadcaster.cpp.

gedit static_turtle_tf2_broadcaster.cpp

Type the following code inside it.

#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <cstdio>
#include <tf2/LinearMath/Quaternion.h>


std::string static_turtle_name;

int main(int argc, char **argv)
{
  ros::init(argc,argv, "my_static_tf2_broadcaster");
  if(argc != 8)
  {
    ROS_ERROR("Invalid number of parameters\nusage: static_turtle_tf2_broadcaster child_frame_name x y z roll pitch yaw");
    return -1;
  }
  if(strcmp(argv[1],"world")==0)
  {
    ROS_ERROR("Your static turtle name cannot be 'world'");
    return -1;

  }
  static_turtle_name = argv[1];
  
  // Create a StaticTransformBroadcaster object that we 
  // will use to send the transformations.
  static tf2_ros::StaticTransformBroadcaster static_broadcaster;
  
  // Create a TransformStamped object which will be the message 
  // that will be sent over once it is filled in.
  geometry_msgs::TransformStamped static_transformStamped;

  // Initialize the TransformedStamped object with some data.
  static_transformStamped.header.stamp = ros::Time::now();
  static_transformStamped.header.frame_id = "world";
  static_transformStamped.child_frame_id = static_turtle_name;
  static_transformStamped.transform.translation.x = atof(argv[2]);
  static_transformStamped.transform.translation.y = atof(argv[3]);
  static_transformStamped.transform.translation.z = atof(argv[4]);
  tf2::Quaternion quat;
  quat.setRPY(atof(argv[5]), atof(argv[6]), atof(argv[7]));
  
  // Send the transform using the StaticTransformBroadcaster 
  // sendTransform function.
  static_transformStamped.transform.rotation.x = quat.x();
  static_transformStamped.transform.rotation.y = quat.y();
  static_transformStamped.transform.rotation.z = quat.z();
  static_transformStamped.transform.rotation.w = quat.w();
  static_broadcaster.sendTransform(static_transformStamped);
  ROS_INFO("Spinning until killed publishing %s to world", static_turtle_name.c_str());
  ros::spin();
  return 0;
};

Press Save and close the editor.

Now go to the CMakeLists.txt file.

cd ..

Open CMakeLists.txt.

gedit CMakeLists.txt

Add these two lines to the bottom of the file.

add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp)
target_link_libraries(static_turtle_tf2_broadcaster  ${catkin_LIBRARIES} )

Click Save, and close the editor.

Now build the package.

cd ~/catkin_ws/

catkin_make

Here is what you should see:

Now let’s run the code. Open up the ROS Master. In a new terminal window, type:

roscore

In another terminal tab, launch the static_turtle_tf2 broadcaster.

rosrun learning_tf2 static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

Let’s see if the static_tranform has been published on the tf_static topic.

rostopic echo /tf_static

Here is the output:

That’s it for static transforms. Note that in practice, you won’t write the code we wrote above. You will instead use the tf2_ros package. The launch file at the bottom of this page is what you would use.

The instructions for writing and using a tf2 static broadcaster in Python are located at this page.

How to Write a tf2 Broadcaster in C++

In this section, we will see how to broadcast coordinate frames of a robot to tf2 using C++. The official tutorial is at this page, but I’ll walk you through the whole process below.

Go to the src folder of the learning_tf2 package.

roscd learning_tf2/src

Open up a new C++ file. 

gedit turtle_tf2_broadcaster.cpp

Type the following code:

#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg){

  // Create a TransformBroadcaster object that we 
  // will use to send the transformations.
  static tf2_ros::TransformBroadcaster br;
  
  // Create a TransformStamped object which will be the message 
  // that will be sent over once it is filled in.
  geometry_msgs::TransformStamped transformStamped;
 

  // Initialize the TransformedStamped object with some data.  
  transformStamped.header.stamp = ros::Time::now(); // Stamp with the current time
  transformStamped.header.frame_id = "world"; // Set name of parent frame of the link
  transformStamped.child_frame_id = turtle_name; // Set the name of the child node of the link
  
  // Copy information from 3D turtle post to 3D transform
  transformStamped.transform.translation.x = msg->x;
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 0.0;
  tf2::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

  // Send the transform
  br.sendTransform(transformStamped);
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_broadcaster");

  ros::NodeHandle private_node("~");
  if (! private_node.hasParam("turtle"))
  {
    if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
    turtle_name = argv[1];
  }
  else
  {
    private_node.getParam("turtle", turtle_name);
  }
    
  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

  ros::spin();
  return 0;
};

Click Save and close the editor.

Open up the CMakeLists.txt file.

roscd learning_tf2

gedit CMakeLists.txt

Add the following lines to the bottom of the file:

add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
target_link_libraries(turtle_tf2_broadcaster
 ${catkin_LIBRARIES}
)

Save and close the file.

Now, build the package.

cd ~/catkin_ws/

catkin_make

Now, go back to the package.

roscd learning_tf2 

Create a new folder called launch.

mkdir launch

Add the following launch file.

cd launch

gedit start_demo.launch

Type the following code inside it.

 <launch>
     <!-- Turtlesim Node-->
    <node pkg="turtlesim" 
	      type="turtlesim_node" 
		  name="sim"
    />

    <node pkg="turtlesim" 
	      type="turtle_teleop_key" 
		  name="teleop" 
		  output="screen"
	/>
	
    <!-- Axes -->
    <param name="scale_linear" 
		   value="2" 
		   type="double"
	/>	
    <param name="scale_angular" 
		   value="2" 
		   type="double"
	/>
    <node pkg="learning_tf2" 
		  type="turtle_tf2_broadcaster"
          args="/turtle1" 
		  name="turtle1_tf2_broadcaster" 
	/>	
    <node pkg="learning_tf2" 
		  type="turtle_tf2_broadcaster"
          args="/turtle2" 
		  name="turtle2_tf2_broadcaster" 
	/>
 </launch>

Click Save and close.

Now edit your CMakeLists.txt file again.

roscd learning_tf2

gedit CMakeLists.txt

Add this at the bottom of the file:

## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES
 start_demo.launch
 # myfile2
 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

Save and close the file.

Now type this command:

roslaunch learning_tf2 start_demo.launch

You should see a single turtle.

In another terminal window, type this command:

rosrun tf tf_echo /world /turtle1

Select the terminal window where you did the roslaunch command, and drive the turtle around. You should see the pose of turtle1.

Press CTRL+C on all terminal windows to close out everything.

How to Write a tf2 Listener in C++

In this section, we will see how to use tf2 to get access to frame transformations. The official tutorial is at this page, but I’ll walk you through the whole process below.

Go to the src folder of the learning_tf2 package.

roscd learning_tf2/src

Open up a new C++ file. 

gedit turtle_tf2_listener.cpp

Type the following code:

#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_listener");

  ros::NodeHandle node;

  ros::service::waitForService("spawn");
  ros::ServiceClient spawner =
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn turtle;
  turtle.request.x = 4;
  turtle.request.y = 2;
  turtle.request.theta = 0;
  turtle.request.name = "turtle2";
  spawner.call(turtle);

  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

  // Create TransformListener object to receive tf2 transformations.
  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);

  ros::Rate rate(10.0);
  
  
  // Query listener for a specific transformation
  while (node.ok()){
    geometry_msgs::TransformStamped transformStamped;
    try{
	  // Get the transform between two frames
	  // First argument is the target frame, the second argument is the source frame
	  // We want to transform to the target frame from the source frame.
	  // ros::Time(0) gets us the most recent transform.
      transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",
                               ros::Time(0));
    }
    catch (tf2::TransformException &ex) {
      ROS_WARN("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

    geometry_msgs::Twist vel_msg;

    vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y,
                                    transformStamped.transform.translation.x);
    vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) +
                                  pow(transformStamped.transform.translation.y, 2));
    turtle_vel.publish(vel_msg);

    rate.sleep();
  }
  return 0;
};

Click Save and close the editor.

Open up the CMakeLists.txt file.

roscd learning_tf2

gedit CMakeLists.txt

Add the following lines to the bottom of the file:

add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)
target_link_libraries(turtle_tf2_listener
 ${catkin_LIBRARIES}
)

Save and close the file.

Now, build the package.

cd ~/catkin_ws/

catkin_make

Now, go back to the package.

roscd learning_tf2 

Go inside the launch file.

cd launch

gedit start_demo.launch

Add the following just before the </launch> tag.

   <node pkg="learning_tf2" type="turtle_tf2_listener" name="listener" />

Click Save and close.

Now type this command:

roslaunch learning_tf2 start_demo.launch

You should see two turtles.

Select the terminal window where you did the roslaunch command, and drive the turtle around. You should see one of the turtles follow the other. 

Press CTRL+C on all terminal windows to close out everything.

How to Write a tf2 Broadcaster in Python

In this section, we will see how to broadcast coordinate frames of a robot to tf2 using Python. The official tutorial is at this page, but I’ll walk you through the whole process below.

Go to your package, and create a new directory called nodes.

roscd learning_tf2

mkdir nodes

cd nodes

Open a new file called turtle_tf2_broadcaster.py.

gedit turtle_tf2_broadcaster.py

Type the following code inside it.

#!/usr/bin/env python3
import rospy

# Because of transformations
import tf_conversions

import tf2_ros
import geometry_msgs.msg
import turtlesim.msg


def handle_turtle_pose(msg, turtlename):
    br = tf2_ros.TransformBroadcaster()
    t = geometry_msgs.msg.TransformStamped()

    # Give the transform being published a timestamp
    t.header.stamp = rospy.Time.now()
	
	# Set the name of the parent link
    t.header.frame_id = "world"
	
	# Set the name of the child node
    t.child_frame_id = turtlename
	
	# Broadcast the turtle's translation and rotation
	# Published as a transform from frame "world" to frame "turtleX"
	# Copy the information from the 3D turtle post into the
	# 3D transform.
    t.transform.translation.x = msg.x
    t.transform.translation.y = msg.y
    t.transform.translation.z = 0.0
    q = tf_conversions.transformations.quaternion_from_euler(0, 0, msg.theta)
    t.transform.rotation.x = q[0]
    t.transform.rotation.y = q[1]
    t.transform.rotation.z = q[2]
    t.transform.rotation.w = q[3]

    # Pass in the transform and send it
    br.sendTransform(t)

if __name__ == '__main__':
    rospy.init_node('tf2_turtle_broadcaster')
	
	# This node takes a single parameter "turtle", 
	# which specifies a turtle name, e.g. "turtle1" or "turtle2".
    turtlename = rospy.get_param('~turtle')
	
	# The node subscribes to topic "turtleX/pose" 
	# and runs function handle_turtle_pose on every incoming message.
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

Save, and close it.

Make the node executable.

chmod +x nodes/turtle_tf2_broadcaster.py

Now, move into your launch folder.

roscd learning_tf2/launch

Modify your launch file, so that it looks like this.

gedit start_demo.launch

<node name="turtle1_tf2_broadcaster" 
      pkg="learning_tf2" 
      type="turtle_tf2_broadcaster.py" 
      respawn="false" 
      output="screen" >
      <param name="turtle" type="string" value="turtle1" />
</node>
<node name="turtle2_tf2_broadcaster" 
      pkg="learning_tf2" 
      type="turtle_tf2_broadcaster.py" 
      respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle2" /> 
</node>

Save and close the editor.

Build it.

cd ~/catkin_ws/

catkin_make

Now type:

roslaunch learning_tf2 start_demo.launch

You should see one turtle.

To see the results type this command in another terminal tab:

rosrun tf tf_echo /world /turtle1

If you select the terminal where you typed roslaunch, you will can drive the turtle and watch the pose change.

How to Write a tf2 Listener in Python

In this section, we will see how to use tf2 to get access to frame transformations. The official tutorial is at this page, but I’ll walk you through the whole process below.

Go inside the nodes folder of your package and create a new Python program named turtle_tf2_listener.py.

roscd learning_tf2/nodes

gedit turtle_tf2_listener.py

Type this code in there, and click save.

#!/usr/bin/env python3
import rospy

import math
import tf2_ros
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('tf2_turtle_listener')

    tfBuffer = tf2_ros.Buffer()
	
	# Facilitates the receiving of transforms
    listener = tf2_ros.TransformListener(tfBuffer)

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    turtle_name = rospy.get_param('turtle', 'turtle2')
    spawner(4, 2, 0, turtle_name)

    turtle_vel = rospy.Publisher('%s/cmd_vel' % turtle_name, geometry_msgs.msg.Twist, queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
		    # The arguments are  target frame, source frame. We want to get the latest transform.
            trans = tfBuffer.lookup_transform(turtle_name, 'turtle1', rospy.Time())
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
            rate.sleep()
            continue

        msg = geometry_msgs.msg.Twist()

        msg.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)
        msg.linear.x = 0.5 * math.sqrt(trans.transform.translation.x ** 2 + trans.transform.translation.y ** 2)

        turtle_vel.publish(msg)

        rate.sleep()

Close the editor.

Make the node executable.

chmod +x nodes/turtle_tf2_listener.py

Now type:

roscd learning_tf2/launch

gedit start_demo.launch

Add this just before the </launch> tab of the launch file.

<node pkg="learning_tf2" type="turtle_tf2_listener.py" name="listener" output="screen"/>

Save and close the editor.

Build it.

cd ~/catkin_ws/

catkin_make

Now edit your CMakeLists.txt file.

roscd learning_tf2

gedit CMakeLists.txt

Make sure this code is in there somewhere near or at the bottom:

## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES
 start_demo.launch
 # myfile2
 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

Save and close.

Now go to a new terminal window, and type this to run the nodes:

roslaunch learning_tf2 start_demo.launch

You’re done! Keep building!

In this section, we’ll take a quick look at how to get started with Gazebo. Gazebo is a robot simulator that enables you to create realistic simulations of robots in complex indoor and outdoor environments.

If you already have ROS on your system, the stand-alone version of Gazebo is already installed. Let’s check that out now.

Open up a new terminal window, and type the following command. It normally takes a while to launch the first time, so just sit back and relax while Gazebo does its thing:

gazebo

Here is what your screen should look like. You should see an empty world.

Press CTRL+C in the terminal window to close it.

Gazebo is an independent framework, so what you saw above is the stand-alone version of this program. Since our main framework for robotics development will be ROS, we need to learn how to use Gazebo’s plugins (which we discussed earlier in this tutorial) so that you can access the Gazebo functionality when you’re working with ROS.

There are six types of plugins in Gazebo:

1. World
2. Model
3. Sensor
4. System
5. Visual
6. GUI

Each plugin type is responsible for providing your simulated robot with different functionality. For example, if you want to model sensors (e.g. IMU), you would use the Sensor plugin. The World plugin gives you control over the physics engine and the lighting of the environment the simulated robot is in.

Create a Gazebo Plugin

Let’s develop and load a minimalist “hello world” plugin that is based on the World plugin type mentioned above. This plugin will only consist of a class and a few functions. 

We’ll follow the official tutorial here at the Gazebo website.

Let’s install the Gazebo development files. You can see what version of gazebo you have on your system by typing the following command:

apt list --installed

I have Gazebo 11, so we need to install the Gazebo development files for Gazebo 11.

Open a new terminal window, and type the following command (note, it might already be installed):

sudo apt-get install libgazebo11-dev

Now, we need to make a new directory.

mkdir ~/gazebo_plugin_tutorial

Move into that folder.

cd ~/gazebo_plugin_tutorial

Open a new C++ file.

gedit hello_world.cc

Type the following code.

// Includes a core set of basic Gazebo functions
#include <gazebo/gazebo.hh>

// All plugins must be in the gazebo namespace
namespace gazebo
{
  // Each plugin has to inherit from a plugin type.
  // In this case, we are inheriting from the WorldPlugin type.
  class WorldPluginTutorial : public WorldPlugin
  {
    public: WorldPluginTutorial() : WorldPlugin()
            {
              printf("Hello World!\n");
            }

    // This function is mandatory. It receives an element in 
	// Simulation Description Format (SDF) that contains the elements
	// and attributes that are specified in the loaded SDF file.
    public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
            {
            }
  };
  // Register the plugin with the simulator.
  // Note that there are matching register macros for the six types
  // of plugins in Gazebo.
  GZ_REGISTER_WORLD_PLUGIN(WorldPluginTutorial)
}

Save it, and close the editor.

Compile the Plugin

Type the following command:

gedit ~/gazebo_plugin_tutorial/CMakeLists.txt

Add the following to this file.

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")

add_library(hello_world SHARED hello_world.cc)
target_link_libraries(hello_world ${GAZEBO_LIBRARIES})

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")

Click Save, and close the editor.

Create a new directory called build.

mkdir ~/gazebo_plugin_tutorial/build

cd ~/gazebo_plugin_tutorial/build

Compile the code:

cmake ../

make

You should now see this message:

Now, add your library path to the GAZEBO_PLUGIN_PATH:

export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/gazebo_plugin_tutorial/build

So that we don’t have to execute the command above each time we open a new terminal window, let’s add this line to the bottom of our bashrc file.

gedit ~/.bashrc

Click Save, and close the editor.

Use the Plugin

Now that we have our compiled plugin (~/gazebo_plugin_tutorial/build/libhello_world.so), we can attach it to a file in Simulation Description Format. The way it works on startup is as follows:

1. Gazebo parses the SDF file
2. Gazebo locates the plugin you made
3. Gazebo loads the code in the plugin

Let’s create the SDF file.

Type the following command to open up a new file:

gedit ~/gazebo_plugin_tutorial/hello.world

Add the following code:

<?xml version="1.0"?>
<sdf version="1.4">
  <world name="default">
    <plugin name="hello_world" filename="libhello_world.so"/>
  </world>
</sdf>

Click Save, and close the editor.

Lastly, open the file using this command:

gzserver ~/gazebo_plugin_tutorial/hello.world --verbose

Here is the output:

In this section, we’ll learn how to work with ROS nodelets.

What is a ROS nodelet?

A node in ROS is a program that runs a specific process. These nodes communicate back and forth with each other on the ROS network via special ROS constructs, known as topics. The disadvantage of this framework is that message passing can get pretty slow if there is a large volume of data that needs to be passed (e.g. data from 3D sensor). 

Fortunately, ROS has something called a nodelet. A nodelet is a special type of ROS node that enables you to run multiple nodes in a single process on a single computer. These nodes are actually separate threads that can communicate with each other directly without going through the traditional ROS communication network middleman. 

The cool thing about nodelets is that they can load as plugins (see the last section of this tutorial), which means that you can add them to your application or remove them from your application without having to make major modifications to your main source code.

The disadvantage of a nodelet is that if a nodelet goes down, all the running processes within it go down as well. Also, the nodelet concept is only implemented in C++, so you can’t use Python.

Bottom Line: If you need a quick and efficient way to pass large amounts of data from one node to another, consider using a nodelet. Otherwise, always use nodes because they are more flexible.

With that background, let’s take a look now at how to create a ROS nodelet. I will loosely follow the official tutorial on the ROS website.

How to Create a ROS Nodelet

The nodelet that we will create will subscribe to a topic (/ros_in). It will then receive a message via that topic and then republish that message to another topic (/ros_out).

Open a new terminal window.

Type the following command:

cd ~/catkin_ws/src

Create a new package called nodelet_basics

catkin_create_pkg nodelet_basics nodelet roscpp std_msgs

The nodelet package we included in the command above provides the tools to build a nodelet.

Move to the src folder of the package you just created.

cd nodelet_basics/src

Create a new file. This file will have the code for the nodelet.

gedit hello_world.cpp

Type the following lines of code:

/*
Author: Addison Sears-Collins

This ROS nodelet will subscribe to a topic (/ros_in). 
It will then receive a message via that topic and then 
republish that message to another topic (/ros_out). 

Date: 6/19/2020

ROS Version: ROS Noetic Ninjemys
*/

// Add the necessary includes
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <stdio.h>


namespace nodelet_basics
{
	/*********
	* Nodelet
	**********/

	// This code here means the class needs to inherit from 
	// nodelet::Nodelet
	class Hello_World : public nodelet::Nodelet
	{
	
		public:
			Hello_World()
			{
			}

		private:
			// The onInit method is called by nodelet manager. 
			// It is responsible for initializing the nodelet.
			virtual void onInit()
			{
				// Create a NodeHandle object
				ros::NodeHandle& private_nh = getPrivateNodeHandle();
				NODELET_DEBUG("Initializing nodelet...");
			
				// Create a publisher topic
				pub = private_nh.advertise<std_msgs::String>("ros_out",10); 
			
				// Create a subscriber topic
				sub = private_nh.subscribe("ros_in",10, &Hello_World::callback, this);	
			}

			ros::Publisher pub;
			ros::Subscriber sub;
	
			// Display messages from /ros_in topic to the terminal window.
			// Publish to /ros_out topic
			void callback(const std_msgs::String::ConstPtr& input)
			{

				std_msgs::String output;
				output.data = input->data;
				NODELET_DEBUG("msg data = %s",output.data.c_str());
				ROS_INFO("msg data = %s",output.data.c_str());
				pub.publish(output);		
			}
	};
	// Export the Hello_World class as a plugin using the
	// PLUGINLIB_EXPORT_CLASS macro.
	PLUGINLIB_EXPORT_CLASS(nodelet_basics::Hello_World, nodelet::Nodelet);
}

Save and close the editor.

Now, let’s create an xml file that describes the plugin.

roscd nodelet_basics

gedit nodelet_plugins.xml

Add these lines. 

<library path="lib/libnodelet_basics">
  <class name="nodelet_basics/Hello_World" 
         type="nodelet_basics::Hello_World" 
		 base_class_type="nodelet::Nodelet">
	<description>
		This nodelet receives a message and publishes it.
	</description>
  </class>
</library>

This nodelet receives a message and publishes it.

Save and close the editor.

Open the package.xml file.

gedit package.xml

Make sure you add this line between the <export></export> tags.

<nodelet plugin="${prefix}/nodelet_plugins.xml" />

Save and close the file.

Now let’s edit CMakeLists.txt.

gedit CMakeLists.txt

Add these lines to the bottom of the CMakeLists.txt file.

## Declare C++ library
add_library(nodelet_basics src/hello_world.cpp)

## Link libraries
target_link_libraries(nodelet_basics ${catkin_LIBRARIES}) 

Click Save and close the editor.

Build the package.

cd ~/catkin_ws/

catkin_make

If the build is successful, you’ll see a shared object named libnodelet_basics.co. This file is the plugin.

Now let’s create a launch file.

roscd nodelet_basics

mkdir launch

cd launch

gedit hello_world_launch.launch

<launch>
  <node pkg="nodelet" 
        type="nodelet" 
        name="standalone_nodelet"  
        args="manager" 
        output="screen"
  />

  <node pkg="nodelet" 
        type="nodelet" 
        name="Hello_World" 
        args="load nodelet_basics/Hello_World standalone_nodelet" 
        output="screen"
  />
</launch>

Save and close the editor.

Now, launch the nodelet. Remember when you use the roslaunch command, you don’t have to launch the ROS master using roscore.

roslaunch nodelet_basics hello_world_launch.launch

Here is the output.

In another terminal, check out the list of active nodes.

rosnode list

In another terminal, check out the list of topics.

rostopic list

Let’s publish a string message to the /ros_in topic. In a new terminal, type the following

rostopic echo /Hello_World/ros_out

This code above is now ready for you to publish your string. It will republish the string it receives. Let’s type the string now.

rostopic pub /Hello_World/ros_in std_msgs/String "Hi Automatic Addison"

Here is what you should see on the /Hello_World/ros_in topic terminal window.

Here is what you should see on the /Hello_World/ros_out terminal window.

Let’s check out the node graph.

rqt_graph

That’s it! You now know how to create a nodelet.