ROS 源碼學習 ---TF

TF是ROS核心庫之一，它記錄了所有frame的變換關係，方便使用者快速調用。TF使用起來非常簡單，核心函數就兩個：一個broadcast，把坐標變換關係發布出去；一個listener監聽所有的broadcast，建立好坐標變換樹方便調用。

然而，在我剛開始學的時候就隱隱感覺這裡有問題：為什麼每個node里要用到listener都要記錄一份tf tree，這樣不是浪費資源嗎？一個全局變換關係搞一個然後直接查不就好了嗎？

就在我閱讀amcl包的源碼時，我又發現了一個怪異的TF使用方式：

// Use a child class to get access to tf2::Buffer class inside of tf_struct TransformListenerWrapper : public tf::TransformListener{ inline tf2_ros::Buffer &getBuffer() {return tf2_buffer_;}};

這裡就完全讓我摸不著頭腦了：為什麼不直接用TF的Listener呢？而且怎麼又出現了一個tf2？

這些問題讓我非常好奇，於是我打算一查到底。但是，我在網上並沒有查到太多的資料，唯一有點啟發的是ROS官方tf2的Wiki。沒辦法，我只好硬著頭皮讀TF的源碼了。讀完後發現，這裡面還真是大有一番門道。

TF2

從wiki上可以看到，TF2是用來替換tf的，尤其是在Migration這一篇說到：tf已經被拋棄啦！

略讀了下major change，其中有幾點非常有意思：

多了個/tf_static，這個是幹啥的呢？

Action Based Query，查變換可以用action了嗎？

就像我想的那樣，不用多個listener來記錄了嗎？

源碼

帶著這些疑問，我找到了ros的源碼，其中跟tf tf2相關的有兩個包：ros/geometry跟ros/geometry2。將兩個包clone下來就可以看了，推薦使用vscode，導航起來非常方便。

先從geometry看起。同樣，我們先嘗試從類繼承圖看起，因為類是最上層的結構。然而，看了下類圖發現這個並沒有什麼用，因為繼承的結構太淺了。

好了，現在我們閉上眼睛想想，tf庫的核心是什麼？沒錯，不就是按時間序列排好的一系列矩陣嗎？我們只要找到核心的數據結構，看它是怎麼組合起來的就好了，其他各種函數都是用來維護或查找這個東西。按照這個思路，我從transform listener的構造函數查起，跳轉了好幾回（過程中發現geometry包是個花架子，核心全在geometry2里），終於找到了最核心的數據結構：class TransformStorage，在geometry2包中：

這個頭函數裡面聲明了一個TransformStorage類，包含了一個transform的基本單元：

這5個數據成員構成了最基本的結構。由於tf支持查不同時間的transform，下一步就是將這個類組合成一系列不同時間的了。再次從源碼裡面尋找，很容易找到了第二個類：TimeCache，在time_cache.h里聲明了一個list：

好了，現在都組合到了TimeCache里，我們再看看誰用到了TimeCache。利用全局搜索功能，不難找到，TimeCache只在BufferCore中用到了：

這個裡面用TimeCache的方法比較特殊，TimeCacheInterfacePtr是一個智能指針(就是引用計數為0後銷毀對象)，BufferCore僅僅記錄了一系列指針，TimeCache全是new出來的，放在heap里。這裡的static參數跟前面提到的/tf_static相關，之後再詳細剖析。

到這裡就很清楚了，核心數據都是BufferCore掌握的。而且從BufferCore的函數可以看到，核心的幾個tf函數都已經在BufferCore中實現了。

下面再找bufferCore被誰使用了。全局搜索可以發現，用到bufferCore的地方非常多，需要仔細篩選，看看到底在哪實例化了bufferCore。這回可就沒這麼順利了，排除掉test用到的，一般類都都只是函數中等待傳入一個bufferCore的引用，並沒有找到實例化的地方（除了一個地方，之後再說）。

還好，線索還沒有斷，我們找到了一個繼承bufferCore的類，buffer（geometry2/tf2_ros）。繼續找誰實例化了buffer，結果在一個不起眼的地方找到了：

這裡又跑到geometry包去了，class Transformer。下面的就順理成章了：TransformListener繼承了Transformer，於是有了Buffer，於是一步步往回走，TransformListener有了核心的數據。

維護

可算是把核心數據打通了。然而，只有核心數據是不行的，還得維護和查找。查找這裡就不說了，可以看到大部分工作已經在bufferCore裡面做好了，後面的基本都是調用而已。這裡我們就談談維護。

顯然，我們用tf的時候需要tf庫不斷訂閱 f主題，然後自動算好怎麼變換，所以這裡我們查關鍵詞 f就好了。可以看到在geometry2的transform listener里訂閱了這個主題（geometry的listener實例化了一個geometry2的，直接調用）：

跟著它的回調函數走，我們發現這個listener引用了一個Buffer類，然後調用的Buffer類的sendTransform，這個函數是從BufferCore繼承過來的，所以再去看BufferCore的sendTransform。sendTransform的核心功能就是檢查frame存不存在，存在就調用TimeCache的inserData記錄下transform，不存在就調用前面提到的AllocateFrame建立一個TimeCache。

到這裡我們就可以回答amcl包的那個問題了：

// Use a child class to get access to tf2::Buffer class inside of tf_struct TransformListenerWrapper : public tf::TransformListener{ inline tf2_ros::Buffer &getBuffer() {return tf2_buffer_;}};

這個wrapper繼承了一個listener，所以一般listener的功能都有了；除此之外還暴露了一個函數getBuffer，也就是跟正常的listener相比，暴露了更底層的buffer。搜索這個getBuffer可以看到，這個getBuffer只在runFromBag函數裡面使用到：

