分享程序员开发的那些事...
更新时间：2022-08-13 12:08:50
如果需要深入了解navigation，那costmap2D必不可少。本文章参考了多篇文章以及ros navigation官网最新的资料，并结合自身的理解完成。该软件作为栅格地图建立的函数包，其输入是现实传感器的数据，输出是2D栅格地图的costmap，该软件包还支持基于map_server的costmap初始化（调用map_server节点，以服务的方式读取地图数据），基于滚动窗口的costmap初始化，以及基于参数的订阅和传感器主题的配置。
可以这么说，无论是激光雷达还是如kinect 或xtion pro深度相机作为传感器跑出的2D或3D SLAM地图，都不能直接用于实际的导航，必须将地图转化为costmap(代价地图)。ROS的costmap通常采用grid(网格)形式。每个单元都可以分为三个状态：Occupied（有障碍，costmap_2d::LETHAL_OBSTACLE=254）、Free （ 无障碍，costmap_2d::FREE_SPACE=0）、Unknown Space（未知区域，costmap_2d::NO_INFORMATION=255）。
在下图中，红色cell代表的是代价地图中的障碍，蓝色cell代表的是通过机器人内切圆半径计算的障碍物膨胀，红色多边形代表的是机器人footprint。 为了使机器人不碰到障碍物，机器人的footprint绝对不允许与红色cell相交，机器人的中心绝对不允许与蓝色cell相交。
上图显示了五边形机器人的轮廓以及对应的划分。坐标系内可以看到我们将区域划分为五个部分，通过利用机器人中心所在的单元格值，判断机器人处于什么碰撞状态
机器人的中心（center cell）与该网格的中心重合，机器人肯定已经与障碍物发生了碰撞
网格中心位于机器人的内切轮廓内，机器人也肯定发生了碰撞
网格中心位于机器人的外切圆与内切圆轮廓之间，此时机器人相当于靠在障碍物附近，此时取决于机器人的方向。
网格中心位于机器人外切圆之外，属于没有障碍物的空间
当我们打开navigation后我们可以看到在move_base中需要全局代价地图（global_costmap）和局部代价地图(local_costmap)参与。因此在2D SLAM中我们存在有两种代价地图。
以turtlebot3为例，我们通过map_server提供的/map话题可以得到下面的已经建好的图模型
全局代价地图（global_costmap） 就是由map_server提供，并在此基础上融合了 障碍物层和膨胀层的参数（包括静态地图层 + 障碍物层 + 膨胀层），从而共同组成了global_costmap。
局部代价地图（local_costmap） 则是查看周围一定范围内的代价地图信息，下图（包括障碍物层 + 膨胀层）。
costmap_2D提供了一种2D代价地图的实现方案，通过传感器传入的信息，来构建2D或者3D的地图，而对于地图构建而言，认清地图的层次非常有必要。总共分为四层，分别是：1.静态地图层；2.障碍物地图层；3.膨胀层；4.以及这三层组合成的master map。
从Hydro开始，Costmap已经可配置每一层信息了。我们可以在costmap_2d::Costmap2DROS中使用接口，并在每一层中使用pluginlib实例化Costmap2DROS并将每一层添加到LayeredCostmap。
costmap_2d包提供了一种可配置框架来维护机器人在代价地图上应该如何导航的信息。 代价地图使用来自传感器的数据和来自静态地图中的信息，通过costmap_2d::Costmap2DROS来存储和更新现实世界中障碍物信息。costmap_2d::Costmap2DROS给用户提供了纯2D的接口，这意味着查询障碍只能在列上进行。
例如，在XY平面上位于同一位置的桌子和鞋，虽然在Z方向上有差异但是它们在costmap_2d::Costmap2DROS对象代价地图中对应的cell上拥有相同的代价值。 这种设计对平面空间进行路径规划是有帮助的。
costmap_2D提供的ROS化功能接口主要就是 costmap_2d::Costmap2DROS，它使用 costmap_2d::LayeredCostmap 来跟踪每一层。 每一层在 Costmap2DROS中以 插件方式被实例化，并被添加到 LayeredCostmap。 每一层可以独立编译，且可使用C++接口实现对代价地图的随意修改，即LayerdCostmap为Costmap2DROS（用户接口）提供了加载地图层的插件机制，每个插件（即地图层）都是Layer类型的。
costmap_2d::Costmap2D 类中实现了用来存储和访问2D代价地图的的基本数据结构。
costmap中各Layer之间的继承关系如图4所示，本文后面我们附的layeredcostmap的相关介绍还会用到这个图。
在navigation的主节点move_base中(costmap隶属于navigation包，或者说是navigation的一个子模块)，建立了两个costmap。其中planner_costmap_ros_是用于全局导航的地图，controller_costmap_ros_是用于局部导航用的地图。图5为costmap的初始化流程。
（1）Costmap初始化首先获得全局坐标系和机器人坐标系的转换
（2）加载各个Layer，例如StaticLayer，ObstacleLayer，InflationLayer。
（3）设置机器人的轮廓
（4）实例化了一个Costmap2DPublisher来发布可视化数据。
（5）通过一个movementCB函数不断检测机器人是否在运动
（6）开启动态参数配置服务，服务启动了更新map的线程。
简单整理了下costmap初始化过程中的各层加载的调用过程：
在move_base刚启动时就建立了两个costmap，而这两个costmap都加载了三个Layer插件，它们的初始化过程如上图所示。
StaticLayer主要为处理gmapping或者amcl等产生的静态地图。
ObstacLayer主要处理机器人移动过程中产生的障碍物信息。
InflationLayer主要处理机器人导航地图上的障碍物信息膨胀（让地图上的障碍物比实际障碍物的大小更大一些），尽可能使机器人更安全的移动。
costmap在mapUpdateLoop线程中执行更新地图的操作，每个层的工作流程如下：
上图是StaticLayer的工作流程，updateBounds阶段将更新的界限设置为整张地图，updateCosts阶段根据rolling参数（是否采用滚动窗口）设置的值，如果是，那静态地图会随着机器人移动而移动，则首先要获取静态地图坐标系到全局坐标系的转换，再更新静态地图层到master map里。