02-TF-tf监听

2016-08-04  本文已影响146人  joey_zhou

1.编码

$ roscd learning_tf
$ vim 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()
  #tf.TransformListener()开启tf.TransformListener结构体,一旦listener开启,就开始接受坐标变换数据,数据有10s的缓存。
    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))
        #核心代码解析:lookupTransform('frame1','frame2',rospy.Time(0))
        #frame1是变换的起始坐标系,frame2是变换的目的坐标系。简单说就是将坐标系1变换到坐标系2.time是转换时间,rospy.Time(0)表示马上转换。最后的结果是坐标系1相对坐标系2的坐标。
        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()

chmod +x nodes/turtle_tf_listener.py

2.运行listener

修改launch文件

vim launch/start_demo.launch

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

roslaunch learning_tf start_demo.launch

3.检查结果

按照官方文档运行后会报错,如下所示是官方文档部分
运行后terminal报错,找不到turtler2坐标系

[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无法读取数据。
实际测试过程中运行ok,可能是电脑比较慢的缘故。。

上一篇下一篇

猜你喜欢

热点阅读