Costmap(代價地圖)（上）
Costmap是機器人收集傳感器信息建立和更新的二維或三維地圖，可以從下圖簡要了解。
上圖中，紅色部分代表costmap中的障礙物，藍色部分表示通過機器人內切圓半徑膨脹出的障礙（注意是機器人內切圓形狀），紅色多邊形是footprint（腳印）(機器人輪廓的垂直投影)。
機器人避障的規則：爲了避免碰撞，footprint不應該和紅色部分有交叉，機器人中心不應該與藍色部分有交叉。
代價地圖結構形式：
ROS的代價地圖（costmap）採用網格（grid）形式，每個網格的值（cell cost）從0~255。分成三種狀態：被佔用（有障礙）、自由區域（無障礙）、未知區域；
具體狀態和值對應有下圖：
上圖可分爲五部分，其中紅色多邊形區域爲機器人的輪廓：
（1） Lethal（致命的）:機器人的中心與該網格的中心重合，此時機器人必然與障礙物衝突。
（2） Inscribed（內切）：網格的外切圓與機器人的輪廓內切，此時機器人也必然與障礙物衝突。
（3）Possibly circumscribed（外切）：網格的外切圓與機器人的輪廓外切，此時機器人相當於靠在障礙物附近，所以不一定衝突。
（4） Freespace（自由空間）：沒有障礙物的空間。
（5） Unknown（未知）：未知的空間。
Code處理邏輯：
ROS中costmap_2d這個包提供了一個可以配置的結構維護costmap,其中Costmap通過costmap_2d::Costmap2DROS對象利用傳感器數據和靜態地圖中的信息來存儲和更新現實世界中障礙物的信息。costmap_2d::Costmap2DROS爲用戶提供了純粹的2維索引，這樣可以只通過columns查詢障礙物。舉個例子來說，一個桌子和一雙鞋子在xy平面的相同位置，有不同的Z座標，在costm_2d::Costmap2DROS目標對應的的costmap中，具有相同的cost值。這旨在幫助規劃平面空間。
Costmap由多層組成，例如在costmap_2d包中，1.StaticLayer（靜態地圖層）是第一層， 2.ObstacleLayer（障礙物層）是第二層，3.InflationLayer（膨脹層）是第三層。 這三層組合成了master map（最終的costmap），供給路線規劃模塊使用。
最終輸出mast map給path planing 模塊使用。
Costmap主接口是costmap_2d::Costmap2DROS，它維持了Costmap在ROS中大多數的相關的功能。它用所包含的costmap_2d::LayeredCostmap類來跟蹤每一個層。每層使用pluginlib（ROS插件機制）來實例化並添加到LayeredCostmap類的對象中。各個層可以被獨立的編譯，且允許使用C++接口對costmap做出任意的改變。
Costmap ROS接口：
ROS對costmap進行了複雜的封裝，提供給用戶的主要接口是Costmap2DROS,而真正的地圖信息是儲存在各個Layer中。下圖可以簡要說明Costmap的各種接口的關係：
Costmap的ObstacleLayer和StaticLayer都繼承於CostmapLayer和Costmap2D,因爲它們都有自己的地圖，Costmap2D爲它們提供存儲地圖的父類，CostmapLayer爲它們提供一些對地圖的操作方法。而inflationLayer因爲沒有維護真正的地圖所以只和CostmapLayer一起繼承於Layer，Layer提供了操作master map的途徑。
LayerdCostmap爲Costmap2DROS（用戶接口）提供了加載地圖層的插件機制，每個插件（即地圖層）都是Layer類型的。
Costmap初始化流程：
在navigation的主節點move_base中，建立了兩個costmap。其中planner_costmap_ros_是用於全局導航的地圖，controller_costmap_ros_是用於局部導航用的地圖。下圖爲costmap的初始化流程。
（1）Costmap初始化首先獲得全局座標系和機器人座標系的轉換（世界座標系和機器人座標系相對變化關係）
（2）加載各個Layer，例如StaticLayer，ObstacleLayer，InflationLayer。
（3）設置機器人的輪廓
（4）實例化了一個Costmap2DPublisher來發布可視化數據。
（5）通過一個movementCB函數不斷檢測機器人是否在運動
（6）開啓動態參數配置服務，服務啓動了更新map的線程。
Costmap更新
Costmap的更新在mapUpdateLoop線程中實現，此線程分爲兩個階段：
（階段一）UpdateBounds：這個階段會更新每個Layer的更新區域，這樣在每個運行週期內減少了數據拷貝的操作時間。StaticLayer的Static map只在第一次做更新，Bounds 範圍是整張Map的大小，而且在UpdateBounds過程中沒有對Static Map層的數據做過任何的更新。ObstacleLayer在這個階段主要的操作是更新ObstaclesMap層的數據，然後更新Bounds。InflationLayer則保持上一次的Bounds。
（階段二）UpdateCosts：這個階段將各層數據逐一拷貝到Master Map，可以通過下圖觀察Master Map的生成流程。（圖來源於David Lu的《Layered Costmaps for Context-Sensitive Navigation》）
在（a）中，初始有三個Layer和Master costmap,Static Layer和Obstacles Layer維護它們自己的柵格地圖，而inflation Layer並沒有。爲了更新costmap,算法首先在各層上調用自己的UpdateBounds方法（b）。爲了決定新的bounds,Obstacles Layer利用新的傳感器數據更新它的costmap。然後每個層輪流用UpdateCosts方法更新Master costmap的某個區域,從Static Layer開始（c），然後是Obstacles Layer(d)，最後是inflation Layer(e)。
整體思路是如上算法過程還要自己閱讀工程。
下一篇系列文章中繼續研究costmap在ROS中的程序架構設計。 --------------------- 本文來自 月黑風高雲遊詩人 的CSDN 博客 ，全文地址請點擊：https://blog.csdn.net/lqygame/article/details/71174342?utm_source=copy