ROS 进阶学习笔记（16）：ROS导航1：关于Costmap_2d Package （代价地图包）

 === 关于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.
 
图片地址：https://i-blog.csdnimg.cn/blog_migrate/c125a0c3f6f7ea70ca038905d31d09fc.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）要执行更新。并且这个数据