=== 關於Costmap_2d Package ===

wiki page: http://wiki.ros.org/costmap_2d

=== 我遇到的問題是 obstacle layer的重新整理頻率太低 ===

 costmap_2d包下的所有類文件：http://docs.ros.org/hydro/api/costmap_2d/html/annotated.html
 其中，值得注意滴是 costmap_2d::ObservationBuffer 這個類，這個類會被 costmap_2d::ObstacleLayer 呼叫，obstacle_layer.cpp(Link of source code)
 其中，observation_keep_time和expected_update_rate應該是決定重新整理頻率的引數，其預設是0.0
 但實際nav_test.py執行時，這個引數是1.0, 我用rosparam set改成0.0了也沒實際效果。

 I'm trying to solve this issue....

=== 我學這個包的時候，儘量總結wiki page上的內容如下：===
所屬Stack: navigation
關於這個包在 ROS Navigation 框架中的位置，參見：ROS探索總結（十三）——導航與定位框架

Sammary:

   1. 實現2D cost map
   2. 輸入： sensor data
   3. 生成： 佔用格資料，inflates cost in a 2D costmap.
   4. 支援 基於map_server的一個costmap的初始化，
      支援 基於rolling window的costmaps,
      支援 基於引數的，對一個感測器topics的配置的訂閱。

wiki web page上有圖(圖片地址見下)，說明神馬是cost map.
 
圖片地址：http://wiki.ros.org/costmap_2d?action=AttachFile&do=get&target=costmap_rviz.png
   紅色cell是costmap 上的障礙，藍色是通過機器人半徑膨脹出的障礙，紅色多邊形是機器人

footprint即垂直投影。要不碰撞，footprint不能和紅色cell有交叉，並且地，機器人中心不能與藍色

cell有交叉。

   costmap_2d這個包提供了一個可以配置的結構。這個結構處理關於機器人在一個occupancy grid（佔

用網格）裡，應該導航到哪裡的資訊。costmap 使用了感測器資料和來自固態地圖的資訊，通過

costmap_2d::Costmap2DROS物件(Object)，來儲存和更新關於障礙物的資訊。

costmap_2d::Costmap2DROS物件，提供了一個purely 2D介面給它的使用者，這意味著queries about

obstacles can only be made in columns(列). 例如，一張桌子和一雙鞋在XY平面上的同一個地方，但

有不同的Z高度，這會讓在costmap_2d::Costmap2DROS物件中對應的cell有相同的cost值（譯者注：看來

，cost可翻譯成“佔用”）。這種設計就是為了便於planar spaces中的路徑規劃。（注：看來

costmap_2d::Costmap2DROS物件很重要）
 
   Hydro版本中，the underlying(已有的) methods used to write data to the costmap is fully

configurable. Each bit of functionality exists in a layer. 舉個例子：靜態地圖在第一層，

obstacles 是另一層。預設地，the obstacle layer maintains information three dimentionally.（

這裡用的是voxel_grid包，關於voxel_grid，它提供了一個關於efficient 3D voxel grid的實現。體素

或立體畫素(voxel)。The occupancy grid can support 3 different representations for the state

of a cell: marked, free, or unknown.）Maintaining 3D 障礙資料使得這些層可以更智慧地處理

marking and clearing.

   主要的介面就是costmap_2d::Costmap2DROS物件，它Maintain了很多有關ROS的功能。
   1. 包括，costmap_2d::LayeredCostmap類，用來keep track of each of the layers。
   2. 每個層是在Costmap2DROS中，用pluginlib來例項化，並加入到LayeredCostmap類的物件中（Each

