#################################### ##FILL ME IN #################################### ## for a custom note with links: ## note =This tutorial assumes you have completed the writing a tf broadcaster tutorial [[tf/Tutorials/Writing a tf broadcaster (Python)|(Python)]] [[tf/Tutorials/Writing a tf broadcaster (C++)|(C++)]] ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0= ## descriptive title for the tutorial ## title =Writing a tf listener (Python) ## multi-line description to be displayed in search ## description = This tutorial teaches you how to use tf to get access to frame transformations. ## the next tutorial description (optional) ## next = Adding a frame ## links to next tutorial (optional) ## next.0.link=[[tf/Tutorials/Adding a frame (Python)|(Python)]] ## next.1.link= ## what level user is this tutorial for ## level= BeginnerCategory ## keywords = #################################### <> <> {{{#!wiki caution tf is deprecated in favor of [[tf2]]. [[tf2]] provides a superset of the functionality of tf and is actually now the implementation under the hood. If you're just learning now it's strongly recommended to use the [[tf2/Tutorials]] instead. }}} <> <> === The Code === Fire up your favorite editor and paste the following code into a new file called '''`nodes/turtle_tf_listener.py`'''. {{{{#!wiki version cturtle diamonback electric fuerte groovy {{{ #!python block=listener #!/usr/bin/env python import roslib roslib.load_manifest('learning_tf') import rospy import math import tf import turtlesim.msg import turtlesim.srv if __name__ == '__main__': rospy.init_node('turtle_tf_listener') listener = tf.TransformListener() rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) spawner(4, 2, 0, 'turtle2') turtle_vel = rospy.Publisher('turtle2/command_velocity', turtlesim.msg.Velocity, queue_size=1) # Groovy no queue_size # turtle_vel = rospy.Publisher('turtle2/command_velocity', turtlesim.msg.Velocity) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue angular = 4 * math.atan2(trans[1], trans[0]) linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2) turtle_vel.publish(turtlesim.msg.Velocity(linear, angular)) rate.sleep() }}} }}}} {{{{#!wiki version hydro_and_newer {{{ #!python block=listenerHydro #!/usr/bin/env python import roslib roslib.load_manifest('learning_tf') import rospy import math import tf import geometry_msgs.msg import turtlesim.srv if __name__ == '__main__': rospy.init_node('turtle_tf_listener') listener = tf.TransformListener() rospy.wait_for_service('spawn') spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn) spawner(4, 2, 0, 'turtle2') turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1) rate = rospy.Rate(10.0) while not rospy.is_shutdown(): try: (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue angular = 4 * math.atan2(trans[1], trans[0]) linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2) cmd = geometry_msgs.msg.Twist() cmd.linear.x = linear cmd.angular.z = angular turtle_vel.publish(cmd) rate.sleep() }}} }}}} Don't forget to make the node executable: {{{ chmod +x nodes/turtle_tf_listener.py }}} === The Code Explained === Now, let's take a look at the code that is relevant to publishing the turtle pose to tf. <> The tf package provides an implementation of a `tf.TransformListener` to help make the task of receiving transforms easier. <> Here, we create a `tf.TransformListener` object. Once the listener is created, it starts receiving tf transformations over the wire, and buffers them for up to 10 seconds. <> Here, the real work is done, we query the listener for a specific transformation by `lookupTransform`. Let's take a look at the arguments: 1. We want the transform from the '/turtle1' frame ... 1. ... to the '/turtle2' frame. 1. The time at which we want to transform. Providing `rospy.Time(0)` will just get us the latest available transform. This function returns two lists. The first is the (x, y, z) linear transformation of the child frame relative to the parent, and the second is the (x, y, z, w) quaternion required to rotate from the parent orientation to the child orientation. 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: {{{ ... }}} <> ## AUTOGENERATED DO NOT DELETE ## TutorialCategory ## TutorialTurtlesim ## PythonCategory ## LearningCategory