参考：【ROS-Navigation】Costmap2D代价地图源码解读-膨胀层InflationLayer[1]
- inflation_radius_。膨胀半径，单位米，示例：0.55。值来自动态配置参数inflation_radius。
- 膨胀cost。膨胀层按自已规则计算出的cost，这cost值语义和costmap一致，但不会出现NO_INFORMATION(255)，以及肯定>FREE_SPACE(0)。
InflationLayer没有自身要维护的栅格地图，直接在主地图上进行操作。它首先根据膨胀参数预计算出两张查找表。updateCosts时，进行N轮迭代。第一轮时，在主地图上设置这些障碍栅格的cost，并以障碍栅格中心，上、下、左、右传染出4个新栅格。第二轮，设置4个新栅格cost，并传染出16个栅格。依此不断迭代，直到传染完整个膨胀区域。等效于完成一个由“将机器人视为一个点”到“考虑机器人本身的足迹”的转变过程，防止因为忽视了足迹而碰上障碍。至于两张查找表作用，cached_distances_用于传染出新栅格时，快速判断该栅格是否处于膨胀区域；cached_costs_则用于快速得知某个栅格的膨胀cost。
一、初始化 InflationLayer::onInitialize
这个函数没什么特别的，最重要的工作是调用了类内的matchSize函数。
void InflationLayer::matchSize()
{
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
resolution_ = costmap->getResolution();
cell_inflation_radius_ = cellDistance(inflation_radius_);
computeCaches();
unsigned int size_x = costmap->getSizeInCellsX(), size_y = costmap->getSizeInCellsY();
if (seen_)
delete[] seen_;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}由于InflationLayer没有继承Costmap2D，所以它和静态地图与障碍地图这两层不同，它没有属于自己的栅格地图要维护，matchSize函数自然不需要根据主地图的参数来调节本层地图。这个函数先获取主地图的分辨率，接着调用cellDistance函数，这个函数可以把global系以米为单位的长度转换成以cell为单位的距离，故获得了地图上的膨胀参数cell_inflation_radius_。
接下来调用computeCaches函数，完成填充两个“查找表”，内容后述。
最后根据主地图大小创建seen_数组，它用于标记cell是否已经过计算。
二、查找表computeCaches & computeCosts
这里称cached_distances_和cached_costs_为查找表，因为它们是后续膨胀计算的查找表。
由于是基于膨胀参数来设置查找表，所以当cell_inflation_radius_==0，直接返回。
第二个if块是用来初始化这两张查找表，只在cell_inflation_radius_发生变化时执行。两张查找表都是尺寸为(cell_inflation_radius_ + 2)x(cell_inflation_radius_ + 2)的二维数组。在为查找表分配内存时，同时填充了cached_distances_查找表，内中元素值是每个点到(0,0)点的三角距离。
void InflationLayer::computeCaches()
{
if (cell_inflation_radius_ == 0)
return;
// based on the inflation radius... compute distance and cost caches
if (cell_inflation_radius_ != cached_cell_inflation_radius_)
{
deleteKernels();
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)
{
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)
{
cached_distances_[i][j] = hypot(i, j);
}
}
cached_cell_inflation_radius_ = cell_inflation_radius_;
}
for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
{
for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
{
cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
cached_costs_索引是点坐标，值是先由该点算出distance，再由distance算出的cost。
}
}以下是新增代码，目的是直观显示这两个查找表。
VALIDATE(cell_inflation_radius_ == 11, null_str);
double cached_distances[13][13];
int cached_costs[13][13]; // 若用unsgned char，visual studio会认为数组是字符串
for (int row = 0; row < cell_inflation_radius_ + 2; row ++) {
for (int col = 0; col < cell_inflation_radius_ + 2; col ++) {
cached_distances[row][col] = cached_distances_[row][col];
cached_costs[row][col] = cached_costs_[row][col];
}
}
}
图1是一次运行时两张表实例，cell_inflation_radius_ = 11，inscribed_radius_ = 0.14。
cached_distances_
查找表的索引是栅格坐标，值是该点到(0,0)点的三角形距离。以(4,11)为例，就是
cached_costs_
查找表的索引是栅格坐标，值是先由该点算出distance，再由distance算出的膨胀cost。
virtual inline unsigned char computeCost(double distance) const
{
unsigned char cost = 0;
if (distance == 0)
cost = LETHAL_OBSTACLE;
else if (distance * resolution_ <= inscribed_radius_)
cost = INSCRIBED_INFLATED_OBSTACLE;
else
{
// make sure cost falls off by Euclidean distance
double euclidean_distance = distance * resolution_;
double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
}
return cost;
}- 若距离为0，即(0,0)点，则设置cached_costs_上的值为LETHAL_OBSTACLE(254)，表示为障碍物本身；
- 若距离(转换到world坐标系后) <= 机器人足迹内切圆半径，设置值为INSCRIBED_INFLATED_OBSTACLE（253），即由于机器人有体积造成的障碍物膨胀；
- 其它距离时，以距离远近为比例（指数型）设置值。
以(4,11)为例，它的距离是11.7047，以它为参数调用computeCost，计算出膨胀cost是27。
2.1 查表：distanceLookup、costLookup
inline double distanceLookup(int mx, int my, int src_x, int src_y)
{
unsigned int dx = abs(mx - src_x);
unsigned int dy = abs(my - src_y);
return cached_distances_[dx][dy];
}
inline unsigned char costLookup(int mx, int my, int src_x, int src_y)
{
unsigned int dx = abs(mx - src_x);
unsigned int dy = abs(my - src_y);
return cached_costs_[dx][dy];
}distanceLookup、costLookup是针对这两个表的查表操作，逻辑一样，分两步。第一步算出索引，第二步以索引查表得到值。参数(src_x, src_y)是障碍栅格，(mx, my)是要计算值的栅格。它们都是map坐标系的栅格坐标。
三、更新膨胀地图边界InflationLayer::updateBounds
膨胀层updateBounds是在传入的bound值的基础上，通过inflation_radius_再次扩张。
- 传给Layer的updateBounds的4个范围参数是index值，对这4变量，Layer可以返回超过实际地图尺寸。
- 调用完各Layer的updateBounds后，LayeredCostmap::updateMap会根据实际尺寸调整这4变量。
- 调整后的xn、yn也是index值。但xn这一行、yn这一列，不应该视为有效坐标。
void InflationLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
double* min_y, double* max_x, double* max_y)
{
if (need_reinflation_)
{
last_min_x_ = *min_x;
last_min_y_ = *min_y;
last_max_x_ = *max_x;
last_max_y_ = *max_y;
// For some reason when I make these -<double>::max() it does not
// work with Costmap2D::worldToMapEnforceBounds(), so I'm using
// -<float>::max() instead.
*min_x = -std::numeric_limits<float>::max();
*min_y = -std::numeric_limits<float>::max();
*max_x = std::numeric_limits<float>::max();
*max_y = std::numeric_limits<float>::max();
need_reinflation_ = false;need_reinflation_指示下次updateBounds时，是否需要更新整张代价地图。初始值true，经过一次updateBounds后改为false。意味初始会更新整张地图，这时给出浮点的最小值、最大值，肯定已超过实际地图尺寸。不过调用它的LayeredCostmap::updateMap会根据实际尺寸调整这4个变量。
使用last_min_x_、last_min_y_、last_max_x_、last_max_y_是为了让主地图更干净清除上次膨胀区。举个例子，第k次，x方向传下的min、max是(1, 20)，第k+1次传下的是(2, 18)，由于k次膨胀区大于k+1次，如果按k+1次的(2,18)复位主地图，主地图会留下k时膨胀区多出的3条脏列。
}
else
{
double tmp_min_x = last_min_x_;
double tmp_min_y = last_min_y_;
double tmp_max_x = last_max_x_;
double tmp_max_y = last_max_y_;
last_min_x_ = *min_x;
last_min_y_ = *min_y;
last_max_x_ = *max_x;
last_max_y_ = *max_y;
*min_x = std::min(tmp_min_x, *min_x) - inflation_radius_;
*min_y = std::min(tmp_min_y, *min_y) - inflation_radius_;
*max_x = std::max(tmp_max_x, *max_x) + inflation_radius_;
*max_y = std::max(tmp_max_y, *max_y) + inflation_radius_;
}
}
四、更新膨胀地图代价InflationLayer::updateCosts
用指针master_array指向主地图，并获取主地图的尺寸，确认seen_数组被正确设置。
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
if (cell_inflation_radius_ == 0)
return;
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");
unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
if (seen_ == NULL) {
ROS_WARN("InflationLayer::updateCosts(): seen_ array is NULL");
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
else if (seen_size_ != size_x * size_y)
{
ROS_WARN("InflationLayer::updateCosts(): seen_ array size is wrong");
delete[] seen_;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
memset(seen_, false, size_x * size_y * sizeof(bool));
// We need to include in the inflation cells outside the bounding
// box min_i...max_j, by the amount cell_inflation_radius_. Cells
// up to that distance outside the box can still influence the costs
// stored in cells inside the box.
min_i -= cell_inflation_radius_;
min_j -= cell_inflation_radius_;
max_i += cell_inflation_radius_;
max_j += cell_inflation_radius_;
min_i = std::max(0, min_i);
min_j = std::max(0, min_j);
max_i = std::min(int(size_x), max_i);
max_j = std::min(int(size_y), max_j);这有个疑问，updateBounds时，已经以膨胀半径传染后的区域反馈给LayeredCostmap，即这时参数给的{min_i、min_j、max_j、max_j}已经包括了传染区域，那这里为什么还要再一次以膨胀半径扩大区域？——因为参数给的区域外面会有障碍栅格，这些栅格传染出区域会落在参数给的区域。这个新{min_i、min_j、max_j、max_j}可连带着找出那些障碍栅格。
接下来遍历bound中的cell，找到cost为LETHAL_OBSTACLE，即障碍物cell，将其以CellData形式放进inflation_cells_[0.0]中，inflation_cells_的定义如下：
std::map<double, std::vector<CellData> > inflation_cells_;
它是一个映射，由浮点数→CellData数组，CellData这个类定义在inflation_layer.h中。一个CellData对应一个栅格，用来记录这个cell的索引(index_)、在map坐标系下坐标(x_, y_)，以及与它最近的障碍物栅格在map坐标系下坐标(src_x_, src_y)。这个映射以到障碍物的距离为key给bound内的cell归类，为膨胀做准备。
自然距离为0对应的cell即障碍物cell本身，目前得到的inflation_cells_只包括障碍物本身。
// Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
// We use a map<distance, list> to emulate the priority queue used before, with a notable performance boost
// Start with lethal obstacles: by definition distance is 0.0
std::vector<CellData>& obs_bin = inflation_cells_[0.0];
for (int j = min_j; j < max_j; j++)
{
for (int i = min_i; i < max_i; i++)
{
int index = master_grid.getIndex(i, j);
unsigned char cost = master_array[index];
if (cost == LETHAL_OBSTACLE)
{
obs_bin.push_back(CellData(index, i, j, i, j));
}
}
}接下来开始由目前的inflation_cells_进行传染，直到将所有的cell填充进去。
二重循环，外层遍历“key”，即距障碍物的距离；内层遍历“value”，即CellData。
内部循环记录当前cell的坐标和离它最近的cell的坐标，然后调用costLookup函数，它会用到查找表cached_costs_，通过该cell与最近障碍物的距离来确定该cell的膨胀cost。
case1 当原cost为NO_INFORMATION
inflate_unknown_用于指示将未知栅格代价设置成膨胀cost的阀值。
- inflate_unknown_为true。阀值只要膨胀cost>FREE_SPACE，代价就设为cost。换句话说，当膨胀到未知栅格，只要存在膨胀cost，就覆盖掉NO_INFORMATION。
- inflate_unknown_为false。阀值需要膨胀cost≥INSCRIBED_INFLATED_OBSTACLE，代价才设为膨胀cost。换句话说，当膨胀到未知栅格，膨胀cost需要INSCRIBED_INFLATED_OBSTACLE或LETHAL_OBSTACLE，才覆盖掉NO_INFORMATION。
不论inflate_unknown_是何值，只要膨胀cost是INSCRIBED_INFLATED_OBSTACLE或LETHAL_OBSTACLE，那肯定会使用膨胀cost。
对未知栅格，如果不能因为inflate_unknown_改为膨胀cost，它将保持原值NO_INFORMATION。设置代码是“std::max(old_cost, cost)”，由于原代价old_cost等于NO_INFORMATION，NO_INFORMATION总是最大值，max的结果只会是它。
case2 原cost不是NO_INFORMATION，取原cost和膨胀cost的较大值
// Process cells by increasing distance; new cells are appended to the corresponding distance bin, so they
// can overtake previously inserted but farther away cells
std::map<double, std::vector<CellData> >::iterator bin;
for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
{
for (int i = 0; i < bin->second.size(); ++i)
{
// process all cells at distance dist_bin.first
const CellData& cell = bin->second[i];
unsigned int index = cell.index_;
// ignore if already visited
if (seen_[index])
{
continue;
}
seen_[index] = true;
unsigned int mx = cell.x_;
unsigned int my = cell.y_;
unsigned int sx = cell.src_x_;
unsigned int sy = cell.src_y_;
// assign the cost associated with the distance from an obstacle to the cell
unsigned char cost = costLookup(mx, my, sx, sy);
unsigned char old_cost = master_array[index];
if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
master_array[index] = cost;
else
master_array[index] = std::max(old_cost, cost);(sx, sy)是距离该cell最近的障碍点，(abs(mx - sx), abs(my - sy))可算出它和障碍点坐标偏移，以它为索引查找cached_costs_，就可得到它的膨胀cost，值存储在变量cost。old_cost是master地图上的原cost。
接下把当前cell的左、下、右、上四个点也放进inflation list。可以确定，此时扩展出的点距离障碍点，比当前点肯定更远，以坐标为索引查找cached_distances_时，得到的肯定是更大的distance。
// attempt to put the neighbors of the current cell onto the inflation list if (mx > 0) enqueue(index - 1, mx - 1, my, sx, sy); if (my > 0) enqueue(index - size_x, mx, my - 1, sx, sy); if (mx < size_x - 1) enqueue(index + 1, mx + 1, my, sx, sy); if (my < size_y - 1) enqueue(index + size_x, mx, my + 1, sx, sy); } } inflation_cells_.clear(); }
五、传染InflationLayer::enquene
这个函数通过调用distanceLookup函数，在查找表cached_distances_上，找到当前cell与最近的障碍物cell(src_x, src_y)的距离，当距离在阈值内，将当前cell加入到inflation_cells_相应键（表示距离）下。
inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
unsigned int src_x, unsigned int src_y)
{
if (!seen_[index])
{
// we compute our distance table one cell further than the inflation radius dictates so we can make the check below
double distance = distanceLookup(mx, my, src_x, src_y);
// we only want to put the cell in the list if it is within the inflation radius of the obstacle point
if (distance > cell_inflation_radius_)
return;
// push the cell data onto the inflation list and mark
inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
}
}参考
- 【ROS-Navigation】Costmap2D代价地图源码解读-膨胀层InflationLayer https://blog.csdn.net/Neo11111/article/details/104869048