layer is instantiated in the Costmap2DROS using pluginlib(http://wiki.ros.org/pluginlib)

and is added to the LayeredCostmap. ）這些層們可能獨立地被編譯，允許通過C++interface來對

costmap任意的改變。
   3. 還有一個costmap_2d::Costmap2D類，implements the basic data structure for storing and

accessing the two dimensional costmap. 實現了基礎的資料結構，用來儲存和讀取2D costmap的資料

結構。
   The main interface is costmap_2d::Costmap2DROS which maintains much of the ROS related

functionality. It contains a costmap_2d::LayeredCostmap which is used to keep track of each

of the layers. Each layer is instantiated in the Costmap2DROS using pluginlib and is added

to the LayeredCostmap. The layers themselves may be compiled individually, allowing

arbitrary changes to the costmap to be made through the C++ interface. The

costmap_2d::Costmap2D class implements the basic data structure for storing and accessing

the two dimensional costmap.
   （看到這裡我有點暈，大意就是costmap例項化了就成了Layer, 通過Costmap2DROS類來例項化，放在

了以LayeredCostmap類下的物件裡，由LayeredCostmap管理。）
   下面就講how the costmap updates the occupancy grid. 帶有去往不同layer工作機制介紹頁面的

連結。
 

【標記和清除機制】

   The costmap automatically subscribes to sensors topics over ROS and updates itself

accordingly. Each sensor is used to either mark (insert obstacle information into the

costmap), clear (remove obstacle information from the costmap), or both. A marking

operation is just an index into an array to change the cost of a cell. 但是，清除機制就包含

通過網格的raytracing（光線追蹤）.這個網格源自每個由感測器報告上來的觀察。如果一個3D資料結構

用來儲存obstacle 資訊，在put them into costmap時，我們要把它的每一列資料重新對映到2D。

【Occupied, Free, and Unkown空間】

   每個方格有255個值，8位
   每個方格在這種資料結構下，有三種狀態：free, occupied, unknown
   每個狀態都有其對應的cost value.
   有些列有相當數量的occupied cells,就被分配了一個 costmap_2d::LETHAL_OBSTACLE cost。
   有些列有相當數量的unknown cells,就被分配了一個 costmap_2d::NO_INFORMATION cost。
   有些列有相當數量的occupied cells，就被分配了一個 costmap_2d::FREE_SPACE cost。
   

【地圖更新】

   執行更新，At the rate specified by the update_frequency 引數。costmap的佔用資料結構

（occupancy structure）要執行更新。並且這個資料結構要對映到costmap,where 合適的cost value要

分配，按照occupied, free and unkown的閾值來。這個進行完了，每個障礙的膨脹就要在每個cell上進

行，在 costmap_2d::LETHAL_OBSTACLE cost 的指導下進行。按照使用者指定的半徑進行繁殖。具體要在

下面inflation process中介紹。

【tf座標變換】

   假設所有的變換都是在 global_frame引數和robot_base_frame指定的 座標系 之間變換。
   transfor_tolerance 引數設定了這些變換之間的最大延遲量（latency），沒有夠快就會被

navigation stack叫停機器人。

【Inflation膨脹】

  圖片：http://wiki.ros.org/costmap_2d?action=AttachFile&do=get&target=costmapspec.png
 
  定義了5個象徵符號for costmap values
    1. Lethal - 致命的
    2. Inscribed - 內切的
    3. Possibly circumscribed - 可能外切的
        the true value is influenced by both the inscribed_radius and inflation_radius parameters
    4. Freespace - 0=costvalue, 機器人可去的地方
    5. Unknown - No info
    6. 其他介於free 和 possibly circumscribed之間的。就看它的距離和使用者定義的decay函數了。

    這些定義背後的原理就是： 我們把有些東西留給了對路徑規劃器的實現方式上。是不是要考慮實際的
footprint, 仍然給他們了足夠的資訊讓他們拿來生成tracing out the footprint的cost.
    總結： 在ROS的導航中，costmap_2d這個包主要負責根據感測器的資訊建立和更新二維或三維的地圖。ROS的地圖（costmap）採用網格（grid）的形式，每個網格的值從0~255，分為三種狀態：佔用（有障礙物）、無用（空閒的）、未知。
    上圖共分為五個部分：（下面的紅色框圖是機器人的輪廓，旁邊的黑框是上圖的對映位置）
      （1） Lethal（致命的）:機器人的中心與該網格的中心重合，此時機器人必然與障礙物衝突。
      （2） Inscribed（內切）：網格的外切圓與機器人的輪廓內切，此時機器人也必然與障礙物衝突。
      （3） Possibly circumscribed（外切）：網格的外切圓與機器人的輪廓外切，此時機器人相當於靠在障礙物附近，所以不一定衝突。
      （4） Freespace（自由空間）：沒有障礙物的空間。
      （5） Unknown（未知）：未知的空間。

【地圖型別】

  生成costmap_2d::Costmap2DROS有2中方式，

• 第一種是：seed it with a user-generated static map，走到哪裡蔽障就是。
• 第二種：give it a width and height and to set the rolling_window parameter to be true。rolling_window parameter讓機器人處在costmap正中間，當機器人走太遠時，放下障礙物。這種一般用在計程計調整架構（odometric coordinate frame）中，這種架構裡，機器人只關心在一個local area裡的障礙物。這裡涉及到的rolling_window引數是用來設定在機器人移動過程中是否需要滾動視窗，以保持機器人處於中心位置。

【元件及API】

   本節介紹程式設計時具體如何建立上述的物件，引數配置說明，層定義等.

   1. 關於 Costmap2DROS 類

      這 costmap_2d::Costmap2DROS 物件是一個對 costmap_2d::Costmap2D 物件的Wrapper封裝器，把它的功能暴露成一個C++ ROS Wrapper . 它是在ROS的名稱空間裡（這裡我們在下面就以~<name>開頭）進行特殊的初始化。
      一個建立 costmap_2d::Costmap2DROS 物件的程式碼例子如下：
    -------------------Code:-----------------
       #include <tf/transform_listener.h>
       #include <costmap_2d/costmap_2d_ros.h>
       ...
       ...
       tf::TransformListener tf_lsnr(ros::Duration(10));
       costmap_2d::Costmap2DROS costmap_obj("my_costmap",tf);
     --------------------------------------------

     1.1 關於 ROS API

         關於老的引數的注意事項：
         很多引數都相對hydro版本以前的版本變化了，為了更大化引數配置的自由度。那些老引數的名稱空間還是會work, 沒有必要reconfigure你的robot. 但是，當costmap程式碼執行時，首先發生的是，引數們將會被從他們老的地方移除，然後放到the new locations with plugins properly added.

     1.2 Costmap2DROS 物件要收聽的主題：

         ~<name>/footprint (geometry_msgs/Polygon)
         Specification for the footprint of the robot. This replaces the previous parameter specification of the footprint.

     1.3 Costmap2DROS 物件要釋出的主題：

         ~<name>/grid (nav_msgs/OccupancyGrid)
                 The values in the costmap
         ~<name>/grid_updates (map_msgs/OccupancyGridUpdate)
                 The values of the updated area of the costmap
         ~<name>/voxel_grid (costmap_2d/VoxelGrid)
                 Optionally advertised when the underlying occupancy grid uses voxels and the user requests the voxel grid be published.

     1.4 **引數配置說明**

         costmap_2d::Costmap2DROS 物件是可以高度引數配置化的物件，引數涉及到的模組有：座標系、tf, rate節拍, 機器人的解析，and, 地圖管理。
         ==座標系和tf引數==
         ~<name>/global_frame (string, default: "/map")
            The global frame for the costmap to operate in.
            定義costmap操作時，所參考的全域性地圖，預設是/map，String型別

         ~<name>/robot_base_frame (string, default: "base_link")
            The name of the frame for the base link of the robot.
            機器人基礎座標系

         ~<name>/transform_tolerance (double, default: 0.2)
            Specifies the delay in transform (tf) data that is tolerable in seconds.
            定義了tf資料可以容忍（tolerable）延遲到達的時間.
This parameter serves as a safeguard to losing a link in the tf tree while still allowing an amount of latency the user is comfortable with to exist in the system. For example, a transform being 0.2 seconds out-of-date may be tolerable, but a transform being 8 seconds out of date is not. If the tf transform between the coordinate frames specified by the global_frame and robot_base_frame parameters is transform_tolerance seconds older than ros::Time::now(), then the navigation stack will stop the robot.
            如果一個tf變換超過了tolerance限定的時間，機器人將被停止。

         ==rate節拍引數==
         ~<name>/update_frequency (double, default: 5.0)
            The frequency in Hz for the map to be updated.
            地圖更新速度，Hz

         ~<name>/publish_frequency (double, default: 0.0)
            The frequency in Hz for the map to be publish display information.
            地圖釋出到顯示資訊的頻率

         == 地圖管理引數 ==
         ~<name>/rolling_window (bool, default: false)
            Whether or not to use a rolling window version of the costmap.
            是不是要用滾動視窗模式（機器人不動）來檢視地圖
            If the static_map parameter is set to true, this parameter must be set to false. 如果static_map引數設定為1，這個引數就要設定為0

         == 下面這些引數是可以被有些層重寫的，名字上講，就是the static map層 ==
         ~<name>/width (int, default: 10)
            The width of the map in meters.
         ~<name>/height (int, default: 10)
            The height of the map in meters.
         ~<name>/resolution (double, default: 0.05)
            The resolution of the map in meters/cell.

         ~<name>/origin_x (double, default: 0.0)
            The x origin of the map in the global frame in meters.
         ~<name>/origin_y (double, default: 0.0)
            The y origin of the map in the global frame in meters.

         "width," "height," 和"resolution" 設定設定costmap代價地圖長（米）、高（米）和解析度（米/格）。解析度可以設定的與靜態地圖不同，但是一般情況下兩者是相同的。

     **以上就是costmap引數配置說明**這些引數是存放在 local_costmap_params.yaml 檔案中的

     1.5 tf變換要求

         (value of global_frame parameter) → (value of robot_base_frame parameter)
         Usually provided by a node responsible for odometry or localization such as amcl(link).
         通常都是由負責odometry或者定位的node，比如amcl node，來提供這個tr 變換。

     1.6 C++ API

         For C++ level API documentation on the costmap_2d::Costmap2DROS class, please see the following page:Costmap2DROS C++ API

   2. 關於層 Layer 的 Specification 指派/定義

• Static Map Layer - The static map layer represents a largely unchanging portion of the costmap, like those generated by SLAM.
• Obstacle Map Layer - The obstacle layer tracks the obstacles as read by the sensor data. The ObstacleCostmapPlugin marks and raytraces obstacles in two dimensions, while theVoxelCostmapPlugin does so in three dimensions.
• Inflation Layer - The inflation layer is an optimization that adds new values around lethal obstacles (i.e. inflates the obstacles) in order to make the costmap represent the configuration space of the robot.
  
• Other Layers - Other layers can be implemented and used in the costmap via pluginlib. Any additional plugins are welcomed to be listed and linked to below.
  >> SocialCostmapPlugin
  >>
  >>
  

以上就是對wiki/costmap_2D官方頁面的不準確翻譯，純當學習。下面我想分享一個 古-月 的部落格《ROS探索總結（十九）--如何配置機器人的導航功能》中有關cost_map代價地圖配置的內容：

古月的部落格：代價地圖的配置 (local_costmap)& (global_costmap)

       導航功能包使用兩種代價地圖儲存周圍環境中的障礙資訊，一種用於全域性路徑規劃，一種用於本地路徑規劃和實時避障。兩種代價地圖需要使用一些共同和獨立的配置檔案：通用配置檔案，全域性規劃配置檔案，本地規劃配置檔案。以下將詳細講解這三種配置檔案：

（1）通用配置檔案（Common Configuration (local_costmap) &(global_costmap)）

      代價地圖用來儲存周圍環境的障礙資訊，其中需要註明地圖關注的機器人感測器訊息，以便於地圖資訊進行更行。針對兩種代價地圖通用的配置選項，建立名為costmap_common_params.yaml的配置檔案：

1. obstacle_range: 2.5  
2. raytrace_range: 3.0  
3. footprint: [[x0, y0], [x1, y1], ... [xn, yn]]  
4. #robot_radius: ir_of_robot  
5. inflation_radius: 0.55  
6. observation_sources: laser_scan_sensor point_cloud_sensor  
7. laser_scan_sensor: {sensor_frame: frame_name, data_type: LaserScan, topic: topic_name, marking: true, clearing: true}  
8. point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true}  

      詳細解析以上配置檔案的內容：

