Skip to content

Commit

Permalink
[nav2_costmap_2s]: free correctly the memory in the InflationLayer (r…
Browse files Browse the repository at this point in the history
…os-navigation#4424)

* [nav2_costmap_2s]: free correctly the memory in the InflationLayer

Signed-off-by: Davide Faconti <[email protected]>

* Remove 4 redundant bytes from CellData in Inflationlayer

Note: performance was measured and this is slightly faster, even if we
need to recompute the index. Probably a benefit at the level of the CPU
cache

Signed-off-by: Davide Faconti <[email protected]>

* formatting

Signed-off-by: Davide Faconti <[email protected]>

---------

Signed-off-by: Davide Faconti <[email protected]>
  • Loading branch information
facontidavide authored and Manos-G committed Aug 1, 2024
1 parent 4678d78 commit 168945e
Show file tree
Hide file tree
Showing 3 changed files with 25 additions and 33 deletions.
6 changes: 2 additions & 4 deletions nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,18 +60,16 @@ class CellData
public:
/**
* @brief Constructor for a CellData objects
* @param i The index of the cell in the cost map
* @param x The x coordinate of the cell in the cost map
* @param y The y coordinate of the cell in the cost map
* @param sx The x coordinate of the closest obstacle cell in the costmap
* @param sy The y coordinate of the closest obstacle cell in the costmap
* @return
*/
CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy)
: index_(static_cast<unsigned int>(i)), x_(x), y_(y), src_x_(sx), src_y_(sy)
CellData(unsigned int x, unsigned int y, unsigned int sx, unsigned int sy)
: x_(x), y_(y), src_x_(sx), src_y_(sy)
{
}
unsigned int index_;
unsigned int x_, y_;
unsigned int src_x_, src_y_;
};
Expand Down
33 changes: 15 additions & 18 deletions nav2_costmap_2d/plugins/inflation_layer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -230,25 +230,32 @@ InflationLayer::updateCosts(

// Start with lethal obstacles: by definition distance is 0.0
auto & obs_bin = inflation_cells_[0];
obs_bin.reserve(200);
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)) {
obs_bin.emplace_back(index, i, j, i, j);
obs_bin.emplace_back(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 (auto & dist_bin : inflation_cells_) {
dist_bin.reserve(200);
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_;
const CellData & cell = dist_bin[i];
unsigned int mx = cell.x_;
unsigned int my = cell.y_;
unsigned int sx = cell.src_x_;
unsigned int sy = cell.src_y_;
unsigned int index = master_grid.getIndex(mx, my);

// ignore if already visited
if (seen_[index]) {
Expand All @@ -257,11 +264,6 @@ InflationLayer::updateCosts(

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];
Expand Down Expand Up @@ -296,11 +298,9 @@ InflationLayer::updateCosts(
enqueue(index + size_x, mx, my + 1, sx, sy);
}
}
}

for (auto & dist : inflation_cells_) {
dist.clear();
dist.reserve(200);
// This level of inflation_cells_ is not needed anymore. We can free the memory
// Note that dist_bin.clear() is not enough, because it won't free the memory
dist_bin = std::vector<CellData>();
}

current_ = true;
Expand Down Expand Up @@ -334,8 +334,8 @@ InflationLayer::enqueue(
const unsigned int r = cell_inflation_radius_ + 2;

// push the cell data onto the inflation list and mark
inflation_cells_[distance_matrix_[mx - src_x + r][my - src_y + r]].emplace_back(
index, mx, my, src_x, src_y);
const auto dist = distance_matrix_[mx - src_x + r][my - src_y + r];
inflation_cells_[dist].emplace_back(mx, my, src_x, src_y);
}
}

Expand Down Expand Up @@ -372,9 +372,6 @@ InflationLayer::computeCaches()
int max_dist = generateIntegerDistances();
inflation_cells_.clear();
inflation_cells_.resize(max_dist + 1);
for (auto & dist : inflation_cells_) {
dist.reserve(200);
}
}

int
Expand Down
19 changes: 8 additions & 11 deletions nav2_costmap_2d/test/integration/inflation_tests.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -125,15 +125,16 @@ void TestNode::validatePointInflation(
bool * seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()];
memset(seen, false, costmap->getSizeInCellsX() * costmap->getSizeInCellsY() * sizeof(bool));
std::map<double, std::vector<CellData>> m;
CellData initial(costmap->getIndex(mx, my), mx, my, mx, my);
CellData initial(mx, my, mx, my);
m[0].push_back(initial);
for (std::map<double, std::vector<CellData>>::iterator bin = m.begin();
bin != m.end(); ++bin)
{
for (unsigned int i = 0; i < bin->second.size(); ++i) {
const CellData cell = bin->second[i];
if (!seen[cell.index_]) {
seen[cell.index_] = true;
const auto index = costmap->getIndex(cell.x_, cell.y_);
if (!seen[index]) {
seen[index] = true;
unsigned int dx = (cell.x_ > cell.src_x_) ? cell.x_ - cell.src_x_ : cell.src_x_ - cell.x_;
unsigned int dy = (cell.y_ > cell.src_y_) ? cell.y_ - cell.src_y_ : cell.src_y_ - cell.y_;
double dist = std::hypot(dx, dy);
Expand All @@ -152,23 +153,19 @@ void TestNode::validatePointInflation(
}

if (cell.x_ > 0) {
CellData data(costmap->getIndex(cell.x_ - 1, cell.y_),
cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_);
CellData data(cell.x_ - 1, cell.y_, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
if (cell.y_ > 0) {
CellData data(costmap->getIndex(cell.x_, cell.y_ - 1),
cell.x_, cell.y_ - 1, cell.src_x_, cell.src_y_);
CellData data(cell.x_, cell.y_ - 1, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
if (cell.x_ < costmap->getSizeInCellsX() - 1) {
CellData data(costmap->getIndex(cell.x_ + 1, cell.y_),
cell.x_ + 1, cell.y_, cell.src_x_, cell.src_y_);
CellData data(cell.x_ + 1, cell.y_, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
if (cell.y_ < costmap->getSizeInCellsY() - 1) {
CellData data(costmap->getIndex(cell.x_, cell.y_ + 1),
cell.x_, cell.y_ + 1, cell.src_x_, cell.src_y_);
CellData data(cell.x_, cell.y_ + 1, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
}
Expand Down

0 comments on commit 168945e

Please sign in to comment.