ROS Melodic中costmap2D详解

0 概述

如果需要深入了解navigation，那costmap2D必不可少。本文章参考了多篇文章以及ros navigation官网最新的资料，并结合自身的理解完成。该软件作为栅格地图建立的函数包，其输入是现实传感器的数据，输出是2D栅格地图的costmap，该软件包还支持基于map_server的costmap初始化（调用map_server节点，以服务的方式读取地图数据），基于滚动窗口的costmap初始化，以及基于参数的订阅和传感器主题的配置。

1 cost值

可以这么说，无论是激光雷达还是如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相交。

2 空间状态

上图显示了五边形机器人的轮廓以及对应的划分。坐标系内可以看到我们将区域划分为五个部分，通过利用机器人中心所在的单元格值，判断机器人处于什么碰撞状态

“Lethal”（254）（致命的）

机器人的中心（center cell）与该网格的中心重合，机器人肯定已经与障碍物发生了碰撞
“Inscribed”（253）（内切的）

网格中心位于机器人的内切轮廓内，机器人也肯定发生了碰撞
“Possibly circumscribed”（128-252）（可能受限）

网格中心位于机器人的外切圆与内切圆轮廓之间，此时机器人相当于靠在障碍物附近，此时取决于机器人的方向。
“Freespace”（0-127）（自由空间）

网格中心位于机器人外切圆之外，属于没有障碍物的空间
“Unknown” （未知的空间）

3 地图类型

当我们打开navigation后我们可以看到在move_base中需要全局代价地图（global_costmap）和局部代价地图(local_costmap)参与。因此在2D SLAM中我们存在有两种代价地图。

以turtlebot3为例，我们通过map_server提供的/map话题可以得到下面的已经建好的图模型

全局代价地图（global_costmap） 就是由map_server提供，并在此基础上融合了障碍物层和膨胀层的参数（包括静态地图层 + 障碍物层 + 膨胀层），从而共同组成了global_costmap。

局部代价地图（local_costmap） 则是查看周围一定范围内的代价地图信息，下图（包括障碍物层 + 膨胀层）。

4 地图层次

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的相关介绍还会用到这个图。

Costmap初始化流程

在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中各层的更新

简单整理了下costmap初始化过程中的各层加载的调用过程：

在move_base刚启动时就建立了两个costmap，而这两个costmap都加载了三个Layer插件，它们的初始化过程如上图所示。

StaticLayer主要为处理gmapping或者amcl等产生的静态地图。
ObstacLayer主要处理机器人移动过程中产生的障碍物信息。
InflationLayer主要处理机器人导航地图上的障碍物信息膨胀（让地图上的障碍物比实际障碍物的大小更大一些），尽可能使机器人更安全的移动。
costmap在mapUpdateLoop线程中执行更新地图的操作，每个层的工作流程如下：

静态地图层（StaticLayer）

上图是StaticLayer的工作流程，updateBounds阶段将更新的界限设置为整张地图，updateCosts阶段根据rolling参数（是否采用滚动窗口）设置的值，如果是，那静态地图会随着机器人移动而移动，则首先要获取静态地图坐标系到全局坐标系的转换，再更新静态地图层到master map里。
障碍物地图层（ObstacleLayer）

上图是ObstacleLayer的工作流程，updateBounds阶段将获取传感器传来的障碍物信息经过处理后放入一个观察队列中，updateCosts阶段则将障碍物的信息更新到master map。
膨胀层（inflationLayer）

上图是inflationLayer的工作流程，updateBounds阶段由于本层没有维护的map，所以维持上一层地图调用的Bounds值（处理区域）。updateCosts阶段用了一个CellData结构存储master map中每个grid点的信息，其中包括这个点的二维索引和这个点附近最近的障碍物的二维索引。改变每个障碍物CELL附近前后左右四个CELL的cost值，更新到master map就完成了障碍物的膨胀。

5 订阅主题

5.1 Costmap2DROS类

通过读这个类的重要的接口，我们发现Costmap2DROS主要是调用LayeredCostmap类干活。因此，我们可以看具体调用了LayeredCostmap的哪些功能，为下一步分析铺垫。
重要接口：

Costmap2DROS（）构造函数
检查是否存在robot base frame到 global的变换;创建一个LayeredCostmap类型的layered_costmap_;订阅与发布footprint主题，并更新机器人footprint;创建costmap2D发布者，发布地图。
Costmap2DROS类中发送地图信息

publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap);

start()函数
调用layered_costmap_，并激活每层。