1. obstacle_range: 2.5  
2. raytrace_range: 3.0  

       這兩個引數用來設定代價地圖中障礙物的相關閾值。obstacle_range引數用來設定機器人檢測障礙物的最大範圍，設定為2.5意為在2.5米範圍內檢測到的障礙資訊，才會在地圖中進行更新。raytrace_range引數用來設定機器人檢測自由空間的最大範圍，設定為3.0意為在3米範圍內，機器人將根據感測器的資訊，清除範圍內的自由空間。

1. footprint: [[x0, y0], [x1, y1], ... [xn, yn]]  
2. #robot_radius: ir_of_robot  
3. inflation_radius: 0.55  

       這些引數用來設定機器人在二維地圖上的佔用面積，如果機器人外形是圓形，則需要設定機器人的外形半徑。所有引數以機器人的中心作為座標（0，0）點。inflation_radius引數是設定障礙物的膨脹引數，也就是機器人應該與障礙物保持的最小安全距離，這裡設定為0.55意為為機器人規劃的路徑應該與機器人保持0.55米以上的安全距離。

1. observation_sources: laser_scan_sensorpoint_cloud_sensor  

     observation_sources引數列出了代價地圖需要關注的所有感測器資訊，每一個感測器資訊都將在後邊列出詳細資訊。

