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