old_cost为原本的cost值，cost为膨胀值
void
InflationLayer::updateCosts(
nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j,
int max_i,
int max_j)
{
std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
if (!enabled_ || (cell_inflation_radius_ == 0)) {
return;
}
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
for (auto & dist : inflation_cells_) {
RCLCPP_FATAL_EXPRESSION(
logger_,
!dist.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_.size() != size_x * size_y) {
RCLCPP_WARN(
logger_, "InflationLayer::updateCosts(): seen_ vector size is wrong");
seen_ = std::vector<bool>(size_x * size_y, false);
}
std::fill(begin(seen_), end(seen_), false);
// 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.
const int base_min_i = min_i;
const int base_min_j = min_j;
const int base_max_i = max_i;
const int base_max_j = max_j;
min_i -= static_cast<int>(cell_inflation_radius_);
min_j -= static_cast<int>(cell_inflation_radius_);
max_i += static_cast<int>(cell_inflation_radius_);
max_j += static_cast<int>(cell_inflation_radius_);
min_i = std::max(0, min_i);
min_j = std::max(0, min_j);
max_i = std::min(static_cast<int>(size_x), max_i);
max_j = std::min(static_cast<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
// Start with lethal obstacles: by definition distance is 0.0
auto & obs_bin = inflation_cells_[0];
for (int j = min_j; j < max_j; j++) {
for (int i = min_i; i < max_i; i++) {
int index = static_cast<int>(master_grid.getIndex(i, j));
unsigned char cost = master_array[index];
if (cost == LETHAL_OBSTACLE || (inflate_around_unknown_ && cost == NO_INFORMATION ) || cost == MAP_BOUNDARIES ) {
obs_bin.emplace_back(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
for (const auto & dist_bin : inflation_cells_) {
for (std::size_t i = 0; i < dist_bin.size(); ++i) {
// Do not use iterator or for-range based loops to
// iterate though dist_bin, since it's size might
// change when a new cell is enqueued, invalidating all iterators
unsigned int index = dist_bin[i].index_;
// ignore if already visited
if (seen_[index]) {
continue;
}
seen_[index] = true;
unsigned int mx = dist_bin[i].x_;
unsigned int my = dist_bin[i].y_;
unsigned int sx = dist_bin[i].src_x_;
unsigned int sy = dist_bin[i].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];
// In order to avoid artifacts appeared out of boundary areas
// when some layer is going after inflation_layer,
// we need to apply inflation_layer only to inside of given bounds
if (static_cast<int>(mx) >= base_min_i &&
static_cast<int>(my) >= base_min_j &&
static_cast<int>(mx) < base_max_i &&
static_cast<int>(my) < base_max_j)
{
if (old_cost == NO_INFORMATION)
{
if (inflate_unknown_ && cost > FREE_SPACE)
{
master_array[index] = cost;
}else
master_array[index] = old_cost;
} else if (old_cost == nav2_costmap_2d::TYPE_PASSAGE)
{
master_array[index] = old_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);
}
}
}
for (auto & dist : inflation_cells_) {
dist.clear();
dist.reserve(200);
}
current_ = true;
}