1. laser_scan_sensor: {sensor_frame: frame_name, data_type:LaserScan, topic: topic_name, marking: true, clearing: true}  

       以鐳射雷達為例，sensor_frame標識感測器的參考系名稱，data_type表示鐳射資料或者點雲資料使用的訊息型別，topic_name表示感測器釋出的話題名稱，而marking和clearing引數用來表示是否需要使用感測器的實時資訊來新增或清楚代價地圖中的障礙物資訊。

（2）全域性規劃配置檔案（Global Configuration (global_costmap)）

        全域性規劃配置檔案用來儲存用於全域性代價地圖的配置引數，我們使用global_costmap_params.yaml來命名，內容如下：

1. global_costmap:  
2.  global_frame: /map  
3.  robot_base_frame: base_link  
4.  update_frequency: 5.0  
5.   static_map:true  

     global_frame引數用來表示全域性代價地圖需要在那個參考系下執行，這裡我們選擇了map這個參考系。robot_base_frame引數表示代價地圖可以參考的機器人本體的參考系。update_frequency引數絕地全域性地圖資訊更新的頻率，單位是Hz。static_map引數決定代價地圖是否需要根據map_server提供的地圖資訊進行初始化，如果你不需要使用已有的地圖或者map_server，最好將該引數設定為false。

（3）本地規劃配置檔案（Local Configuration (local_costmap)）

       本地規劃配置檔案用來儲存用於本地代價地圖的配置引數，命名為local_costmap_params.yaml，內容如下：

1. local_costmap:  
2.  global_frame: odom  
3.  robot_base_frame: base_link  
4.  update_frequency: 5.0  
5.  publish_frequency: 2.0  
6.   static_map:false  
7.  rolling_window: true  
8.   width: 6.0  
9.   height: 6.0  
10.   resolution:0.05  

      "global_frame", "robot_base_frame","update_frequency", 和 "static_map"引數的意義與全域性規劃配置檔案中的引數相同。publish_frequency設定代價地圖釋出視覺化資訊的頻率，單位是Hz。rolling_window引數是用來設定在機器人移動過程中是否需要滾動視窗，以保持機器人處於中心位置。"width," "height," 和"resolution" 設定設定代價地圖長（米）、高（米）和解析度（米/格）。解析度可以設定的與靜態地圖不同，但是一般情況下兩者是相同的。