ROS与Python入门教程-TF-编写tf的listener(监听器)

ROS与Python入门教程-TF-编写tf的listener(监听器)

说明

• 介绍创建TF监听器使用TF

创建TF监听器

• 创建节点nodes/turtle_tf_listener.py

$ roscd learning_tf
$ touch nodes/turtle_tf_listener.py
$ chmod +x nodes/turtle_tf_listener.py
$ rosed nodes/turtle_tf_listener.py

• 手工输入代码：

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

代码分析：

• 代码：import tf

• 分析：TF包提供了一个tf.transformlistener实现使接收任务将更容易。

• 代码：listener = tf.TransformListener()

• 分析：在这里，我们创建了一个tf.transformlistener对象。一旦侦听器被创建，它就开始接收转换，并缓冲他们长达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

• 分析：
  • 通过listener.lookupTransform函数来实现获取变换
  • 第一参数：变换从/turtle2坐标系
  • 第二参数：到/turtle1坐标系
  • rospy.Time(0)，想变换的时间，rospy.Time(0)获取最新的变换。

运行节点

• 打开start_demo.launch

$ roscd learning_tf
$ rosed launch/start_demo.launch

• 手工输入增加的代码

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

• 运行

$ roslaunch learning_tf start_demo.launch

检查结果：

• 要查看是否有工作，只使用箭头键驱动第一只海龟（确保你的终端窗口是活动的，而不是你的模拟器窗口），你会看到第二只海龟跟随第一只！

异常情况

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

• 这是因为监听器进行变换计算，但/turtle2来没获取，变换发布需要一定的时间来启动。