stop()函数
调用layered_costmap_，并停止每层。

resetLayers()函数
调用layered_costmap_，并reset每层。

updateMap()函数
调用layered_costmap_->updateMap(x, y, yaw)以机器人所在中心更新地图，并发布footprint。

getBaseFrameID()函数
返回costmap的本地框架。

getCostmap ()函数
返回一个指向“主”costmap的指针，它接收来自所有层的更新。

getGlobalFrameID () 函数
返回costmap的全局帧。

getLayeredCostmap()函数
调用layered_costmap_->updateMap(x, y, yaw)以机器人所在中心更新地图，并发布footprint。

getOrientedFootprint()函数
在机器人的当前姿势构建机器人的Footprint信息。

getRobotFootprint()函数
以点向量的形式返回机器人的当前Footprint信息。

getRobotFootprintPolygon()函数
以几何图形形式返回当前填充的示意图。

getRobotPose()函数
在costmap的全局框架中获取机器人的姿势。

getTransformTolerance()函数
返回可容许的转换（tf）数据延迟（以秒为单位）

getUnpaddedRobotFootprint()函数
以点向量的形式返回机器人当前未添加的足迹。

mapUpdateLoop()函数
地图更新函数

movementCB()函数

5.2 LayeredCostmap基类

此类调用了costmap2d类、各层layer的功能。

LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown)构造函数
初始化坐标系、滚动窗、坐标范围

addPlugin（）函数
添加让多个layer层入栈。

updateMap（）函数
调用costmap2d类的功能：costmap_.updateOrigin(new_origin_x, new_origin_y)更新地图原点（当为rollingwindow时）;
调用layer：(*plugin)->updateBounds，更新每层范围。
调用costmap_.resetMap（）
调用(*plugin)->updateCosts更新花费

resizeMap（）函数
调用costmap2d类的功能：costmap_.resizeMap，重置地图大小、分辨率。
调用(*plugin)->matchSize()，使每层尺寸匹配。

getUpdatedBounds()函数
得到地图和layer的界限。左下角和右上角。

设置了circumscribed_radius_, inscribed_radius_

========================================================

5.3 layer虚基类

定义的接口：

layer（）构造函数
需要注意初始化了一个LayeredCostmap对象，以及一个tf

updateBounds（）虚函数

updateCosts（）虚函数

activate（）虚函数

deactivate（）虚函数

reset（）虚函数

=========================================================

5.4 costmap2D基类

主要负责坐标转换，保存costmap等。各种打杂，稍后看。

updateOrigin（）函数
当移动地图的原点时，不仅仅重新设置原点，还将两地图重合的部分，重新复制到新地图中应该在的地方去。其他未知的区域cost暂时设置为default。如下图所示。

5.5 CostmapLayer类

同时继承了layer和costmap2D类。主要定以了几种更新cost的方法。

protected：

updateWithTrueOverwrite（）函数
master[it] = costmap_[it]，将costmap的值复制到mastergrid中。

updateWithOverwrite（）函数
根上个函数代码一模一样，很神奇

updateWithAddition（）函数
master_array[it] = sum，以costmap的值加上master_array本身的值的和更新master_array。（大于254则254）

updateWithMax（）函数
当master_array[it]小于costmap_[it];master_array[it] = costmap_[it];用最大的更新master_array.

============================================================

5.6 StaticLayer类

用于处理事先建立好的静态地图。

onInitialize()函数
从ros参数服务器获取参数，
订阅map主题，调用incomingMap（）函数。
订阅map_updates主题，调用incomingUpdate（）函数。

incomingMap（）函数
根据收到的map信息，调用layered_costmap_->resizeMap（）调整layer尺寸。
然后调用下述语句，将收到的new_map的数据赋值到costmap_中：
unsigned char value = new_map->data[index];
costmap_[index] = interpretValue(value);

incomingUpdate（）函数
当地图更新时，调用
costmap_[index] = interpretValue(update->data[di++]);更新花费。

updateBounds（）函数
更新最大最小范围。

updateCosts（）函数
如果isrolling为假，则调用：
updateWithTrueOverwrite或者updateWithMax更新地图。
如果isrolling为真：
将global的点的cost复制到master_grid的map坐标系下对应点的cost中。

=========================================================

5.7 ObstacleLayer类(会用到5.8，5.9存储观测信息)

