Note: This tutorial assumes you have a working knowledge of compiling programs in ROS and you have completed the Introduction to tf2 tutorial.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Writing a tf2 static broadcaster (C++)

Description: This tutorial teaches you how to broadcast static coordinate frames to tf2

Tutorial Level: BEGINNER

Next Tutorial: Writing a tf2 broadcaster (C++)

In this tutorial we will write code to publish static transforms to tf2. This is a standalone tutorial covering the basics of static transforms.

In the next two tutorials we will write the code to reproduce the demo from the tf2 introduction tutorial. After that, the following tutorials focus on extending the demo with more advanced tf2 features.

Create a learning_tf2 package

First we will create a catkin package that will be used for this tutorial and the following ones. This package called learning_tf2 will depend on tf2, tf2_ros, roscpp, rospy and turtlesim. Code for this tutorial is stored here.

 $ catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim

How to broadcast transforms

This tutorial teaches you how to broadcast coordinate frames to tf2. In this case, we want to broadcast the changing coordinate frames of the turtles, as they move around.

Let's first create the source files. Go to the package we just created:

 $ roscd learning_tf2

The Code

Fire up your favourite editor to paste the following code into a new file called src/static_turtle_tf2_broadcaster.cpp.

   1 #include <ros/ros.h>
   2 #include <tf2_ros/static_transform_broadcaster.h>
   3 #include <geometry_msgs/TransformStamped.h>
   4 #include <cstdio>
   5 #include <tf2/LinearMath/Quaternion.h>
   6 
   7 
   8 std::string static_turtle_name;
   9 
  10 int main(int argc, char **argv)
  11 {
  12   ros::init(argc,argv, "my_static_tf2_broadcaster");
  13   if(argc != 8)
  14   {
  15     ROS_ERROR("Invalid number of parameters\nusage: static_turtle_tf2_broadcaster child_frame_name x y z roll pitch yaw");
  16     return -1;
  17   }
  18   if(strcmp(argv[1],"world")==0)
  19   {
  20     ROS_ERROR("Your static turtle name cannot be 'world'");
  21     return -1;
  22 
  23   }
  24   static_turtle_name = argv[1];
  25   static tf2_ros::StaticTransformBroadcaster static_broadcaster;
  26   geometry_msgs::TransformStamped static_transformStamped;
  27 
  28   static_transformStamped.header.stamp = ros::Time::now();
  29   static_transformStamped.header.frame_id = "world";
  30   static_transformStamped.child_frame_id = static_turtle_name;
  31   static_transformStamped.transform.translation.x = atof(argv[2]);
  32   static_transformStamped.transform.translation.y = atof(argv[3]);
  33   static_transformStamped.transform.translation.z = atof(argv[4]);
  34   tf2::Quaternion quat;
  35   quat.setRPY(atof(argv[5]), atof(argv[6]), atof(argv[7]));
  36   static_transformStamped.transform.rotation.x = quat.x();
  37   static_transformStamped.transform.rotation.y = quat.y();
  38   static_transformStamped.transform.rotation.z = quat.z();
  39   static_transformStamped.transform.rotation.w = quat.w();
  40   static_broadcaster.sendTransform(static_transformStamped);
  41   ROS_INFO("Spinning until killed publishing %s to world", static_turtle_name.c_str());
  42   ros::spin();
  43   return 0;
  44 };

The code explained

Now let's look at the code that is relevant to publishing the static_turtle_pose to tf2

   3 #include <geometry_msgs/TransformStamped.h>
   4 

  24   static_turtle_name = argv[1];

Here we create a StaticTransformBroadcaster object that we'll use later to send transformations over the wire.

  25   static tf2_ros::StaticTransformBroadcaster static_broadcaster;
  26   geometry_msgs::TransformStamped static_transformStamped;
  27 
  28   static_transformStamped.header.stamp = ros::Time::now();
  29   static_transformStamped.header.frame_id = "world";

Here we create a TransformStamped object which will be the message we will send over once populated. Before stuffing the actual transform values we need to give it the appropriate metadata.

  39   static_transformStamped.transform.rotation.w = quat.w();

Finally we send the transform using the StaticTransformBroadcaster sendTransform function.

Running the Static Broadcaster

Now that we created the code, let's compile it first. Open the CMakeLists.txt file and add the following:

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

and try to build your package

 $ catkin_make

If everything went well you should have a binary file called static_turtle_tf2_broadcaster in your bin folder.

First start a roscore in a separate terminal

$ . /opt/ros/$ROS-DISTRO/setup.bash
$ roscore

You can now run your static_turtle_tf2_broadcaster by running

 $ rosrun learning_tf2 static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

Checking the results

We can now check that the static_transform has been published by echoing the tf_static topic

 $ rostopic echo /tf_static

If everything went well you should see a single static transform

transforms:
  -
    header:
      seq: 0
      stamp:
        secs: 1459282870
        nsecs: 126883440
      frame_id: world
    child_frame_id: mystaticturtle
    transform:
      translation:
        x: 0.0
        y: 0.0
        z: 1.0
      rotation:
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
---

The proper way to publish static transforms

This tutorial aimed to show how StaticTransformBroadcaster can be used to publish static transforms. In your real development process you shouldn't have to write this code yourself and should privilege the use of the dedicated tf2_ros tool to do so. tf2_ros provides an executable named static_transform_publisher that can be used either as a commandline tool or a node that you can add to your launchfiles.

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id

  • Publish a static coordinate transform to tf2 using an x/y/z offset in meters and yaw/pitch/roll in radians. (yaw is rotation about Z, pitch is rotation about Y, and roll is rotation about X).

static_transform_publisher x y z qx qy qz qw frame_id child_frame_id

  • Publish a static coordinate transform to tf2 using an x/y/z offset in meters and quaternion.

Unlike in tf, there is no period argument, and a latched topic is used.

static_transform_publisher is designed both as a command-line tool for manual use, as well as for use within roslaunch files for setting static transforms. For example:

   1 <launch>
   2 <node pkg="tf2_ros" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 link1_parent link1" />
   3 </launch>

Wiki: tf2/Tutorials/Writing a tf2 static broadcaster (C++) (last edited 2020-04-28 00:22:20 by TullyFoote)