costmap_2d代码解析之三：体素层VoxelLayer和膨胀层InflationLayer插件

ROS

https://blog.csdn.net/jinking01/article/details/79447495
https://blog.csdn.net/jinking01/article/details/79455962

一、体素层VoxelLayer
关联的还有voxel_grid.h和cpp
对于VoxelLayer::updateBounds 更新过程和 ObstacleLayer::updateBounds 基本一致，只是增加了z 作为判断是否将2d地图的点设定为LETHAL_OBSTACLE
由于依赖按位与和或的整数运算的底层实现，体素网格仅支持每个体素列16个不同的级别。

1.void VoxelLayer::onInitialize()
costmap_2d::VoxelGrid????????????
clearing_endpoints??????????????

2.void VoxelLayer::setupDynamicReconfigure(ros::NodeHandle& nh)
调用VoxelLayer::reconfigureCB

3.void VoxelLayer::reconfigureCB(costmap_2d::VoxelPluginConfig &config, uint32_t level)
z_voxels？？？？？？？？？？？
unknown_threshold_ = config.unknown_threshold + (VOXEL_BITS - size_z_);？？？？？
VOXEL_BITS=16

if(size_z_ > 16){
      ROS_INFO("Error, this implementation can only support up to 16 z values (%d)", size_z); ？？？

调用matchSize()

4.void VoxelLayer::matchSize()
调用 void VoxelGrid::resize(unsigned int size_x, unsigned int size_y, unsigned int size_z),其中该函数中的uint32_t unknown_col = ~((uint32_t)0)>>16;？？？？？？？？？？
ROS_ASSERT(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);???????

5.void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
mark the cell in the voxel grid and check if we should also mark it in the costmap;
增加了z 作为判断是否将2d地图的点设定为LETHAL_OBSTACLE，避免了地面上有点还标志该位置为致命的，而是考虑z，看看高出地面多少？obstacle_layer避免该情况发生可以设置min_obstacle_height大于0的一个值？这样理解对吗？

二、膨胀层InflationLayer

1.void InflationLayer::onInitialize()
最重要的工作是调用了本类内的matchSize函数。由于InflationLayer没有继承Costmap2D，所以它和静态地图与障碍地图这两层不同，它没有属于自己的栅格地图要维护，所以matchSize函数自然不需要根据主地图的参数来调节本层地图。

matchSize函数函数先获取主地图的分辨率，接着调用cellDistance函数，这个函数可以把global系以米为单位的长度转换成以cell为单位的距离，故获得了地图上的膨胀参数cell_inflation_radius_。

接下来调用computeCaches函数，完成两个“参考矩阵”的填充，内容后述。

最后根据主地图大小创建seen_数组，它用于标记cell是否已经过计算。

2.void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y)
更新bound这里和其他两层的主要区别是，膨胀层在传入的bound的值的基础上，通过inflation_radius_再次扩张。

在C/C++11中，std::numeric_limits为模板类，在库编译平台提供基础算术类型的极值等属性信息，取代传统C语言，所采用的预处理常数。比较常用的使用是对于给定的基础类型用来判断在当前系统上的最大值、最小值。若使用此类，需包含头文件。https://blog.csdn.net/fengbingchun/article/details/77922558

3.void InflationLayer::onFootprintChanged()
InflationLayer类继承Layer类，该函数是Layer类中最先声明，每当足迹变化时，LayeredCostmap都会调用此方法（通过LayeredCostmap :: setFootprint（））。 覆盖以通知机器人的占地面积变化。

4.void InflationLayer::computeCaches()
//这里称cached_distances_和cached_costs_为参考矩阵，因为它们是后续膨胀计算的参照物。
////成本和代价的数组“行”大小都是cell_inflation_radius_+2

 cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2];
    cached_distances_ = new double*[cell_inflation_radius_ + 2];



for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
    { ////成本和代价的数组“列”大小也是cell_inflation_radius_+2
      cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
      cached_distances_[i] = new double[cell_inflation_radius_ + 2];
      for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
      {
        ////每个元素的值即为它到[0][0]点的三角距离。[0][0]点即障碍物所在点
        cached_distances_[i][j] = hypot(i, j);
      }
    }
    //设置cached_cell_inflation_radius_，所以第二次再进入的时候就不再执行这个if结构
    cached_cell_inflation_radius_ = cell_inflation_radius_;

调用了computeCost(cached_distances_[i][j])：，在（~.h文件中定义
若距离为0，即(0,0)点，则设置cached_costs_上的值为LETHAL_OBSTACLE(254)，表示为障碍物本身；
若距离（转换到global系后）≤ 机器人足迹内切圆半径，设置值为INSCRIBED_INFLATED_OBSTACLE（253），即由于机器人有体积造成的障碍物膨胀；
若机器人足迹内切圆半径 < 距离（转换到global系后）≤ cell_inflation_radius_，则以距离远近为比例（指数型）设置值。
所以，最终cached_costs_上，原点值为254，原点到机器人足迹内切圆半径范围内为253，其余按比例下降（指数型，近处骤降），cell_inflation_radius_（膨胀参数）以外没有值。

5.updateBounds和updateCosts函数参考https://blog.csdn.net/Neo11111/article/details/104869048或代码标注
更新bound这里和其他两层的主要区别是，膨胀层在传入的bound的值的基础上，通过inflation_radius_再次扩张。