onInitialize()
读取ros参数服务器中的参数;
创建一个observation_buffers_，是一个vecotr，里面存储三个ObservationBuffer类型数据。实现为三类来源（LaserScan、PointCloud、PointCloud2）的存储初始化。以后数据就存储在这里啦。
要往observation_buffers_存储数据，用到了下面两个类，监听数据来源，并完成数据转换和存储：
1、分别建立三种类型数据的message_filters::Subscriber。
2、分别建立三种类型数据的tf2_ros::MessageFilter，并注册回调函数，回调函数负责数据转换和存储。

laserScanCallback（）回调函数
以sensor_msgs::LaserScan类型消息为输入，将laserscan数据转换为sensor_msgs::PointCloud2类型数据，并调用ObservationBuffer类的bufferCloud(cloud)方法，将该消息存储到一个链表（也就是ObservationBuffer类中的成员）中。

laserScanValidInfCallback（）回调函数
同上一个函数功能相同。但前面增加了一个步骤，滤去了inf无穷大数据。

pointCloudCallback（）回调函数
以PointCloud类型数据为输入，将该数据转换为ensor_msgs::PointCloud2类型数据并存储。

pointCloud2Callback（）回调函数
不需要转换直接进行存储。

updateBounds（）函数
更新origin，更新界（minx，miny，maxx，maxy）。
根据障碍所在坐标将costmap中对应栅格cost设为LETHAL_OBSTACLE。
并且更新footprint。

updateCosts（）函数
根据选择的方式combination_method_：
调用updateWithOverwrite（）或者updateWithMax()将本层的costmap写入到master_grid中。

getMarkingObservations（）函数
将marking_buffers_中的三个来源的点云格式数据，全部压栈到std::vector< Observation>中。

getClearingObservations（）函数
将clearing_buffers_中的三个来源的点云格式数据，全部压栈到std::vector< Observation>中。

raytraceFreespace（）函数
将clearing_observation中的点云数据不再地图中的投影到地图边界，并将该点云数据到传感系坐标原点连线的路径全部标记为freespace。

activate()函数
开始订阅传感器数据，并重置更新时间（ObservationBuffer类的更新时间，代表了里面数据是否新鲜）。

deactivate()函数
关闭订阅数据。

reset()函数
重新active一下。

===============================================================

5.8 Observation类

存储点云数据，只有构造函数，没有其他函数。
重要的数据类型：
sensor_msgs::PointCloud2* cloud_ 储存点云数组
obstacle_range_，raytrace_range_

5.9 ObservationBuffer类

std::list < Observation> observation_list_ 保存点云数据

setGlobalFrame（）函数
设置observation buffer的全局坐标系。遍历observation_list_中的每个Observation类型的数据，将其转换到new global frame中。

bufferCloud（）函数
将观测到的sensor_msgs::PointCloud2& cloud数据转换到全局坐标系globalframe中，并保存到observation_list_链表中。（包括观察原点和点云数据的转换，其他附加信息直接复制（如点云尺寸，时间戳））

purgeStaleObservations（）函数
清除陈旧的观测数据。
对observation_list_里面存储的数据的时间戳判断，清除过旧的数据。

getObservations（）函数
将observation_list_链表中的数据复制到一个vector< Observation >中。

===============================================

5.10inflation_layer类

InflationLayer()构造函数
除了成员初始化外，新建了一个递归互斥体inflation_access_

onInitialize()函数
锁住互斥体，动态重配一些参数。

computeCost（）函数
输入为到障碍物的栅格距离，输出为cost，函数为e的-x次方形式。

computeCaches()函数
计算了两张正方形表cached_costs_，cached_distances_
cached_distances_存的是，cached_distances_(i,j)到cached_distances_（0,0）的距离。cached_distances_存的是对应的cost。通过computeCost（）计算。

matchSize()函数
根据master的尺寸，计算自己的seen_size_=size_x * size_y，和seen_（seen_size_长度的bool数组。）并computeCaches()。

enqueue（）函数
输入为index，目标点，源点。
如果没有访问该目标点，则计算到源点距离，并将该点放到std::map<double, std::vector< CellData> > inflation_cells_;中。
这样就建立了一个距离与一系列点的映射。

updateBounds（）函数
最小值取上次输入和这次输入的最小，最大值取上次输入和这次输入的最大。并将此值（last_min_x_等）保存到此类中。

updateCosts（）函数
先找到障碍点，及cost=LETHAL_OBSTACLE的点，存入inflation_cells_映射。
然后从这些点依次向外扩张（上下左右四个点），调用enqueue（）将扩张点放入inflation_cells_映射，并对扩张点按照距离查找cost值，设置master的cost。完成聚合。