二 tf / Tutorials / Writing a tf listener (Python)

原文地址：http://wiki.ros.org/tf/Tutorials/Writing a tf listener (Python)

Writing a tf listener (Python)

描述：本教程教您如何使用tf获得坐标系变换。
在前面的教程中，我们创建了一个tf广播器来发布乌龟对tf的姿态。在本教程中，我们将创建一个tf侦听器来开始使用tf。

1 How to create a tf listener

让我们首先创建源文件。转到在上一教程中创建的包：

 $ roscd learning_tf

1.1 The Code

启动您最喜欢的编辑器，并将以下代码粘贴到一个名为nodes/turtle_tf_listener.py的新文件中。



   1 #!/usr/bin/env python  
   2 import roslib
   3 roslib.load_manifest('learning_tf')
   4 import rospy
   5 import math
   6 import tf
   7 import geometry_msgs.msg
   8 import turtlesim.srv
   9 
  10 if __name__ == '__main__':
  11     rospy.init_node('turtle_tf_listener')
  12 
  13     listener = tf.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/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)
  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 (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
  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         cmd = geometry_msgs.msg.Twist()
  31         cmd.linear.x = linear
  32         cmd.angular.z = angular
  33         turtle_vel.publish(cmd)
  34 
  35         rate.sleep()

不要忘记使节点可执行：

chmod +x nodes/turtle_tf_listener.py

1.2 The Code Explained

现在，让我们看一下与向tf发布海龟姿态相关的代码。

   6 import tf

tf包提供了tf.TransformListener的实现，以帮助简化接收转换的任务。

  13     listener = tf.TransformListener()

这里，我们创建一个tf.TransformListener对象。一旦侦听器被创建，它就开始通过连线接收tf转换，并缓冲它们长达10秒。

  23     rate = rospy.Rate(10.0)
  24     while not rospy.is_shutdown():
  25         try:
  26             (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
  27         except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
  28             continue

在这里，实际工作已经完成，我们通过lookupTransform查询侦听器以获得specific transform。让我们看看这些arguments：
(1) We want the transform from the ‘/turtle1’ frame …
(2) … to the ‘/turtle2’ frame.
(3) The time at which we want to transform. Providing rospy.Time(0) will just get us the latest available transform.
这个函数返回两个列表。第一个是子坐标系相对于父坐标系的(x，y，z)线性变换，第二个是从父方向旋转到子方向所需的(x，y，z，w)四元数。
所有这些都被包装到一个try-except块中，以捕获可能的异常。

2 Running the listener

使用文本编辑器，打开名为start_demo.launch的启动文件，并添加以下行：

  <launch>
    ...
    <node pkg="learning_tf" type="turtle_tf_listener.py" 
          name="listener" />
  </launch>

首先，确保您停止了前一个教程中的启动文件（使用ctrl-c）。现在，您已经准备好开始您的完整turtle demo
：

 $ roslaunch learning_tf start_demo.launch

你会看到turtlesim中有两只turtle。

3 Checking the results

要查看是否有效，只需使用箭头键驱动第一只乌龟（确保您的终端窗口是活动的，而不是您的模拟器窗口），您将看到第二只乌龟跟在第一只乌龟后面！
当turtlesim开始，你可能会看到：

[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.

这是因为我们的listener试图在收到关于turtle2的消息之前计算transform，因为要在turtlesim产卵并开始广播tf坐标系需要一点时间。
现在，您已经准备好进入下一个教程，在那里您将学习如何添加坐标系。