【L2】讓 ROS 幫你處理煩雜的座標轉換！

<Pic Source>

不論是自動車、機器人、機器手臂 … ，當一切要讓電腦處理時，他都必須要用數學表示(嗚 …)

• 機器手臂每個關節如何轉動？
• 目標位置 vs 現在位置 ？
• Sensor 間資料如何比對 ？
• 上一筆資料跟這一筆資料如何比對？
• 我如何用之前收到的資訊，判斷下一步的動作？

這一切的起點都是『座標轉換』… 讓不同來源、時間的資訊能有比較基礎  

學習重點：

• Topic 1 – Broadcaster : 報~報~報~ 自己 Frame 的位置。
• Topic 2 – Listener：收~收~收~ 讓我看看你在哪？
• Topic 3 – Add a frame ：參考點給你一個 Frame !
• Topic 4 – About Time：加進時間~ 搞死你！
• Topic 5 – Time Travel：時光旅行！？我要抓舊資料！

• 教材：ROS WIKI Tutorial – tf    <link>

Topic 1 – Broadcaster  <link>

概述：Tutorial 常用的模擬小烏龜又出來啦！而且一次還來兩隻(第2隻拼命的想追第1隻，爬上他上面(羞~))，第一步：搞清楚兩隻各在哪裡！

Result :

At time 1440983505.904
– Translation: [5.544, 5.544, 0.000]
– Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
in RPY (radian) [0.000, -0.000, 0.000]
in RPY (degree) [0.000, -0.000, 0.000]

※ With Time Stamp ※注意它是相對哪個參考矩陣！(此為絕對世界座標)

※ Rotation 提供 四元數Quaternion(wiki) / Euler – Roll Pitch Taw 兩種表示式(各有擅場)

程式碼拆解：

Step 1. 搞清楚要服務誰
19  turtlename = rospy.get_param(‘~turtle’)
※ 利用 Parameter 讀取設定，可以讓 Node 更有彈性。

Step 2. Subscribe Pose Topic

20  rospy.Subscriber(‘/%s/pose’ % turtlename,
21                              turtlesim.msg.Pose,
22                              handle_turtle_pose,
23                              turtlename)

※ rospy.Subscriber ( topic 名子 / 資料類型 / 丟給誰處理 / 接手的人需要的 Tag )

※ 實際應為 Odometry / IMU / GPS … etc Node 所發佈的 Topic.

Step 3. Subscribe Pose Topic

10   br = tf.TransformBroadcaster()
11   br.sendTransform((msg.x, msg.y, 0),
12                                 tf.transformations.quaternion_from_euler(0, 0, msg.theta),
13                                 rospy.Time.now(),
14                                 turtlename,
15                                 “world")

※ br.sendTransform(平移矩陣，轉換成四元數表示法，打上 Time Stamp ，Tag ，相對於世界座標 )

Test ：

按照教學跑 launch file 會開啟模擬器、turtle 1 鍵盤控制器及兩個新建的Node廣播兩隻烏龜的 Frame

$   rosrun tf tf_echo /world /turtle1  // 我要 龜1 絕對座標

$   rosrun tf tf_echo /turtle2 /turtle1  // 我要 龜1座標 相對龜2座標的轉換矩陣。

$   rosrun tf tf_echo /turtle1 /turtle2  // 我要 龜2座標 相對龜1座標的轉換矩陣。

※ 後兩個跑跑看，就知道 tf 的強大了。

Topic 2 – Listener  <link>

Result :

創造一支龜2 →讀到龜1相對位置→按照預設準則輸出命令→前進→壓上去 XD

程式碼拆解：

code 15 ~ 19 : 呼叫 turtlesim 的 service/ spwan 新建一隻烏龜(/Turtle2)，並設定控制他的 Command Publisher..

Listener 使用說明:

Step 0. $ import tf    // 呼叫 library

Step 1. $ listener = tf.TransformListener()       // 開啟接收器，它有 10 s 的 Buffer 容量。

Step 2. 使用 lookupTransform 轉換所需的座標視角

23    rate = rospy.Rate(10.0)                       // 搭配 rate.sleep 控制 Loop 執行頻率：10Hz
24    while not rospy.is_shutdown():          // Loop 持續
25        try:
26            (trans,rot) = listener.lookupTransform(‘/turtle2’, ‘/turtle1’, rospy.Time(0))
27        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
28             continue

※ listener.lookupTransform(‘觀察者 Frame ‘, ‘ 目標Frame ‘ , 定義要的時間)

※ rospy.Time(0) 指要手上最近的資料。

Topic 3 – Add a Frame  <link>

甚麼時候你需要加 Frame :

• 新增一個 Sensor 時
• 某機構關節處
• 你不想騎上另ㄧ支烏龜的時候… XD <Set Target Point …>

程式碼拆解：

Step 0. $ import tf    // 呼叫 library

Step 1. $ br = tf.TransformBroadcaster()  // 開啟廣播器 Objrct

Step 2. 使用 sendTransform 由已有座標建立一個相對座標軸

13     br.sendTransform((0.0, 2.0, 0.0),                       // 平移矩陣
14                                   (0.0, 0.0, 0.0, 1.0),                // 四元素旋轉矩陣
15                                    rospy.Time.now(),               // 叫他用這時間點做
16                                    “carrot1″,                             // 命名新矩陣
17                                    “turtle1″)                              // 他老爸 (Parent frame)

Topic 4- tf and Time <link>

Issue : " Data Transport 永遠有 Delay “

將 Listener 中 LookTransform 的時間 rospy.Time(0)  改為 rospy.Time.now() →馬上悲劇，因為轉換矩陣還沒進來！

立馬介紹你便宜行事的解法 " waitForTransform" Block until timeout or transform is available.
1   listener.waitForTransform(“/turtle2″, “/carrot1″, rospy.Time(), rospy.Duration(4.0))
2   while not rospy.is_shutdown():
3       try:
4           now = rospy.Time.now()
5           listener.waitForTransform(“/turtle2″, “/carrot1″, now, rospy.Duration(4.0))
6           (trans,rot) = listener.lookupTransform(“/turtle2″, “/carrot1″, now)

簡單，我只等你四秒 ~ 快送過來 … 等等 … 這樣接下來的指令不都悲劇了嗎？

• Danger  <TF ROS Tutorial: Part 2 of 4 約 5:00 處 >
• No data comimg in if a single thread
• Block all process <can make all process late >
• Delay the system unnecessarily  <無線傳輸資料~用這~最慘！>

另一個同步的方法  tf::messageFilter <似乎還沒出 python API … 期待中…>

Purpose :

• Privide a non-blocking way to queue data
• pending transform data availability

Usage :

• Register a target frame and an incoming topic
• Receive a callback when transforms are available

Topic 5 –Time travel with tf <link>

其實就是教 lookupTrandform 主從座標軸，可以個別設定要選用的時間點而已。XD

   1 try:
   2    now = rospy.Time.now()
   3    past = now - rospy.Duration(5.0)
   4    listener.waitForTransformFull("/turtle2", now,
   5                                  "/turtle1", past,
   6                                  "/world", rospy.Duration(1.0))
   7    (trans, rot) = listener.lookupTransformFull("/turtle2", now,
   8                                       "/turtle1", past,
   9                                       "/world")

注意：這裡使用的是 waitForTransformFull /lookupTransformFull