costmap_2d代码解析之一：Costmap2DROS类、layerdCostmap类、lLayer类和Costmap2D类、CostmapLayer类

ROS

• 关于costmap_2d在move_base.cpp中:

  //create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map
  planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
  planner_costmap_ros_->pause();//作用：使调用进程（线程）进入休眠状态（就是挂起）；直到接收到信号且信号函数成功返回 pause函数才会返回。返回值：始终返回-1.

  //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map
  controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
  controller_costmap_ros_->pause();

  // Start actively updating costmaps based on sensor data
  planner_costmap_ros_->start();
  controller_costmap_ros_->start();

  //advertise a service for clearing the costmaps
  clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

https://blog.csdn.net/Neo11111/article/details/104798065记录对Costmap2DROS类和Costmap2D类的阅读和理解。
https://blog.csdn.net/Neo11111/article/details/104844646记录对LayeredCostmap类和CostmapLayer类的阅读和理解。
Costmap2DROS类是对整个代价地图内容的封装，包含了整个流程。

LayeredCostmap类是Costmap2DROS的类成员，它是“主地图”，也能够管理各层地图，因为它含有指向各层子地图的指针，能够调用子地图的类方法，开启子地图的更新（看图2）。并且，各层子地图最后都会合并到主地图上，提供给规划器的使用。它含有Costmap2D类成员，这个类就是底层地图，用于记录地图数据。LayeredCostmap类的地图更新函数主要分为两步，先更新bound，再更新cost，它调用Layer类方法，即updateBounds和updateCosts在layer.h中被定义，之后在各层子地图中被重载。

由Layer类单独派生出InflationLayer类，即膨胀层，用它来反映障碍物在地图上向周边的膨胀。由于它的父类中不含Costmap2D类，所以其实膨胀层自身没有栅格地图要维护，这一点和另外两层有区别。

Costmap2D类是记录地图数据的底层，它记录地图的x、y方向的尺寸，地图的分辨率，地图原点位置，以及用unsigned char类型记录地图内容。

CostmapLayer类派生自Layer类和Costmap2D类。Layer类中含有子地图层用到的一些函数，如声明了onInitialize()、匹配主地图size的matchSize()、更新bound的updateBounds、更新cost的updateCosts、和主地图合并等；Costmap2D类存储该层维护的地图数据。CostmapLayer类作为静态层和障碍层的基类，提供了一些对地图层进行操作的函数。由CostmapLayer类派生出StaticLayer类和ObstacleLayer类，即静态层和障碍层，前者获取静态地图，后者通过传感器数据不断更新，获得能反映障碍物信息的子地图。ObstacleLayer类又派生出VoxelLayer类。

1.Costmap2DROS类
costmap_2d_publisher.h和cpp，其关联了layered_Costmap.h、layered.h（里面的initialize函数）、costmap_2d_publisher.h、footprint.h。

图2：

2.LayeredCostmap类
关联了layer.h、costmap_2d.h、cost_values.h和footprint.h。

//地图尺寸设置 LayeredCostmap::resizeMap
//这个函数在Costmap2DROS动态配置参数的回调函数ReconfigureCB中被调用，作用是在开启地图更新线程之前，调用Costmap2D的resizeMap函数，用给定参数重新设置主地图的尺寸、原点、分辨率，再通过plugin指针调用各层地图的matchSize，使其以上参数和主地图匹配。
void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,double origin_y, bool size_locked)



//这个函数在Costmap2DROS的地图更新线程中被循环调用。它分为两步：
//第一步：更新bound，即确定地图更新的范围；第二步：更新cost，更新每层地图cell对应的cost值后整合到主地图上。
//设置好minx_、miny_、maxx_、maxy_的初始值，
//然后对每一层的子地图调用其updateBounds函数，传入minx_、miny_、maxx_、maxy_，函数将新的bound填充进去。
//updateBounds函数在Layer类中声明，在各层地图中被重载，lay.h中声明updateBounds函数时指出This is called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to update. Each layer can increase the size of this bounds.
//第二步使用到的updateCosts函数也是如此，在Layer类中声明。这两个函数的具体内容在各层地图部分详述
void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)

LayeredCostmap类与Layer类、Costmap2D类的关系

3.Layer类
关联了costmap_2d.h、layered_costmap.h。
//updateBounds函数在Layer类中声明，在各层地图中被重载，lay.h中声明updateBounds函数时指出This is called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to update. Each layer can increase the size of this bounds.
//的updateCosts函数也是如此，在Layer类中声明。

virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,double* max_x, double* max_y) {}

virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) {}

bool isCurrent() const

virtual void matchSize() {}

4.Costmap2D类

关于operator=(const Costmap2D& map)看https://www.cnblogs.com/ZY-Dream/p/10068993.html，重载操作符

5.CostmapLayer类
这个类继承自Layer类和Costmap2D类，它是地图插件（静态层和障碍层）的基类。它的类方法主要用于处理bound和用几种不同的策略合并子地图和主地图。
class CostmapLayer : public Layer, public Costmap2D

matchSize函数用主地图的尺寸来设置该层地图的尺寸；

以下三个是关于边界的处理
addExtraBounds函数将传入的bound与数据成员的值比较，如果传入的bound范围更大，则更新数据成员的值；

useExtraBounds函数更新参数中指定的边界框，以包括来自addExtraBounds调用的边界框。 如果未调用addExtraBounds，则该方法将不执行任何操作。在调用addExtraBounds函数后使用，它将传入的bound与更新后的数据成员比较，将更大的范围通过传入的指针填充，并恢复数据成员初始值，认为将add的bound使用过了；

touch函数传入一个bound和一个坐标，若坐标不在bound范围内，它扩张bound，使其包含坐标；

以下四个是关于合并子地图和主地图时cell的数据如何合并
updateWithTrueOverwrite函数用当前子地图数据（包括未知cell）覆盖主地图对应区域；

updateWithOverwrite函数用当前子地图数据（不包括未知cell）覆盖主地图对应区域；

updateWithMax函数用当前子地图数据（不包括未知cell）更新主地图对应区域，若对应cell的cost值比主地图大或主地图该cell为未知时，用子地图数据覆盖，否则保留主地图原数据；

updateWithAddition函数用当前子地图数据（不包括未知cell）更新主地图对应区域，若主地图该cell为未知，用子地图数据覆盖；否则，在主地图原数据基础上+子地图数据（将进行限制避免cost值溢出，即If the sum value is larger than INSCRIBED_INFLATED_OBSTACLE, the master value is set to (INSCRIBED_INFLATED_OBSTACLE - 1).）