| Note: This tutorial assumes you have completed the writing a tf2 broadcaster tutorial (Python) (C++). |
Writing a tf2 listener (Python)
Description: This tutorial teaches you how to use tf2 to get access to frame transformations.Tutorial Level: BEGINNER
Next Tutorial: Adding a frame (Python) (C++)
Contents
In the previous tutorias we created a tf2 broadcaster to publish the pose of a turtle to tf2. In this tutorial we'll create a tf2 listener to start using tf2.
How to create a tf2 listener
Let's first create the source files. Go to the package we created in the previous tutorial:
$ roscd learning_tf2
The Code
Fire up your favorite editor and paste the following code into a new file called nodes/turtle_tf2_listener.py.
1 #!/usr/bin/env python
2 import roslib
3 roslib.load_manifest('learning_tf2')
4 import rospy
5 import math
6 import tf2
7 import turtlesim.msg
8 import turtlesim.srv
9
10 if __name__ == '__main__':
11 rospy.init_node('tf2_turtle')
12
13 listener = tf2.TransformListener()
14
15 rospy.wait_for_service('spawn')
16 spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
17 spawner(4, 2, 0, 'turtle2')
18
19 turtle_vel = rospy.Publisher('turtle2/command_velocity', turtlesim.msg.Velocity)
20
21 rate = rospy.Rate(10.0)
22 while not rospy.is_shutdown():
23 try:
24 (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
25 except (tf2.LookupException, tf2.ConnectivityException):
26 continue
27
28 angular = 4 * math.atan2(trans[1], trans[0])
29 linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
30 turtle_vel.publish(turtlesim.msg.Velocity(linear, angular))
31
32 rate.sleep()
Don't forget to make the node executable:
chmod +x nodes/turtle_tf2_listener.py
The Code Explained
Now, let's take a look at the code that is relevant to publishing the turtle pose to tf2.
6 import tf2
The tf2 package provides an implementation of a tf2.TransformListener to help make the task of receiving transforms easier.
13 listener = tf2.TransformListener()
Here, we create a tf2.TransformListener object. Once the listener is created, it starts receiving tf2 transformations over the wire, and buffers them for up to 10 seconds.
23 try:
24 (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
25 except (tf2.LookupException, tf2.ConnectivityException):
26 continue
27
28 angular = 4 * math.atan2(trans[1], trans[0])
29 linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
30 turtle_vel.publish(turtlesim.msg.Velocity(linear, angular))
Here, the real work is done, we query the listener for a specific transformation. Let's take a look at the arguments:
- We want the transform from this frame ...
- ... to this frame.
The time at which we want to transform. Providing rospy.Time(0) will just get us the latest available transform.
All this is wrapped in a try-except block to catch possible exceptions.
Running the listener
With your text editor, open the launch file called start_demo.launch, and add the following lines:
<launch>
...
<node pkg="learning_tf2" type="turtle_tf2_listener.py"
name="listener" />
</launch>First, make sure you stopped the launch file from the previous tutorial (use ctrl-c). Now you're ready to start your full turtle demo:
$ roslaunch learning_tf2 start_demo.launch
You should see the turtle sim with two turtles.
Checking the results
To see if things work, simply drive around the first turtle using the arrow keys (make sure your terminal window is active, not your simulator window), and you'll see the second turtle following the first one!
When the turtlesim starts up you may see:
[ERROR] 1253915565.300572000: Frame id /turtle2 does not exist! When trying to transform between /turtle1 and /turtle2. [ERROR] 1253915565.401172000: Frame id /turtle2 does not exist! When trying to transform between /turtle1 and /turtle2.
This happens because our listener is trying to compute the transform before turtle 2 is spawned in turtlesim and broadcasting a tf2 frame.
Now you're ready to move on to the next tutorial, where you'll learn how to add a frame (Python) (C++)