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

coordsystems_img

<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
廣告
本篇發表於 未分類。將永久鏈結加入書籤。

8 Responses to 【L2】讓 ROS 幫你處理煩雜的座標轉換!

  1. Alyson 說道:

    thank you!! It`s really helpful to understand TF library

  2. 索尼 說道:

    HI~請問何時還會有分享會呢?很想參加^^

  3. 索尼 說道:

    上次沒有哦!
    我自今年四月開始研究ROS,直到最近才拜讀您的文章
    台灣似乎非常少相關的資源
    向您看齊!

發表迴響

在下方填入你的資料或按右方圖示以社群網站登入:

WordPress.com Logo

您的留言將使用 WordPress.com 帳號。 登出 /  變更 )

Google+ photo

您的留言將使用 Google+ 帳號。 登出 /  變更 )

Twitter picture

您的留言將使用 Twitter 帳號。 登出 /  變更 )

Facebook照片

您的留言將使用 Facebook 帳號。 登出 /  變更 )

w

連結到 %s