tf_pub.publish(msg); for (size_t ii=0; ii<tf_msg->transforms.size(); ++ii) { tf_->getBuffer().setTransform(tf_msg->transforms[ii], "rosbag_authority"); }

這種用法雖然奇怪，但可以將存在rosBag裡面的數據直接塞到listener裡面。

tf2改進

到這裡我們可以看到，雖然tf的核心代碼都在tf2中，但是我們並沒有用到tf2帶來的好處。下面我們就來看看tf2帶來好處的一系列源碼吧。

f_static

在我們從最基本的數據結構往上讀的時候可以看到static如影隨形，這個明顯跟tf2 migration里提到的 f_static相關。現在我們來好好捋一捋static這條線。

可以看到，在geometry2的tf2包中的time_cache.h不光聲明了一個TimeCache，還有一個TimeCacheInterface跟一個StaticCache。TimeCacheInterface是一個介面，用來統一TimeCache跟StaticCache實現多態；TimeCache就像我們之前說到的，是一系列TransformStorage的集合；StaticCache比較有意思，它只存了一個TransformStorage。

在bufferCore創建TimeCache的時候也有創建staticCache的代碼：

TimeCacheInterfacePtr BufferCore::allocateFrame(CompactFrameID cfid, bool is_static){ TimeCacheInterfacePtr frame_ptr = frames_[cfid]; if (is_static) { frames_[cfid] = TimeCacheInterfacePtr(new StaticCache()); } else { frames_[cfid] = TimeCacheInterfacePtr(new TimeCache(cache_time_)); } return frames_[cfid];}

同樣，普通的tf訂閱的主題是 f，在同樣的地方也訂閱了 f_static; 之後一路傳遞一個is_static的bool變數到bufferCore的sendTranform，然後調用上面說到的allocateFrame。所以，這個 f_static只在最底層的staticCache有區別。

結合wiki上對tf_static的介紹，再比較staticCache跟TimeCache的insertData的區別，tf_static的意思現在已經很清楚了：

staticCache只存了最新的一個變換，而且wiki要求一定要 using latched topics，也就是當訂閱者訂閱了但還沒建立起來的時候，發布者會保存最新一次的消息,這樣就不會錯過了；所以wiki上說「總是能查到變換」。

wiki上還說tf2提供了一個static_transform_publisher。在geometry2的tf2_ros里有兩個cpp：static_transform_broadcaster static_transform_broadcaster_program，可以看到，在static_transform_broadcaster里定義了一個發布者:

StaticTransformBroadcaster::StaticTransformBroadcaster(){ publisher_ = node_.advertise<tf2_msgs::TFMessage>("/tf_static", 100, true);};

最後一個參數true就代表使用latched topics。

在static_transform_broadcaster_program建立了一個node，根據參數往外發變換：

int main(int argc, char ** argv){ //Initialize ROS ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName); tf2_ros::StaticTransformBroadcaster broadcaster; geometry_msgs::TransformStamped msg;

所以，所謂的static_transform_publisher不過是一個簡單的node而已。

Action Based Query

這個feature解決了我剛看tf的疑惑：為什麼不能來一個全局listener然後各部分查呢？

同樣是在geometry2/tf2_ros里，有一個buffer_server類跟一個buffer_server_main：buffer_server就是提供action服務的了，buffer_server_main就是一個node，建立了一個全局listener，然後跑action服務：

int main(int argc, char** argv){ ros::init(argc, argv, "tf_buffer"); ros::NodeHandle nh; double buffer_size; nh.param("buffer_size", buffer_size, 120.0); // WIM: this works fine: tf2_ros::Buffer buffer_core(ros::Duration(buffer_size+0)); // WTF?? tf2_ros::TransformListener listener(buffer_core); tf2_ros::BufferServer buffer_server(buffer_core, "tf2_buffer_server", false); buffer_server.start(); // But you should probably read this instead: // http://www.informit.com/guides/content.aspx?g=cplusplus&seqNum=439 ros::spin();}

可以看到這裡listener的初始化需要一個buffer（這就是之前提到實例化bufferCore的地方），這是因為這裡的listener是tf2的，其實tf的listener也需要一個buffer，只不過tf直接在類里定義了，而tf2需要手動給。另外那個WTF是ros作者寫的...

server啟動了，可以使用BufferClient類來拿數據，可以看到bufferClient類的函數就是我們熟悉的listener的函數，所以這樣就實現了一套tf tree，分散式查詢的效果。看懂這些代碼需要搞清楚action的概念，不清楚的話可以看wiki。利用action的查找可以實現一個time out的功能，不用像以前那樣傻等一會兒再查了。

總結

對於tf庫，剛開始我的態度是能用就用，可是慢慢有了這些疑惑卻查不到資料，只好讀一讀tf的源碼了。

源碼這個東西有優點也有缺點，優點是包含了所有你想知道的東西，缺點當然是太多了，並不是專門給使用者看的。所以我的態度是盡量先查源碼的介紹，帶著問題讀，以核心數據結構、框架為先。

tf庫還是很好理解的，我們從最基本的數據結構一路追溯到listener，弄清楚了整體的框架，然後針對各個feature各個擊破，基本打通了tf的經脈。可以看到，tf庫的設計建立在ros通訊框架之上，簡化了很多工作；tf2的改進也是很給力的，不過好像目前原來的tf庫也沒什麼大問題，所以ros也沒強推tf2，tf2的wiki寫的也不是那麼清楚。

總之，這個tf庫還是挺好玩的。