ROS導航包源碼學習3 --- costmap_2d

amcl包研究完了，下面就是規划了，由於規劃的基礎是地圖，所以要先研究costmap_2d這個包。

跟amcl包相比，costmap_2d更加龐大，代碼行數達到9000行，比amcl包還多3000。光看costmap_2d的include目錄就讓人絕望了：頭文件都有17個。顯然，這個時候略讀都不太現實，我們需要一個更高效的方式來抓住costmap_2d的主幹。

類圖

這個時候我們就需要類繼承圖。以前光聽書上吹類的概念的好處，並沒有實際的體會，現在碰到這種大工程才懂。用doxygen+graphviz解析整個navigation stack，找到costmap_2d的繼承圖如下：

看到這個圖後我終於明白了整個costmap_2d的設計。以doxygen生成的文檔為大綱，先讀了下layer，發現這個類整個就是一個虛類，提供了最重要的兩個介面，updateCosts跟updateBounds，updateBounds計算打算更新的範圍，updateCosts就是更新cost了。介面這個概念說起來也沒啥，就是規定了下名字，實現了下多態嘛。但是，正因為有介面這個東西，costmap_2d才能實現插件化地圖，也就是繼承了layer的那幾個staicLayer / obstacleLayer / inflationLayer。插件化地圖的概念有一篇論文講的很清楚，在ros的注釋當中也推薦了，Layered Costmaps for Context-Sensitive Navigation，比較懶的看下論文的圖就行：)

所以，看上去介面讓代碼更多，其實理解起來變得更簡單，不管後面的類有多少函數，主要看updateCosts跟updateBounds這兩個就行。layer類中還使用到了LayeredCostmap，用來將各個層聚合在一起。LayeredCostmap類中主要的更新函數是updateMap，可以看到在udpateMap里利用到前面提到的多態，遍歷調用了所有層的兩個核心函數。不得不吐槽LayeredCostmap這個名字起的一點都不好，這哪裡看得出來是用來聚合的。

基類中還有一個costmap2D，這個類讀下來感覺沒啥核心功能，就換算下坐標啊，找多邊形框起來了哪些點啊。可以認為是一個打雜的類，現在暫時不用管，因為我們的目標是找到最有價值的部分。唯一值得注意的是costmap2D裡面有個unsigned char* costmap_;這個應該就是之後繼承了costmap2D的各種層的實體了。

然後就是繼承了layer跟costmap2D的costmapLayer了。costmaplayer其實就定義了幾個更新cost的方法，比如不同層同一點的cost是覆蓋 相加 還是選最大。不得不再吐槽一句這個名字起的也不好，一個類的名字至少要能看出來這個類打算幹啥吧。

其他

這樣一來整個costmap_2d的整體框架就清楚了，對照著Include目錄，還有這麼幾個類沒提到：

observation observationBuffer，就是記錄點雲的，一次觀測得到一堆點雲，存到observation類的pcl::PointCloud<pcl::PointXYZ>* cloud_里，多次觀測得到的一系列點雲存到observationBuffer的std::list<Observation> observation_list_里。

costmap2dROS，封裝整個功能，對外提供簡潔的介面。這個名字起的很好，在所有的包里，帶ROS後綴的都是封裝功能用的，可以理解為加精。

OK，現在各個類的功能都清楚了，下一步就可以往自己的方向深挖了。

top down

costmap_2d這個包更偏向工程，不像amcl包有一個核心的演算法。所以，在清楚了整個大框架的基礎上，研究costmap_2d最好抓住它是如何使用的，一步一步往下走，搞清楚整個的流程以便以後改寫。costmap2dROS封裝好了全部的功能，從costmap2dROS看是最有效率的。

costmap2dROS的頭文件還是很長的，我們要抓住重點，先排除掉不用看的：

private不用看，因為肯定是被調用的；含有get的函數不用看，因為就是讀取下變數的；含有footprint不用看，這是處理底座的。剩下來的函數有：start stop pause resume updatemap resetlayers.

start stop pause resume可以看到僅僅在調用layeredCostmap激活每層；updatemap也是調用layeredCostmap更新地圖，resetlayers還是在調用layeredCostmap。可見主要的工作都是layeredCostmap這個包工頭在干，layeredCostmap拿到命令後叫手下各層小弟幹活，然後合起來發給costmap2dROS。

稍微讀下layeredCostmap，可以看到它包工頭的氣質展現地淋漓盡致：聲明一個插件集合std::vector<boost::shared_ptr<Layer> > plugins_;在每個函數裡面都是遍歷這個插件集合然後調用每一層的具體函數。這樣插件化的設計讓地圖的可擴展性很強。這樣一來從上到下的調用結構就打通了，我們可以開始看真正幹活的幾個類了。

layers

staticLayer處理的是事先建好的地圖。除了所有Layer都有的updateBounds updateCosts外，staticLayer還有兩個獨有的incomingMap incomingUpdate，也就是接收到地圖就更新，這裡更新的是staticlayer的costmap，之後調用的updateCosts才真正反應到結果上。

obstacleLayer稍微複雜點，因為它需要根據雷達信息實時更新。obstacleLayer用了4個函數來處理不同的信息類型：laserScanCallback laserScanValidInfCallback pointCloudCallback pointCloud2Callback，這些函數在初始化的時候跟掃描數據直接綁定，所以一拿到數據地圖就可以更新，等到updateCost的時候反映到結果上。所有的信息都會被處理變成ObservationBuffer類型，也就是一系列三維點云：std::list<Observation> observation_list_;obstacle之後的所有計算都是基於這個類型的。

obstacleLayer裡面利用這個ObservationBuffer的方法有點繞。首先，它定義了三個ObservationBuffer的集合：

std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_; ///< @brief Used to store observations from various sensorsstd::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_; ///< @brief Used to store observation buffers used for marking obstaclesstd::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_; ///< @brief Used to store observation buffers used for clearing obstacles

定義成集合的目的是為了使用不同感測器的數據，但其實初始化只放了一個。marking_buffers_是為了記錄標記為障礙的點雲，但在初始化的時候是直接指向了所有的觀測數據observation_buffers_。既然這樣為啥要這麼麻煩？這是因為在obstacleLayer還定義了個直接插入數據的函數用來測試，所以需要另起爐灶來個marking_buffers_。

clearing_buffers_是為了將小車到障礙物之間的距離都標記為可行，這個也是直接指向觀測數據，也有用來測試的插入數據的函數。在updateBounds裡面有兩個循環來處理這兩個buffer，更新obstacleLayer內部的costmap。同樣，這個costmap更新後不是直接反映到結果，而是在updateCost裡面再讀出來。

inflationLayer就很好理解了，它的主要部分都在udpateCosts里。比較有意思的是inflationLayer存了兩個緩存，根據離圓心的距離存了一個cost，一個dist，一格格往外膨脹的時候直接查就行了。實現一格格膨脹的效果用到的方法跟之前amcl包建立地圖的occ_dist屬性一模一樣。

voxelLayer是對付三維情況的，這裡沒用到，就不說了。整個看下來這個costmap_2d設計的還是挺不錯的，這種介面化然後包工頭的設計思路值得學習。