如下图是更新地图膨胀
cpp
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
//用指针master_array指向主地图，并获取主地图的尺寸，确认seen_数组被正确设置。
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
if (!enabled_ || (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);
// 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
//接下来遍历bound中的cell，找到cost为LETHAL_OBSTACLE，即障碍物cell，将其以CellData形式放进inflation_cells_[0.0]中，inflation_cells_的定义如下：
//std::map<double, std::vector > inflation_cells_;
//它是一个映射，由浮点数→CellData数组，CellData这个类定义在inflation_layer.h中，
//是专门用来记录当前cell的索引和与它最近的障碍物的索引的。这个映射以距离障碍物的距离为标准给bound内的cell归类，为膨胀做准备；
//自然距离为0对应的cell即障碍物cell本身，
//目前得到的inflation_cells_只包括障碍物本身。
// 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));
}
}
}
// 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);
// 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没有自身的栅格地图要维护，直接在主地图上进行操作，它根据膨胀参数设置用来膨胀的"参考矩阵"，并在主地图上从障碍物出发，不断传播更新，完成对整个地图障碍的膨胀。
不断的从障碍物周围"上下左右"开始膨胀，并且根据参考矩阵去计算不同距离的栅格权重。
cpp
// 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); unsigned char cost = costLookup(mx, my, sx, sy);为参考矩阵计算， master_array[index] = cost;为当前cell栅格赋值。
cpp
// 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);
}这是考察下一个点，上下左右，如果在障碍物半径内，则继续循环到上述的" unsigned char cost = costLookup(mx, my, sx, sy);为参考矩阵计算， master_array[index] = cost;为当前cell栅格赋值。"
如此循环即可，类似于A*算法的膨胀。
cpp
/**
*@brief给定成本图中某个单元格的索引，将其放入等待障碍物膨胀的列表中
*@param grid成本图
*@param index单元格的索引
*@param mx单元格的x坐标（可以根据索引计算，但可以节省存储时间）
*@param my单元格的y坐标（可以根据索引计算，但可以节省存储时间）
*@param src_x障碍点通货膨胀的x指数开始于
*@param src_y障碍点通货膨胀的y指数始于
*/
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));
}
}
cpp
double distance = distanceLookup(mx, my, src_x, src_y);使用计算考察点和障碍物的距离，然后判断是否在膨胀半径内，如果是，则保存到nflation_cells_中用于下一次循环中。
Reference