ROS 機器人技術 - TF 坐標系統基本概念

項目寫 ROS 語義融合節點需要使用 TF 坐標，這裡記錄下我學習下 TF 包的筆記。

一、TF 和 TF2 是什麼？1.1 TF

ROS TF 是一個可以讓機器人隨著時間跟蹤坐標系之間變換關係的功能包。

TF 的內部是樹形結構，內部的坐標系變換通過緩衝區維護，可以讓用戶在任意時間將點、向量等數據的坐標在不同的坐標系中變換。

我們知道一個機器人系統通常含有多個隨著時間變化的 3D 坐標系（coordinate frames），比如常見坐標系如下：

TF 包的作用就是隨著時間的變化跟蹤這些坐標系，並且用戶可以訪問過去和現在的一些坐標變換，比如：

5 秒前，機器人頭部坐標系相對於世界坐標系的變換關係是怎樣的？機器人的爪子夾取的物體相對於機器人中心坐標系的位置在哪裡？

並且 TF 系統在 ROS 中是分布式的，意思是一個 ROS 系統中廣播的所有坐標系變換關係對於所有的 ROS 節點都是可用的。

每個訂閱了 TF 消息的節點都會緩衝一份當前機器人所有坐標系的變換關係，使得坐標變換非常方便。

比如你定好了機器人的雷達和相機的相對位置，並且標定的他們的坐標關係，那麼通過在 ROS 中用 TF 廣播雷達和相機的坐標關係，然後所有節點都可以使用你發布的這個變換。

TF：

http://wiki.ros.org/tf

1.2 TF2

自從 ROS Hydro 版本發布後，ROS 官方就棄用了 TF 包，轉而用第二代的 TF2 代替。

TF2 完全兼容第一代的 TF，並且 TF2 接口更清晰，更加容易使用，官方也建議最新的工作使用 TF2 系統。

所以大家現在創建的項目使用最新的 TF2 就可以了，完全兼容 TF的。

TF2：

http://wiki.ros.org/tf2

以下我就用 TF 代表 TF2 了哈。

二、使用 TF 系統的 2 個重要步驟

ROS 官方給了使用 TF 系統的 2 個重要步驟，每個使用 TF 的同學都應該提前知道這 2 步，跟發布和訂閱主題類似：

廣播 TF 變換：向 ROS 中廣播坐標系的相對變換關係，ROS 系統的不同部分都可以廣播坐標變換，不需要進行同步等操作。監聽 TF 變換：接收並緩存 ROS 系統中廣播的所有坐標系變換，並且可以查詢指定幀之間的坐標變換。

基本的 TF 概念就介紹完了，下面用個小烏龜跟隨的例子來看下 TF 系統到底做了什麼。

三、小烏龜帶你理解 TF 如何工作3.1 安裝並運行小烏龜

我們用官方的 Demo 來解釋 TF 系統，先來安裝 Demo：

sudo apt-get install ros-$ROS_DISTRO-turtle-tf2 ros-$ROS_DISTRO-tf2-tools ros-$ROS_DISTRO-tf
注意要把 $ROS_DISTRO 替換為你的 ROS 版本，比如 kinect：
sudo apt-get install ros-kinect-turtle-tf2 ros-kinect-tf2-tools ros-kinect-tf
安裝好後運行這個 demo，記得學習 ROS 環境搭建的基礎：
roslaunch turtle_tf2 turtle_tf2_demo.launch
運行出現 2 個經典的小烏龜：
切換到運行 roslaunch 的終端，按鍵盤的上下左右鍵移動小烏龜，會發現第二個白色小烏龜會跟隨第一個綠色小烏龜而運動：
3.2 分析小烏龜
上面這個例子中使用了 3 個坐標系：
turtle1 frame：第一個綠色小烏龜的坐標系turtle2 frame：第二個白色小烏龜的坐標系
在程序中使用 TF 廣播小烏龜之間的參考系，然後在監聽 TF 消息後計算 2 個烏龜的位置差，從而使得第二個烏龜跟隨第一個烏龜移動。
tf 提供了 view_frame 工具來查看系統當前廣播的 TF 變換：
rosrun tf2_tools view_frames.py
會生成一個 frames.pdf 文件：
Listening to tf data during 5 seconds...
Generating graph in frames.pdf file...
我們直接打開這個 pdf 文件：
evince frames.pdf
解釋下：
當前 TF 系統廣播了 3 個坐標系：world frame、turtle1 frame、turtle2 frameworld frame 是 turtle1 和 turtle2 的父坐標系節點除了坐標系的關係，還包括調試用的發送頻率，緩衝區長度，最新的和最早的轉換
還可以實時查看 ROS 系統中存在的 TF 變換：
rosrun rqt_tf_tree rqt_tf_tree
如下圖：圖片待補充
除了查看參考系的結構圖，TF 還提供一個能夠查看兩個坐標系相對變換的工具 tf_echo：
# 輸出 source_frame 到 target_frame 的坐標變換關係
# 或者 target_frame 相對於 source_frame 的坐標變換關係
rosrun tf tf_echo [source_frame] [target_frame]
turtle1 到 turtle2 的坐標變換公式如下，先把 turtle1 轉到 world 坐標系，再從 world 坐標系轉到 turtle2 坐標系：
我們來輸出第一個綠色烏龜到第二個白色烏龜的坐標變換：
rosrun tf tf_echo turtle1 turtle2
輸出類似下面的信息：
當你用上下左右鍵移動第一個烏龜，會發現上面的坐標輸出會隨之更新，裡面顯示了一些表示烏龜 2 相對於烏龜 1 的坐標（X, Y, Z）和旋轉角（四元數和歐拉角表示）。
3.3 使用 RVIZ 查看 TF 變換
我們可以使用 RVIZ 來直觀的查看系統當中的 TF 變換，啟動上面 demo 對應的 rviz 配置：
rosrun rviz rviz -d `rospack find turtle_tf2`/rviz/turtle_rviz.rviz
顯示如下，記得搜索下 Rviz 的基本用法哈，這裡滑動滑鼠滾輪可以放大查看，使用鍵盤移動小烏龜會發現坐標系也會隨之移動：
今天就寫這些，下期跟大家繼續分享 TF 的使用方法。