前言

大家好，介紹完筆者團隊的控制自走車研究後，今天我們則要來介紹如何在 Gazebo 世界中取得我們機器人的座標資訊。在實際找出座標資訊之前，我們也會先簡單介紹一下 ROS 中座標系之間的關係，畢竟釐清這個概念對於 ROS 開發來說也是不可或缺的環節，那麼以下就直接進入今天的內容吧!

座標系

在介紹 .xacro 檔時我們使用了上圖來解釋 <link> 與 <joint> 之間的關係，而今天我們再將這張圖做進一步的延伸，用來解釋定義座標系的重要性：

左圖為靜止狀態中機器人各個部件的座標軸位置，假設今天機器人想要拿取一個放在桌上的物體，可以看到機器人的兩節手臂在動作時，其座標軸 arm_1、arm_2 也跟著改變，而且 arm_2 的位置又受到 arm_1 段的影響，更甚至今天機器人在離桌子更遠的地方，那麼要機器人拿取桌上的物體就必須計算出更多座標系之間的資訊。

回傳 Gazebo 中機器人的座標資訊

當然，今天我們的主題並沒有那麼複雜，主要還是著重在於得到機器人在 Gazebo 當中的座標資訊，其實也不用擔心太複雜，因為 ROS 本身已經有一套相當好用的 Package 可以使用啦，馬上就來介紹一下吧。

tf 函式庫

tf 是 ROS 系統中內建的一個 Package，其功能在於可以讓使用者快速地得到兩個部件之間其座標的相對資訊，甚至可以運算出過去的時間點中某個部件對應的空間資訊。由於一個機器人常常是由複數個部件所構成，因此 tf 函式庫的出現可說是替開發者節省了很多時間。以下我們先透過幾個簡單的 tf 指令來瞭解一下其功用：

基本 tf 指令

首先，先開啟任意一個帶有 waffle 的 Gazebo 世界：

roslaunch turtlebot3_gazebo turtlebot3_world.launch

接著可以透過以下指令來看當前座標系之間的關係：

rosrun tf view_frames

看到 Terminal 上跳出幾行訊息後，此時便會生成一份叫做 frames.pdf 的檔案如下圖：

在這當中可以看到目前有兩個座標系分別為 odom 與 base_footprint，odom 即為 Gazebo 世界中的原點座標；而 base_footprint 便是 waffle 本身的座標。那麼為什麼兩者會命名為 odom 與 base_footprint 呢?原因可以從 turtlebot3_waffle.gazebo.xacro 當中的這一段定義中看到：

  <gazebo>
    <plugin name="turtlebot3_waffle_controller" filename="libgazebo_ros_diff_drive.so">
      
      ...
      
      <odometryFrame>odom</odometryFrame>
      
      ...
      
      <robotBaseFrame>base_footprint</robotBaseFrame>
      
      ...
      
    </plugin>
  </gazebo>

有沒有覺得 .xacro 檔愈來愈神奇阿，可以看到檔案中對其預設的命名即為 odom 與 base_footprint。那麼接著我們能再透過以下指令觀察到兩者之間的相對座標：

rosrun tf tf_echo base_footprint odom

可以看到當前 Terminal 上回傳了 Translation、Rotation in Quaternion 與 Rotation in RPY，以下簡單個別解釋一下：

1. Translation: 位置資訊，即為卡氏座標系的 [x, y, z]
2. Rotation in Quaternion: 旋轉資訊，四個參數為四元數表示法 a+bi+cj+dk
3. Rotation in RPY: 旋轉資訊，就是曾有有提到的 [roll, pitch, yaw]

根據 Translation 所回饋的數值我們就可以知道這應為 waffle 相對於原點的座標資訊，因此原指令 rosrun tf tf_echo [frame1] [frame2] 表示的是 [frame1] 相對於 [frame2] 的座標資訊。

基本的指令操作就介紹到這邊，接著我們進入 tf 程式碼的部分。tf 函式庫分成 C++ 與 Python 兩種語法可做使用，而今天筆者就使用 Python 與一個簡單的範例來介紹如何回傳機器人在 Gazebo 當中的座標：

程式碼

首先，先在 catkin_ws/src/ 下建立一個名為 tf_test.py 的 Package

catkin_create_pkg tf_test rospy

接著建立檔案 tf_listener.py，並輸入以下程式碼：

#!/usr/bin/env python  
import roslib
roslib.load_manifest('tf_test')
import rospy
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('waffle_position')
    listener = tf.TransformListener()

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/odom', '/base_footprint', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        print('x = ',trans[0])
        print('y = ',trans[1])
        rate.sleep()

我們直接來看這段程式碼：

        try:
            (trans,rot) = listener.lookupTransform('/odom', '/base_footprint', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

其中，listener.lookupTransform 函式的定義為：

lookupTransform(target_frame, source_frame, time) -> (position, quaternion)

意思是說，執行該函式後將得到 source_frame 相對於 target_frame 的座標資訊，time 表示相對時間，使用 rospy.time(0) 則表示兩者當前的相對資訊。該函式會輸出成 position 與 quaternion 兩個 list，分別儲存 Translation (x, y, z) 與 Rotation (x, y, z, w)。
在此範例中，我們欲求得當前 base_footprint 相對於 odom 之座標，並將其輸出為 trans 與 rot。

成功建立後，我們繼續使用 turtlebot3_world.launch 來進行測試。成功開啟世界後，執行前幾天在 robot_control/src/ 中建立好的 test.py，接著再回到 tf_test/src/ 下執行 tf_listener.py，就能看到 Terminal 上不斷印出當前 waffle 的座標囉。

結語

今天我們介紹大致介紹了座標系的重要性，以及使用兩個基礎的 tf 指令以及一個基本的 tf 函式，希望大家對於 tf 這項工具有一個初步的認識。明天我們則會開始介紹 rviz 在 Gazebo 當中的應用。