ROS：关于tf的探索（2）Writing a tf listener(Python)

在前面的教程中，我们创造了一个broadcaster去发布乌龟的坐标给tf，在本节中，将创建一个listener来使用tf。

1.如何创建一个listener

首先创建资源文件，切换到包目录下

roscd learning_tf

1.1代码
创建一个nodes/turtle_tf_listener.py的文件，并将如下代码复制其中。

vim nodes/turtle_tf_listener.py

【vim编辑器：按i开始插入，编辑完成后，按 ESC，输入“：wq”保存退出】

#!/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('tf_turtle')

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

使Python文件可执行

chmod +x nodes/turtle_tf_listener.py

1.2代码解释

import tf

tf包使得tf.TransformListener可实现，使接收变换的任务更加容易。

listener = tf.TransformListener()

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

 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

到这里，真正的工作做完了，我们通过 lookupTransform 向listener询问到了特定的转换，让我们来看以下都有哪些参数：

我们想让转换从这个框架...
... 到这个框架.
我们想要转换的时间。 提供rospy.Time（0）只会得到我们最新的可用变换。

所有这些都包裹在try-except块中以捕获可能的异常。

2.执行listener

用编辑器打开start_demo.launch文件，添加下面的语句

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

执行下面的语句即可看到两只乌龟。

roslaunch learning_tf start_demo.launch

3.检查结果

通过控制方向键来控制一只乌龟运动，可以看到另一只乌龟在追随所控制的乌龟。