美文网首页
02-TF-tf监听

02-TF-tf监听

作者: joey_zhou | 来源:发表于2016-08-04 18:58 被阅读146次

    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,可能是电脑比较慢的缘故。。

    相关文章

      网友评论

          本文标题:02-TF-tf监听

          本文链接:https://www.haomeiwen.com/subject/jzbmdttx.html