2019年07月02日21:43:05
ROS中的TF
官網建議新工作直接使用tf2，因為它有一個更清潔的界面，和更好的使用體驗。（自ROS Hydro以來，tf第一代已被“棄用”，轉而支持tf2）
TF介紹
TF(TransForm)，就是坐標轉換，包括了位置和姿態兩個方面的變換。注意區分坐標轉換和坐標系轉換。
坐標轉換是一個坐標在不同坐標系下的表示，而坐標系轉換不同坐標系的相對位姿關系。
ROS中機器人模型包含大量的部件，每一個部件統稱之為link（比如手部、頭部、某個關節、某個連桿）,每一個link上面對應着一個frame（坐標系）, 用frame表示該部件的坐標系，frame和link是綁定在一起的。
TF是一個通俗的名稱，實際上它有很多含義：
- 可以被當做是一種標准規范，這套標准定義了坐標轉換的數據格式和數據結構．tf本質是樹狀的數據結構，即"tf tree"。
- tf也可以看成是一個話題
/tf，話題中的消息保存的就是tf tree的數據結構格式。維護了整個機器人的甚至是地圖的坐標轉換關系。維持並更新機器人整個坐標系的話題是/tf，/tf話題表示的內容是整個機器人的tf樹，而非僅僅是某兩個坐標系的轉換關系，這樣的話，/tf話題是需要很多的節點來維護的，每一個節點維護兩個frame之間的關系。 - tf還可以看成是一個package,它當中包含了很多的工具．比如可視化，查看關節間的tf,debug tf等等．
- tf含有一部分的API接口，用來節點程序中的編程。TF對發布器與訂閱器進行了封裝，使開發者通過TF的接口更加簡單地建立對TF樹中某些坐標系轉換關系的維護與訂閱。
tf是一個樹狀結構，維護坐標系之間的關系，靠話題通信機制來持續地發布不同link之間的坐標關系。作為樹狀結構，要保證父子坐標系都有某個節點在持續地發布他們之間的位姿關系，才能使樹狀結構保持完整。只有父子坐標系的位姿關系能被正確的發布，才能保證任意兩個frame之間的連通。
如果出現某一環節的斷裂，就會引發error系統報錯．所以完整的tf tree不能有任何斷層的地方，這樣我們才能查清楚任意兩個frame之間的關系。
每兩個相鄰frame之間靠節點發布它們之間的位姿關系，這種節點稱為broadcaster。broadcaster就是一個發布器publisher,如果兩個frame之間發生了相對運動，broadcaster就會發布相關消息。
TF的原理
TF樹的結構
TF庫的目的是實現系統中任一個點在所有坐標系之間的坐標變換。也就是說，只要給定一個坐標系下的一個點的坐標，就能獲得這個點在其他任意坐標系的坐標。
為了達到上述目的，就需要提供當前ROS系統的中任意兩個坐標系的位姿變換關系。
那么TF是用什么方式來描述與記錄任意兩個坐標系的位姿變換關系的呢？
這里存在一個問題。假設有n個坐標系，那么他們之間的組合關系有c（n，2）個。如果這樣窮舉個數會非常多，所以不會采用這個方法。
為了更合理、更高效地表示任意坐標系的變換關系。TF使用多層多叉樹的形式來描述這個這個ROS系統的坐標系，樹中的每一個節點都是一個坐標系。TF樹的特點是每個節點只要一個父節點，即采用每個坐標系都有一個父坐標系，可以有多個子坐標系的原則。
TF坐標系表示規范
每個坐標系都有一個父坐標系，可以有多個子坐標系。TF樹就是以父子坐標系的形式來組織的，最上面是父坐標系，往下是子坐標系。
在TF樹中具有父子關系的坐標系是相鄰的，用帶箭頭的線連接起來。在TF樹中用箭頭表示這種父子關系。
上圖所表示的TF樹中base_link坐標系是base_footprint的子坐標系，base_cover_link坐標系也是base_footprint的子坐標系。
描述規范：
- source、target frame是在進行坐標變換時的概念，source是坐標變換的源坐標系，target是目標坐標系。這個時候，這個變換代表的是坐標變換。
- parent、child frame是在描述坐標系變換時的概念，parent是原坐標系，child是變換后的坐標系，這個時候這個變換描述的是坐標系變換，也是child坐標系在parent坐標系下的描述。
- a frame到b frame的坐標系變換（frame transform），也表示了b frame在a frame的描述，也代表了把一個點在b frame里坐標變換成在a frame里坐標的坐標變換。
- 從parent到child的坐標系變換（frame transform）等同於把一個點從child坐標系向parent坐標系的坐標變換，等於child坐標系在parent frame坐標系的姿態描述。
TF樹的通信方式與TF樹的具體表示
TF樹的建立和維護是基於Topic通信機制的。
根據TF樹的原理，它是靠建立與維護每個父子坐標系的變換關系來維護整個系統的所有坐標系的變換關系的。每個parent 坐標系到child坐標系變換關系是靠被稱為broadcastor的發布器節點來持續發布的。
雖然是靠Topic通信機制發布的parent 坐標系到child坐標系的變換，但並不是讓每一對父子坐標系都發布一個話題，實際上發布的唯一個話題是/topic，該話題集合了所有發布的父子坐標系的變換關系。
也就是說TF機制並不是讓每一對父子坐標系都發布一個話題，而是將所有的父子坐標系都集合到到一個話題上，該話題的消息中傳遞的數據是所有父子坐標系的變換關系，是父子坐標系變換關系的一個大數組。
使用tf的tflisener就可以監聽從任意兩個坐標系的變換。前提是TF的樹上能把這兩個聯通。
TF樹的建立
在開始建立TF樹的時候需要指定第一個父坐標系（parent frame）作為最初的坐標系。比如機器人系統中的map坐標系。
在第一次發布一個從已有的parent frame到新的child frame的坐標系變換時，這棵樹就會添加一個樹枝，之后就是維護。
TF樹的建立和維護靠的是tf提供的tfbroadcastor類的sendtransform接口。
transformBroadcaster()類就是一個publisher,而sendTransform的作用是來封裝publish的函數。
TF樹的維護
在運行過程中要不斷更新已有的parent frame到已有的child frame的坐標系變換，從而保證最新的位姿轉換關系。
作為樹狀結構，要保證父子frame都有某個節點在持續地發布這兩個frame之間的位姿關系，才能使樹狀結構保持完整。只有每一個父子的frame的位姿關系能被正確的發布，才能保證任意兩個frame之間的連通。
TF樹的使用
一旦正常的建立一個TF樹，保證每個父子坐標系都能得到正常的維護，那么就可以利用TF提供的訂閱器，訂閱任意兩個坐標系的轉換關系。
如何根據TF樹得到任意坐標系的轉換關系？
如果想要獲得任意兩個坐標系的轉換關系，其實訂閱器是收取的/tf話題上的消息，該消息集合了所有發布的父子坐標系的變換關系。訂閱器接收的其實是當前時刻的整個TF樹，然后搜索這棵樹，根據不同的父子坐標系關系找到一條變換的路徑。這條變換路徑就能通過父子關系通路連接起所求的這兩個坐標系，從而通過不斷將該通路上的變換矩陣相乘得到最終的所求的這兩個坐標系的變換關系。
TF對發布器與訂閱器進行了封裝，使開發者通過TF的接口更加簡單地建立對TF樹中某些坐標系轉換關系的維護與訂閱。用tf的tflisener監聽某一個指定的從一個a frame到b frame的變換即可。
總結(引用網絡資源)：
基本原理是，tfbroadcastor的類里有個publisher，tflisener的類里有個subscriber，一個發布叫/tf的topic，一個訂閱這個topic，傳送的消息message里包含了每一對parent frameid和child frameid的信息。這個機制意味着，所有的tb會發布某一特定的parent到child的變換，而所有tl會收到所有的這些變換，然后tl利用一個tfbuffercore的數據結構維護一個完整的樹結構及其狀態。基於此，tl在使用這棵樹時，會用lookuptransform或waitfortransform來獲得任意坐標系之間的變換。
這樣即使只要是一個tflisener（即只監聽兩個坐標系的變換關系），就要跟所有tfbroadcastor建立連接，就要收取/tf上的整個TF樹，還要負責搜索這棵樹，找到一條變換的路徑，然后通過變換矩陣相乘得到兩個坐標系最終的變換關系。
TF的特點
優點：
- 各種數值計算的細節，你不用考慮，tf庫可以幫你
- 接口很簡潔，會廣播和監聽就行；
- 問題找的很准，那就是需要維護坐標系之間的關系，尤其是父子坐標系的關系
- 提供了很多工具程序
- 考慮了與時間相關的變換
- 支持tf-prefix，可以在多機器人上用。通過讓不同機器人使用不同的prefix，來區分機器人。如果只有一個機器人一般是使用
/
缺點：
- 樹的結構很簡單，但有時候很笨拙。對於同級的坐標系，就需要從下到上找共同先輩，然后從這個先輩再往下找，進而確定二者的關系。
- 每個訂閱器要想獲得某兩個坐標系的關系都要搜索同一顆樹，這樣的開銷太大，主要是網絡傳輸的負荷比較大。
- 很難滿足實時性的要求，這一點比較顯然。這也是為什么TF會將每個變換存10秒鍾的數據
- 雖然整體比較容易上手但是很多細節不易理解。比如，now（）和time（0）；比如，技術文檔里的一些術語名詞；比如，采用了機器人里的習慣，與飛行器，慣導，車輛里的習慣區別較大，使用時不能想當然。
TF消息：兩個frame之間的消息
每個父子坐標系之間都會有broadcaster節點來發布消息維系坐標之間的轉換關系。TransformStampde.msg就是/tf話題上消息。該消息格式用來表示兩個frame之間一小段tf變換的相對坐標關系的。
說明：
ROS中實際上是靠TF tree來表示整個系統的坐標系關系的，而非簡單地靠多個兩兩父子坐標系的轉換關系來描述的。這里的TransformStampde.msg消息的TF tree消息類型的片段即其中的一對父子坐標系位姿的描述方式，TF tree消息類型基於TransformStampde.msg消息，因此先介紹TransformStampde.msg。TransformStampde.msg本質上描述的是TF tree中一小段tf變換。
具體消息類型如下：
geometry_msgs/TransformStamped（可見該消息類型是屬於geometry_msgs程序包的，而非tf包）
std_mags/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/Transform transform
geometry_msgs/Vector3 translation
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation
float64 x
float64 y
flaot64 z
float64 w
消息解釋：
該消息表示的的是當前坐標系frame_id和它的子坐標系child_frame_id之間的轉換關系。具體的轉換位姿是由geometry_msgs/Transform消息類型來定義的，該消息類型用三維向量表示平移，用四元組表示旋轉。
TF消息：TF樹的消息類型
/tf話題表示的內容是整個機器人的tf樹，而非僅僅是某兩個坐標系的轉換關系，這樣的話，/tf話題是需要很多的節點來維護的，每一個節點維護兩個父子frame之間的關系。即一個/tf話題上面，可能會有很多個node向上面發送消息。
這樣就相當於TF tree是由很多的frame之間TF拼接而成。剛才說的TransformStampde.msg消息類型表示的是兩個frame之間TF關系，接下來要介紹真正在/tf話題上進行傳輸的TF tree的消息類型。
在tf2中的TF樹對應的消息類型是tf2_msgs/TFMessage.msg。可見該消息位於tf2_msgs程序包內。
tf2_msgs/TFMessage消息的具體格式：
geometry_msgs/TransformStamped[] transforms
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/Transform transform
geometry_msgs/Vector3 translation
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation
float64 x
float64 y
flaot64 z
float64 w
可以看出TF樹的消息類型實際上就是一個TransformStamped類型定義的可變長度數組。也就是說本質就是由很多個兩個frame之間的TF消息TransformStamped形成描述整個機器人的TF樹的消息類型tf2_msgs/TFMessage.msg。
TF在roscpp與rospy中的接口
無論是roscpp中還是rospy中都有TF庫，TF提供了很多有用的接口。這里只大體描述，具體使用的時候再參考具體的資料。
提供的結構種類：
- 數據類型的定義（類）：向量、點、四元數、3*3旋轉矩陣、位姿等
- 數據轉換：給出了旋轉矩陣、四元數、歐拉角、旋轉軸之間的轉換函數
- 關於點、向量、角度、四元數等的運算的函數
- TF類，封裝好了發布器與訂閱器接口。可以將坐標系轉換關系發布到
/tf話題上的一段transform上；也可以訂閱/tf話題，並且得到到從源坐標系到目標坐標系這兩個坐標系之間的轉換關系。
transformBroadcaster()類就是一個publisher,而sendTransform的作用是來封裝publish的函數。在實際的使用中，我們需要在某個Node中構建tf::TransformBroadcaster類，然后調用sendTransform(),將transform發布到/tf的一段transform上。
TransformListener類就是從/tf上接收的類。
TF程序包相關的命令行使用
-
用命令行顯示當前所有frame的方法：
rosrun tf tf_monitor #顯示當前坐標變換樹的信息，主要是名稱和實時的時間延時rostopic echo /tf #以TransformStamped消息類型的數組顯示所有父子frame的位姿轉換關系以上主要是數據顯示
-
根據當前的tf樹創建一個pdf圖：
$ rosrun tf view_frames這個工具首先訂閱
/tf，訂閱5秒鍾，根據這段時間接受到的tf信息，繪制成一張tf tree，然后創建成一個pdf圖。將會以圖形的形式顯示出TF樹中所有的frame和兩個frame 的父子關系及其Broadcaster、Average rate等
-
查看當前的tf樹：
$ rosrun rqt_tf_tree rqt_tf_tree該命令同樣是查詢tf tree的，但是與第一個命令的區別是該命令是動態的查詢當前的tf tree,當前的任何變化都能當即看到，例如何時斷開何時連接，捕捉到這些然后通過rqt插件顯示出來。
-
查看兩個frame之間的變換關系：
$ rosrun tf tf_echo[source_frame][target_frame]將會持續的顯示源坐標系和目標坐標系的位姿變換關系。
該指令可以查詢任意兩個frame的轉換關系。
參考資料
https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter8/8.3.html（基礎講義）