python 中关于ROS的 TF 变换函数返回值

测试 launch 文件如下， test.py 广播 TF 变换， listen_tf.py 文件监听TF变换。

<launch>

  <node name="agv_driver" pkg="test" type="test.py" output="screen" respawn="true">
    <rosparam file="$(find test)/config/test.yaml" command="load" />
  </node>

  <!--node pkg="test" type="test1" name="test1" required="true" output="screen" > 
    <rosparam file="$(find test)/config/test1.yaml" command="load"/>
  </node--> 

  <node name="listen" pkg="test" type="listen_tf.py" output="screen"  respawn="true"> 
  </node>

</launch>

广播 test.py 内容如下，发布了两个TF 的数据

#!/usr/bin/env python
#-*-coding: utf-8 -*-

import rospy
from geometry_msgs.msg import Twist
import tf

class ArduinoROS():
    def __init__(self):
        # 唯一的节点名 日志级别为DEBUG 等级排序为 DEBUG INFO WARN ERROR FATAL
        rospy.init_node('Arduino', log_level=rospy.DEBUG)
        # self. 变量， 从 .yaml 获取参数
        self.base_frame = rospy.get_param("~base_frame", 'base_link')
        self.base_frame1 = rospy.get_param("base_frame", 'base_link')

        self.yaw = 0

        rospy.on_shutdown(self.shutdown)

        while not rospy.is_shutdown():
            #print "param : " + self.base_frame + "  " + self.base_frame1

            des_br = tf.TransformBroadcaster()
            # 发布 tf 变换
            des_br.sendTransform((1000, 1000, 0),
                                (0, 0, 0, 1),
                                rospy.Time.now(),
                                "tf_destination",
                                "world")

            gps_br = tf.TransformBroadcaster()
            gps_br.sendTransform((800, 1200, 0),
                                (0, 0, 0, 1),
                                rospy.Time.now(),
                                "tf_gps",
                                "world")
            
            rospy.sleep(1)

    def shutdown():
        print "shut down node..."
        
if __name__ == '__main__':
    myArduino = ArduinoROS()

监听的 listen_tf.py 的内容如下，

(trans,rot) = listener.lookupTransform('/tf_gps', '/tf_destination', rospy.Time(0))

返回值trans rot是啥， 在网上苦苦找寻无果，只好自己测试得出

#!/usr/bin/env python
#-*-coding: utf-8 -*-

import rospy
from geometry_msgs.msg import Twist
import tf
import math

class ArduinoROS():
    def __init__(self):
        # 唯一的节点名 日志级别为DEBUG 等级排序为 DEBUG INFO WARN ERROR FATAL
        rospy.init_node('listen', log_level=rospy.DEBUG)

        listener = tf.TransformListener()

        rospy.on_shutdown(self.shutdown)

        while not rospy.is_shutdown():
            try:
                (trans,rot) = listener.lookupTransform('/tf_gps', '/tf_destination', rospy.Time(0))
                rospy.loginfo("trans: %f %f %f ", trans[0], trans[1], trans[2])
                rospy.loginfo(" rot : %f %f %f %f", rot[0], rot[1], rot[2], rot[3])

                #print trans, rot
                #angular = 0.4 * math.atan2(trans[1], trans[0])
                #linear  = 0.4 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
                #rospy.loginfo("trans = %f  %f  ,linear = %f ,angular = %f", trans[0], trans[1], linear, angular)

            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                print "param : "

            rospy.sleep(1)

    def shutdown():
        print "shut down node..."
        
if __name__ == '__main__':
    myArduino = ArduinoROS()

 

测试结果如下：

测试结论：

 (trans,rot) = listener.lookupTransform('/tf_gps', '/tf_destination', rospy.Time(0))

返回的trans 是三个轴x  y  z的相对距离

返回的 rot  是四元数变换，不过在这里没有深究

 

现在知道了，这两句话是什么意思了

angular = math.atan2(trans[1], trans[0])
linear  = math.sqrt(trans[0] ** 2 + trans[1] ** 2)