|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
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 == '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 31 32 static_transformStamped.transform.translation.x = float(sys.argv) 33 static_transformStamped.transform.translation.y = float(sys.argv) 34 static_transformStamped.transform.translation.z = float(sys.argv) 35 36 quat = tf.transformations.quaternion_from_euler( 37 float(sys.argv),float(sys.argv),float(sys.argv)) 38 static_transformStamped.transform.rotation.x = quat 39 static_transformStamped.transform.rotation.y = quat 40 static_transformStamped.transform.rotation.z = quat 41 static_transformStamped.transform.rotation.w = quat 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.
Here we create a StaticTransformBroadcaster object that we'll use later to send transformations over the wire.
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.
We need to give the transform being published a timestamp, we'll just stamp it with the current time, rospy.Time.now().
Then, we need to set the name of the parent frame of the link we're creating, in this case "world"
- 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) 33 static_transformStamped.transform.translation.y = float(sys.argv) 34 static_transformStamped.transform.translation.z = float(sys.argv) 35 36 quat = tf.transformations.quaternion_from_euler( 37 float(sys.argv),float(sys.argv),float(sys.argv)) 38 static_transformStamped.transform.rotation.x = quat 39 static_transformStamped.transform.rotation.y = quat 40 static_transformStamped.transform.rotation.z = quat 41 static_transformStamped.transform.rotation.w = quat
Here we populate the 6D pose (translation and rotation) of the turtle.
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
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: