1 運行框架
（1）類體系
（2）Costmap2DROS運行框架
Cosmap2DROS主要作為一個地圖模塊存在，內部會啟動一個地圖更新循環，同時提供給外部管理地圖循環、獲取地圖信息的接口。
其主要接口如下：
void start()---啟動地圖運行，內部包括激活各層、啟動地圖更新循環。可以在stop或者pause調用之后用於重新啟動地圖
void stop()---停止地圖運行，內部包括去激活各層、停止地圖更新循環
void pause()---暫停地圖運行，內部包括停止地圖更新循環
void resum()---復位地圖運行，內部包括恢復地圖更新循環
void resetLayers()---重置地圖，內部包括重置總地圖、重置地圖各層
void isCurrent()---判斷地圖是否有效
bool getRobotPose(tf::Stamped<tf::Pose>& global_pose) const---獲取機器人在地圖global frame中的位姿
Costmap2D* getCostmap()---獲取全局地圖costmap2D
std::string getGlobalFrameID()---獲取地圖的global frame id
std::string getBaseFrameID()---獲取機器人坐標系base frame id
LayeredCostmap* getLayeredCostmap()---獲取地圖對象LayeredCostmap
geometry_msgs::Polygon getRobotFootprintPolygon()---獲取機器人邊界（在機器人坐標系下，包含padding）
std::vector<geometry_msgs::Point> getRobotFootprint()---獲取機器人邊界（在機器人坐標系下，包含padding）
std::vector<geometry_msgs::Point>getUnpaddedRobotFootprint()---獲取機器人邊界（在機器人坐標系下，不包含padding）
void getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const---獲取機器人邊界（在地圖全局坐標系下，包含padding）
void setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)---設置機器人邊界
void setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)---設置機器人邊界
在Costmap2DROS的構造函數中，加載參數，創建各層，做好初始化工作后，啟動動態配置。
在動態配置回調函數中會根據參數對地圖進行調整，另外還會重新啟動地圖更新線程mapUpdateLoop（因此，地圖更新線程在這里啟動）。
在地圖更新線程中，會先通過updateMap函數更新地圖，然后根據地圖更新的范圍邊界，通過Costmap2DPublisher發布更新的地圖信息。最后，會根據update_frequency，判斷該地圖更新循環的周期有沒有滿足要求，不滿足則給出警告日志。
updateMap函數是地圖信息更新的地方，其內部調用了LayeredCostmap->updateMap(double robot_x, double robot_y, double robot_yaw)函數。在該函數中，先依據各層的更新情況，判斷地圖更新過的范圍的邊界。然后用初始值重置全局地圖更新邊界范圍內的地圖信息，並用各層的信息在更新邊界內部更新地圖信息。
2 mark和clear
clear和mark操作的執行都在ObstacleLayer中的updateBounds函數中，且必須有點雲的存在才會觸發clear和mark。
（1）觀察緩沖區
首先, clear和mark操作存在於ObstacleLayer中，其實現是基於觀察緩沖區。在ObstacleLayer中存在三個緩沖區數組，用於存放三類緩沖區：觀察緩沖區、mark緩沖區、clear緩沖區。
其中，一個緩沖區（ObservationBuffer）對應一個觀察源，用於存放觀察源的數據。而ObservationBuffer可以認為是List<Observation>，而Observation可以認為是點雲數據。由於可以記錄一段時間內的觀察數據，所以OservationBuffer以列表的形式存儲觀察數據，但默認觀察時間（observation_keep_time）為0，因此其實ObservationBuffer只存最新的一組點雲（Observation）。
在配置文件中所列出的觀察源都會列入觀察緩沖區，但是mark緩沖區和clear緩沖區則根據源內參數決定是否加入該源。由於三類緩沖區存放的都是vector<shared_ptr>，因此內存是同一塊。
LaserScan、PointCloud、PointCloud2數據到來后會在回調函數里都轉為PointCloud2類型，然后由對應的ObservationBuffer存儲，Observation會存儲相對於costmap global坐標系的傳感器原點信息、點雲信息（包含時間戳、坐標系等信息）、obstacle_range、raytrace_range信息。
（2）clear操作
clear操作是在costmap global坐標系下的二維平面內，根據clear緩沖區中的各個Observation，將各個Observation的傳感器原點和點雲之間（用bresenham算法畫直線）的地圖信息設為FREE_SPACE（距離不超過raytrace_range）。
（3）mark操作
mark操作也比較簡單，依次獲取mark緩沖區的各個Observation，並計算Observation中點雲與傳感器原點之間的距離，如果距離不超過obstacle_range，則將點雲對應坐標的地圖信息設為LETHAL_OBSTACLE。
注：由於mark和clear使用的是同一塊Observation數據，因此有時出現邊界點標記但是清除不掉的現象，這估計就是邊界通過畫線的方式進行clear操作時會偶爾覆蓋不到一些像素。解決這個問題的思路是將clear的扇形范圍取的比mark的范圍大一些（針對LaserScan這種扇形數據而言），從而保證邊界的清除效果。由於mark和clear使用同一塊數據，因此比較難以單獨改變范圍。可以采用的一個思路是修改clear部分的代碼，在利用點雲畫直線清除時，加粗直線。另一種是，自己創建節點訂閱激光數據，並發布一個角度范圍小的LaserScan數據，同時在ObstacleLayer中增加一個源，然原始激光對應的源只clear，變換后的激光對應的源只mark。
3 地圖更新
地圖信息的更新並不是每次都更新整幅地圖，而是先根據各層的變化情況，計算出所要更新的邊界，然后用各層去更新該邊界內的區域。
（1）更新總流程
首先，在Costmap2DROS的動態配置回調函數中啟動了地圖更新循環，且地圖更新循環可以由start stop pause resume等函數控制，且地圖更新循環的頻率由updtae_frequency參數進行監視。
在地圖更新循環中，先通過updateMap()函數更新地圖信息，然后再由Costmap2DPublisher的publishCostmap()函數把地圖更新過的部分發布出去。
在updateMap()函數中，先獲取機器人當前位姿，然后調用了LayeredCostmap->updateMap(double robot_x, double robot_y, double robot_yaw)函數更新地圖，並在該函數的最后將機器人在地圖全局坐標系下邊界發布出去。
在LayeredCostmap->updateMap(double robot_x, double robot_y, double robot_yaw)函數中，如果地圖時rolling的，則先更新原點信息。然后，根據各層的更新情況確定地圖更新范圍的邊界。然后，將更新范圍內的地圖信息重置為缺省值。接着，調用各層的updateCosts函數用各層的信息去更新更新范圍內的地圖信息。
（2）各層更新邊界
邊界信息在LayeredCostmap->updateMap(double robot_x, double robot_y, double robot_yaw)創建，並傳給各層進行更新（調用各層的updateBounds）。
對於靜態層，如果是非rolling地圖，則看是否有地圖數據更新（初始化后會在接收到map話題后有一次更新），如果沒有則不更新邊界，否則，根據靜態層更新的區域的邊界更新傳入的邊界。
對於障礙物層，主要是根據mark和clear操作的范圍更新出入的地圖邊界信息（在該層的updateBounds函數中會進行clear和mark操作）。
對於膨脹層，基本是在傳入的地圖邊界信息的基礎上在擴大一個膨脹半徑更新地圖邊界信息。
（3）各層更新地圖
對於靜態層，其內部的自身更新主要是通過訂閱話題的信息進行的。由於一般是通過map_server提供靜態地圖信息，因此一般只會發布一次地圖信息，也就是說靜態一般只會更新一次，即初始化的時候。
另外，也可選擇定於map_topic_update形式的話題，從而動態更新靜態層（目前暫未使用）。
在updateCosts函數中，默認情況下，靜態層會用自己的信息覆蓋傳入的總地圖信息。另外，可以選擇最大值更新機制（通過use_maximum參數）。
對於障礙物層，其內部的自身更新主要是通過傳感器話題的回調函數緩存觀察源數據Observation，然后在updateBounds中根據觀察緩沖區執行mark和clear操作，從而實現地圖層的更新。
在updateCosts函數中，默認情況下，障礙物層會用自己的信息和傳入的總地圖信息進行比對，然后取最大值（NO_INFORMATION雖然為255，但不參與比較直接略過）。另外，可以選擇覆蓋更新機制（通過combination_method參數）。
對於膨脹層，只是對已有的地圖進行膨脹，因此其內部不會進行自身層更新。
在updateCosts函數中，該層對更新范圍內的地圖信息進行膨脹操作，即在LETHA_OBSTACLE信息的像素周圍進行膨脹。
4 存儲信息
NO_INFORMATION = 255
LETHAL_OBSTACLE = 254
INSCRIBED_INFLATED_OBSTACLE = 253
FREE_SPACE = 0
（1）缺省值
對於costmap來說，最內部的數據類型為Costmap2D，其內部通過unsigned char* costmap_來保存最基本的地圖數據。
首先總體層次結構為：Costmap2DROS內部保存的地圖相關對象為LayeredCostmap，而LayeredCostmap內部有一個Costmap2D用來保存總的地圖信息，另外它還包括了層信息std::vector<boost::shared_ptr<Layer>> plugins（層的類型可以是StaticLayer、ObstacleLayer、InflationLayer）。LayeredCostmap最終會將各層的信息匯總到其內部的Costmap2D中。
- LayeredCostmap中的Costmap2D在初始化構造時采用了默認構造函數，將地圖尺寸、分辨率、原點信息、地圖數據都設置為0或者NULL。然后，LayeredCostmap根據傳入進來的在Costmap2DROS中捕獲到track_unkown_space參數先給其內部的總地圖Costmap2D設置地圖數據缺省值NO_INFORMATION或者FREE_SPACE。
- StaticLayer繼承自Layer和Costmap2D（其它兩層相同），因此相當於通過Costmap2D保存地圖信息。StaticLayer在構造時調用默認構造函數，因此Costmap2D也通過默認構造函數進行了設置。在靜態層的onInitialize函數中，獲取到了在該層作用域中的track_unkown_space和unknown_cost_value兩個參數，這兩個參數不是用來設置Costmap2D中地圖數據的缺省值的，而是用來在接收到地圖數據后解析地圖中的未知區域信息（值等於unknown_cost_value）的，根據track_unknown_space的值不同，將訂閱到的地圖信息中的unknown_cost_value轉化為NO_INFORMATION或者FREE_SPACE。由於靜態層的數據會完全被訂閱的地圖信息賦值，因此缺省值對其來說不重要（保持默認構造為0）。
- ObtacleLayer的構造及初始化方式和StaticLayer類似。在該層的OnInitialize函數中，獲取到了在該層作用域中的track_unknown_space參數，然后根據該參數對其繼承自Costmap2D的default_value_參數賦值為NO_INFORMATION或者FREE_SPACE。
- InflationLayer的構造及初始化方法和StaticLayer類似。該層是基於已有地圖進行膨脹，因此不需要有缺省值。
（2）信息融合
地圖各層會有自己的更新機制，例如靜態地圖根據map話題更新自己的數據（也可以設置成只更新一次），動態地圖則根據激光數據更新地圖，膨脹層不會更新地圖，只是對已有地圖進行膨脹。在總地圖（即Costmap2DROS->LayeredCostmap->Costmap2D）更新時，是將重置過的總地圖依次傳入各層，利用各層的數據進行更新。默認情況下，靜態層會用自己的信息覆蓋傳入的總地圖信息；障礙物層會用自己的信息和傳入的總地圖信息進行比對，然后取最大值（NO_INFORMATION雖然為255，但不參與比較直接略過）；膨脹層則對傳入的總地圖信息執行膨脹操作。因此各層的放置順序不能隨意顛倒。
5 地圖有效性-current
對於各層，都有current_參數，該參數繼承自Layer類，且通過Layer::isCurrent查詢。總的地圖的current是通過各層current的與操作計算出來的。
對於靜態層，只要初始化（onInitialize）完成后current_就一直為true
對於障礙物層，其current_參數是各個觀察緩沖區（ObservationBuffer）是否current的與操作，而ObservationBuffer的current取決於緩沖區更新時間（新的觀測數據到來的時間）與expected_update_rate參數的對比。
對於膨脹層，也是只要初始化完成后curretn_就一直為true
6 地圖重置
在Costmap2DROS中，可以通過resetLayers函數對地圖進行重置。
在該函數中，首先將總的地圖信息重置為缺省值。然后調用各層的reset函數對各層進行重置。
對於靜態層，在reset函數中，會調用onInitialize函數重新進行初始化，但基本不進行特別的操作，只是將地圖更新標志位設為true，從而引起邊界的更新（最大地圖邊界），從而導致后面更新地圖時更新整個地圖。
對於障礙物層，在reset函數中，會先取消訂閱傳感器話題，然后復位地圖，然后在重新訂閱傳感器話題，從而保證整個層從新開始。
對於膨脹層，在reset函數中，會調用onInitialize函數進行重新初始化，但其實並沒有特別的地圖信息操作，只是復位一些標記。
版權聲明：本文為博主原創文章，轉載請注明出處：http://www.cnblogs.com/sakabatou/p/8297736.html