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()開(kāi)啟tf.TransformListener結(jié)構(gòu)體累舷，一旦listener開(kāi)啟铃芦，就開(kāi)始接受坐標(biāo)變換數(shù)據(jù)待逞，數(shù)據(jù)有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是變換的起始坐標(biāo)系，frame2是變換的目的坐標(biāo)系。簡(jiǎn)單說(shuō)就是將坐標(biāo)系1變換到坐標(biāo)系2.time是轉(zhuǎn)換時(shí)間，rospy.Time(0)表示馬上轉(zhuǎn)換存谎。最后的結(jié)果是坐標(biāo)系1相對(duì)坐標(biāo)系2的坐標(biāo)。
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.運(yùn)行l(wèi)istener
修改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.檢查結(jié)果
按照官方文檔運(yùn)行后會(huì)報(bào)錯(cuò)肥隆，如下所示是官方文檔部分
運(yùn)行后terminal報(bào)錯(cuò)既荚，找不到turtler2坐標(biāo)系
[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.
出現(xiàn)問(wèn)題的原因是創(chuàng)建第二個(gè)海龜坐標(biāo)系并發(fā)布數(shù)據(jù)需要一些時(shí)間，而在此之前l(fā)istener無(wú)法讀取數(shù)據(jù)栋艳。
實(shí)際測(cè)試過(guò)程中運(yùn)行ok恰聘，可能是電腦比較慢的緣故。吸占。
