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 (Python)

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

Tutorial Level: BEGINNER

Next Tutorial: Writing a tf2 broadcaster (Python)

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

Let's start by creating a new directory called nodes where we will store our Python nodes.

 $ mkdir nodes

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

   1 #!/usr/bin/env python
   2 import rospy
   3 
   4 # to get commandline arguments
   5 import sys
   6 
   7 # because of transformations
   8 import tf
   9 
  10 import tf2_ros
  11 import geometry_msgs.msg
  12 
  13 if __name__ == '__main__':
  14     if len(sys.argv) < 8:
  15         rospy.logerr('Invalid number of parameters\nusage: '
  16                      './static_turtle_tf2_broadcaster.py '
  17                      'child_frame_name x y z roll pitch yaw')
  18         sys.exit(0)
  19     else:
  20         if sys.argv[1] == 'world':
  21             rospy.logerr('Your static turtle name cannot be "world"')
  22             sys.exit(0)
  23 
  24         rospy.init_node('my_static_tf2_broadcaster')
  25         broadcaster = tf2_ros.StaticTransformBroadcaster()
  26         static_transformStamped = geometry_msgs.msg.TransformStamped()
  27 
  28         static_transformStamped.header.stamp = rospy.Time.now()
  29         static_transformStamped.header.frame_id = "world"
  30         static_transformStamped.child_frame_id = sys.argv[1]
  31 
  32         static_transformStamped.transform.translation.x = float(sys.argv[2])
  33         static_transformStamped.transform.translation.y = float(sys.argv[3])
  34         static_transformStamped.transform.translation.z = float(sys.argv[4])
  35 
  36         quat = tf.transformations.quaternion_from_euler(
  37                    float(sys.argv[5]),float(sys.argv[6]),float(sys.argv[7]))
  38         static_transformStamped.transform.rotation.x = quat[0]
  39         static_transformStamped.transform.rotation.y = quat[1]
  40         static_transformStamped.transform.rotation.z = quat[2]
  41         static_transformStamped.transform.rotation.w = quat[3]
  42 
  43         broadcaster.sendTransform(static_transformStamped)
  44         rospy.spin()

The code explained

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

  10 import tf2_ros

The tf2_ros package provides a StaticTransformBroadcaster to make easy the publishing of static transforms. To use the StaticTransformBroadcaster, we need to import the tf2_ros module.

  24         rospy.init_node('my_static_tf2_broadcaster')

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

  25         broadcaster = tf2_ros.StaticTransformBroadcaster()
  26         static_transformStamped = geometry_msgs.msg.TransformStamped()
  27 
  28         static_transformStamped.header.stamp = rospy.Time.now()
  29         static_transformStamped.header.frame_id = "world"
  30         static_transformStamped.child_frame_id = sys.argv[1]

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.

  1. We need to give the transform being published a timestamp, we'll just stamp it with the current time, rospy.Time.now().

  2. Then, we need to set the name of the parent frame of the link we're creating, in this case "world"

  3. Finally, we need to set the name of the child frame of the link we're creating.

  32         static_transformStamped.transform.translation.x = float(sys.argv[2])
  33         static_transformStamped.transform.translation.y = float(sys.argv[3])
  34         static_transformStamped.transform.translation.z = float(sys.argv[4])
  35 
  36         quat = tf.transformations.quaternion_from_euler(
  37                    float(sys.argv[5]),float(sys.argv[6]),float(sys.argv[7]))
  38         static_transformStamped.transform.rotation.x = quat[0]
  39         static_transformStamped.transform.rotation.y = quat[1]
  40         static_transformStamped.transform.rotation.z = quat[2]
  41         static_transformStamped.transform.rotation.w = quat[3]

Here we populate the 6D pose (translation and rotation) of the turtle.

  43         broadcaster.sendTransform(static_transformStamped)

Finally we send the transform using the StaticTransformBroadcaster sendTransform function.

Running the Static Broadcaster

First we need to mark the file as executable.

$ chmod +x ~/tutorial_ws/src/learning_tf2/src/nodes/static_turtle_tf2_broadcaster.py

Now that we created the code, let's compile it first.

and try to build your package

 $ catkin_make

You can now run your static_turtle_tf2_broadcaster.py. First start a roscore in a separate terminal:

$ . devel/setup.bash
$ roscore

and then run:

 $ rosrun learning_tf2 static_turtle_tf2_broadcaster.py mystaticturtle 0 0 1 0 0 0

This sets a turtle pose broadcast for mystaticturtle to float 1m above the ground.

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 (Python) (last edited 2020-04-28 00:55:01 by TullyFoote)