costmap( 代价地图)
costmap是Navigation Stack里的代价地图，它其实也是move_base插件，本质上是C++的动态链接库，用过catkin_make之后生成.so文件，然后move_base在启动时会通过动态加载的方式调用其中的函数。
costmap是一个package是机器人收集传感器信息建立和更新的二维或三维地图，用在move_base的global_map和local_map当中。用于将laser扫描数据或者点云数据转化成一个2d的网格地图。
ROS里的地图的概念，就是/map这个topic，它也是一张图片，一个像素代表了实际的一块面积，用灰度值来表示障碍物存在的可能性。然而在实际的导航任务中，光有一张地图是不够的，机器人需要能动态的把障碍物加入，或者清楚已经不存在的障碍物，有些时候还要在地图上标出危险区域，为路径规划提供更有用的信息。
因为导航的需要，所以出现了代价地图。你可以将代价地图理解为，在/map之上新加的另外几层地图，不仅包含了原始地图信息，还加入了其他辅助信息。
代价地图有一下特点：
１．首先，代价地图有两张，一张是local_costmap，一张是global_costmap，分别用于局部路径规划器和全局路径规划器，而这两个costmap都默认并且只能选择costmap_2d作为插件。
２. 无论是local_costmap还是global_costmap，都可以配置他们的Layer，可以选择多个层次。costmap的Layer包括以下几种：
-
Static Map Layer：静态地图层，通常都是SLAM建立完成的静态地图。
-
Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息。
-
Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。
-
Other Layers：你还可以通过插件的形式自己实现costmap，目前已有
Social Costmap Layer、Range Sensor Layer等开源插件。
地图中分成几部分
灰色部分：安全区域，robot运动在该区域不会发生碰撞
红色部分：危险区域，robot的footprint覆盖到该色网格就肯定会发生碰撞
蓝色部分：风险区域，robot在该区域有可能发生碰撞
红色几何型：robot在地图中所占的区域
2.两个操作
costmap订阅传感器发布的主题数据并更新自身地图，其中有两个操作
marking：标识该区域为障碍信息
clearing：清除该区域的障碍信息
3.网格的三种状态
每个网格会被赋予一个0～254之间的值，代表三种不同的状态
occupied：占有，unknow：未知，free：空闲。
4.地图更新和tf
costmap根据update frequency参数定期来更新地图。每个周期内，根据传感器信息mark/clear地图中的网格。
costmap更新构造地图，默认情况是，global_frame（/map）和 robot_base_frame(/base_link)和他们之间的frame都是连接的，并且定期更新的。如果tf没有按照定期的频率来更新，那navigation stack将会停止robot的运动。
5.网格的赋值
赋值范围为0～254
lethal：值为254，传感器扫描到的实际障碍转化成map中的lethal网格。
inscribed ：内部半径，如果网格和lethal网格距离小于等于该半径，则为inscribed网格，值为253。取robot几何型所能容纳最大的圆的半径。
被标记为253～254的网格是障碍网格，也就是lethal和inscribed网格是障碍网格。当robot的边界覆盖到这两种网格时，一定会发生碰撞。
possibly circumscribed：膨胀半径，以robot的中心为园点旋转一周所能覆盖的最大圆半径，如果所在网格距离lethal小于等于膨胀半径，大于内部半径，则为possibly circumscribed网格。值为128～252，取决与robot的运动方向和轨迹，还有算法。当robot的边界覆盖到这种网格时，可能会发生碰撞。
free：值为0,说明没有任何信息可以阻碍机器人运行到这里。
unkonw：没有任何信息，传感器还没有扫描到该区域，都是障碍网格
1～127：不会发生碰撞的区域，这里的值赋值取决于与lethal的距离，还有用户自定义的算法
6.地图类型
第一种：指定地图，地图里面有长，宽和障碍信息，通常也会有一个定位系统，例如amcl
第二种 : 没有指定地图，保持robot始终在地图的中心位置，随着robot的运动不断构建地图
7.API
订阅的主题：
~<name>/footprint (geometry_msgs/Polygon)：Specification for the footprint of the robot. This replaces the previous parameter specification of the footprint.
发布的主题
~<name>/grid (nav_msgs/OccupancyGrid)：The values in the costmap
~<name>/grid_updates (map_msgs/OccupancyGridUpdate)：The value 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.
参数
Plugins
8.需要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.
9.地图上的层规范
9.1 static map layer
静态地图层，通过slam算法生成的地图，不可改变，不包括动态或静态障碍
9.2 Obstacle Map Layer
障碍地图层，通过传感器数据识别出非固定地图的障碍，包括动态，静态障碍
9.3 Inflation Layer
膨胀层，根据inflation半径计算出的robot所占的可能最大面积，可选的，
9.4 other 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)。
---------------------
感谢作者sunyoop这么精彩的讲述，谢谢！
作者：sunyoop
原文：https://blog.csdn.net/sunyoop/article/details/78183145
参考链接：http://wiki.ros.org/costmap_2d