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 的数据
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 = _param("~base_frame", 'base_link')
self.base_frame1 = _param("base_frame", 'base_link')
self.yaw = 0
<_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),
w(),
"tf_destination",
"world")
gps_br = tf.TransformBroadcaster()
gps_br.sendTransform((800, 1200, 0),
(0, 0, 0, 1),
w(),
"tf_gps",
"world")
rospy.sleep(1)
def shutdown():
print "shut "
if __name__ == '__main__':
myArduino = ArduinoROS()
监听的 listen_tf.py 的内容如下，
(trans,rot) = listener.lookupTransform('/tf_gps', '/tf_destination', rospy.Time(0))返回值trans rot是啥， 在⽹上苦苦寻⽆果，只好⾃⼰测试得出
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()
<_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 "
if __name__ == '__main__':
myArduino = ArduinoROS()
测试结果如下：
测试结论：
lookup函数返回值不对
(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)

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。