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.检查结果

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