ROS Indigo learning_tf-05 now() 和 Time(0)的區別 (Python版) — waitForTransform() 函式

我使用的虛擬機器軟體：VMware Workstation 11
使用的Ubuntu系統：Ubuntu 14.04.4 LTS
ROS 版本：ROS Indigo

一. 前言

這一節要做的事情：使用 tf 的 now() 和 Time(0)的區別 。

為什麼要講這個，這是因為 ROS 的 tf 在進行座標之間的轉換變換不是實時的轉換，它有一個緩衝器來存放一段時間的座標轉換資料，所以有時，可能沒有當前時間的座標轉換資料，使用 lookupTransform()

函式就可以得到你想的某個時間的座標資料，前提是緩衝區裡要有這個時間的座標資料，tf 的座標轉換是自動計算的，所以有時，你想得到當前的時間的座標轉換資料，你呼叫 lookupTransform() 函式去獲取，但是緩衝器裡還沒有來得及去計算現在的座標轉換資料，就是說：現在還沒有。如果你非要獲取，就會出錯，所以你要使用 waitForTransform() 函式來等待 tf 的座標轉換執行緒得到你想要的時間點的座標轉換資料。
簡單的說：waitForTransform() 就是一個安全程式。

什麼意思，下面編寫程式，讓大家深入理解：

二. 寫程式

1 . 講解

程式碼就是下面這個樣子： learning_tf

軟體包中的 nodes/turtle_tf_listener.py :
將原始碼中：

    ----
    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try: 
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue
 

    ----

修改為：

    ----
    rate = rospy.Rate(10.0)
    listener.waitForTransform("/turtle2", "/turtle1", rospy.Time(), rospy.Duration(4.0))
    while not rospy.is_shutdown():
        try: 
            now = rospy.Time.now()
                 listener.waitForTransform('/turtle2', '/turtle1', now, rospy.Duration(4.0))         
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', now)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue
    ----

Q： 如果你在 try 裡使用了 waitForTransform() 函式，但是在 while 外並沒有 waitForTransform() ，程式就不正常執行。 為什麼必須要有while外的 waitForTransform() 函式？為什麼要呼叫 waitForTransform() 函式？

A： 在程式的開始，我們 再生（ /spawn 服務）了一個 turtle2 ，但是在 waitForTransform() 函式執行的時候，tf 裡可能還沒有 turtle2 座標系，就是 turtle2 還沒有 再生 完成。
所以，第一個 waitForTransform() 函式將會等到 turtle2 座標系 再生 完成。
第二個 waitForTransform() 函式將會等到 now 時間的座標系轉換完成，在緩衝器中有。

waitForTransform() 函式的4個形參：

waitForTransform( [父類座標系], [子類座標系], [在這一時刻], rospy.Duration(4.0) )

rospy.Duration(4.0) ： 為 waitForTransform() 函式 的結束條件：最多等待 4 秒，如果提前得到了座標的轉換資訊，直接結束等待。

2 . 編寫程式碼

在learning_tf 程式包中的 nodes 路徑下，新建一個檔案：turtle_tf_listener_timeTravel.py ：

roscd learning_tf/node/
gedit turtle_tf_listener_timeTravel.py

下面是完整的程式。將這段程式複製到 turtle_tf_listener_timeTravel.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()

    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)
    listener.waitForTransform("/turtle2", "/turtle1", rospy.Time(), rospy.Duration(4.0))
    while not rospy.is_shutdown():
        try: 
            now = rospy.Time.now()
            listener.waitForTransform('/turtle2', '/turtle1', now, rospy.Duration(4.0))         
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', now)
        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 777 turtle_tf_listener_timeTravel.py

3 . 編寫啟動檔案

在learning_tf 程式包中的 launch 路徑下，新建一個檔案：start_demo5.launch:

roscd learning_tf/launch/
gedit start_demo5.launch

將下面的程式碼拷貝進去。（下面這段程式碼就是通過 start_demo2.launch 檔案改寫的，基本和它一模一樣）

<launch>
    <!-- Turtlesim Node -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen" />

    <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
        <param name="turtle" type="string" value="turtle1" />
    </node>
    <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
        <param name="turtle" type="string" value="turtle2" />
    </node>

    <node pkg="learning_tf" type="turtle_tf_listener_timeTravel.py" name="listener" />

</launch>

4 . 執行

執行 start_demo5.launch 這個啟動指令碼檔案：

$ roslaunch  learning_tf start_demo5.launch 

執行效果：看起來和之前 “ 編寫一個 監聽器 程式 ”這一節的執行效果一樣 。

Q： 你肯定會問這樣的問題： rospy.Time.now() 和 rospy.Time(0) 有什麼不同嗎？執行時的效果都一樣？

A： 雖然，你注意到 小海龜的行為 沒有明顯的變化。那是因為：實際的時間只差幾毫秒的不同。但是我們為什麼有時使用Time(0) ,有時使用 now()。我這裡只是想告訴你：tf 緩衝和時間延遲是相關聯的。在實際使用 tf 時，這2個可以認為是等同的。

• Time(0) : 是 tf 快取裡的第一個 tf 資訊。
• now() : 是當前這個時間的 tf 資訊。

總結： 現在，我們知道了 now() 和 Time(0) 的區別，那它們究竟有什麼用呢？下一節，我帶你在 ROS 中進行現在與過去中的時間穿梭，你就知道了。
下一節： 現在與過去中穿梭 (Python版) — waitForTransformFull() 函式。