costmap程序架構
接上一期，通過閱讀indigo版本的costmap源碼包，簡單整理了下costmap的程序架構。
在move_base剛啓動時就建立了兩個costmap，而這兩個costmap都加載了三個Layer插件，它們的初始化過程如上圖所示。
StaticLayer主要爲處理gmapping或者amcl等產生的靜態地圖。
ObstacLayer主要處理機器人移動過程中產生的障礙物信息。
InflationLayer主要處理機器人導航地圖上的障礙物信息膨脹（讓地圖上的障礙物比實際障礙物的大小更大一些），儘可能使機器人更安全的移動。
costmap在mapUpdateLoop線程中執行更新地圖的操作，每個層的工作流程如下：
(1)StaticLayer工作流程
上圖是StaticLayer的工作流程，updateBounds階段將更新的界限設置爲整張地圖，updateCosts階段根據rolling參數（是否採用滾動窗口）設置的值，如果是，那靜態地圖會隨着機器人移動而移動，則首先要獲取靜態地圖座標系到全局座標系的轉換，再更新靜態地圖層到master map裏。
(2)ObstacleLayer工作流程
上圖是ObstacleLayer的工作流程，updateBounds階段將獲取傳感器傳來的障礙物信息經過處理後放入一個觀察隊列中，updateCosts階段則將障礙物的信息更新到master map。
(3)inflationLayer工作流程
上圖是inflationLayer的工作流程，updateBounds階段由於本層沒有維護的map，所以維持上一層地圖調用的Bounds值（處理區域）。updateCosts階段用了一個CellData結構存儲master map中每個grid點的信息，其中包括這個點的二維索引和這個點附近最近的障礙物的二維索引。改變每個障礙物CELL附近前後左右四個CELL的cost值，更新到master map就完成了障礙物的膨脹。