本篇教程演示如何建立新坐標(biāo)系，建立新坐標(biāo)系與創(chuàng)建tf廣播類(lèi)似，同時(shí)能加強(qiáng)tf功能。

1.為什么添加坐標(biāo)系

很多實(shí)例中，分系統(tǒng)的本地坐標(biāo)系理解起來(lái)比較簡(jiǎn)單，比如激光雷達(dá)的坐標(biāo)系在雷達(dá)的幾何中心，tf允許在任何傳感中定義一個(gè)坐標(biāo)系，tf的作用也在于整理好坐標(biāo)系之間的轉(zhuǎn)換關(guān)系。

2.坐標(biāo)系在哪兒添加

tf建立了一個(gè)樹(shù)狀結(jié)構(gòu)的坐標(biāo)系組，坐標(biāo)系組中不允許閉環(huán)存在。這意味著一個(gè)坐標(biāo)系只有一個(gè)父坐標(biāo)系，但可以有多個(gè)子坐標(biāo)系。前面示例中使用的系統(tǒng)擁有三個(gè)坐標(biāo)系：世界坐標(biāo)系，海龜1，海龜2.兩個(gè)海龜坐標(biāo)系是世界坐標(biāo)系的子坐標(biāo)系。當(dāng)我們要添加一個(gè)新的坐標(biāo)系到tf中時(shí)，三個(gè)已知的坐標(biāo)中的一個(gè)坐標(biāo)系需要是其父坐標(biāo)系，新的坐標(biāo)系將成為子坐標(biāo)系。

3.如何添加坐標(biāo)系

在海龜示例中，我們?cè)诘谝粋€(gè)海龜中添加一個(gè)坐標(biāo)系，這個(gè)坐標(biāo)系是第二個(gè)海龜?shù)摹昂}卜”，如下所示進(jìn)入當(dāng)前工作包，添加代碼。

$ roscd learning_tf
$ vim nodes/fixed_tf_broadcaster.py

import rospy
import tf

if __name__ == '__main__':
    rospy.init_node('my_tf_broadcaster')
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        br.sendTransform((0.0, 2.0, 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "carrot1",
                         "turtle1")
        rate.sleep()

4.運(yùn)行坐標(biāo)系發(fā)布程序

在start_demo.launch文件中添加下面的內(nèi)容。

$vim launch/start_demo.launch

<launch> ... 
<node pkg="learning_tf" type="fixed_tf_broadcaster.py" name="broadcaster_fixed" />  
</launch>

5.檢查結(jié)果

當(dāng)運(yùn)行上面的launch文件時(shí)，會(huì)發(fā)現(xiàn)結(jié)果與之前一致，海龜2還是跟隨海龜1而不是新建的“carrot”坐標(biāo)系。這是因?yàn)橹暗膌istner文件中海龜2的坐標(biāo)系是參照海龜1的坐標(biāo)系計(jì)算速度的。如果將參考坐標(biāo)系改為“carrot”坐標(biāo)系計(jì)算速度信息，即可使得海龜2跟隨“carrot”坐標(biāo)系。

$ vim nodes/turtle_tf_listener.py

將lookupTransform后面的/turtle1替換為/carrot1

''' 
(trans,rot) = listener.lookupTransform("/turtle2", "/carrot1", rospy.Time(0))
'''

roslaunch learning_tf start_demo.launch、

6.運(yùn)動(dòng)坐標(biāo)系的廣播

我們?cè)诮坛讨行陆ǖ淖鴺?biāo)系相對(duì)于父坐標(biāo)系的位置是固定的，因此是固定坐標(biāo)系。然而，當(dāng)需要發(fā)布一個(gè)移動(dòng)坐標(biāo)系時(shí)，就需要隨時(shí)改變廣播，下面的實(shí)例中嘗試將carrot1坐標(biāo)系設(shè)置為移動(dòng)坐標(biāo)系，隨著時(shí)間改變相對(duì)于海龜坐標(biāo)系1的位置。
#!/usr/bin/env python
import roslib
roslib.load_manifest('learning_tf')

import rospy
import tf
import math

if __name__ == '__main__':
    rospy.init_node('my_tf_broadcaster')
    br = tf.TransformBroadcaster()
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        t = rospy.Time.now().to_sec() * math.pi
        br.sendTransform((2.0 * math.sin(t), 2.0 * math.cos(t), 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "carrot1",
                         "turtle1")
        rate.sleep()