From eb560c4769d746707e916490655eb9f24cb846e3 Mon Sep 17 00:00:00 2001 From: Davide Faconti Date: Wed, 12 Jun 2024 19:43:17 +0200 Subject: [PATCH] [nav2_costmap_2s]: free correctly the memory in the InflationLayer (#4424) * [nav2_costmap_2s]: free correctly the memory in the InflationLayer Signed-off-by: Davide Faconti * 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 * formatting Signed-off-by: Davide Faconti --------- Signed-off-by: Davide Faconti --- .../nav2_costmap_2d/inflation_layer.hpp | 6 ++-- nav2_costmap_2d/plugins/inflation_layer.cpp | 33 +++++++++---------- .../test/integration/inflation_tests.cpp | 19 +++++------ 3 files changed, 25 insertions(+), 33 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index b83010c62d..0ae010e94b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -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(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_; }; diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 067877e548..e2542aa9cf 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -230,12 +230,13 @@ 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(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); } } } @@ -243,12 +244,18 @@ InflationLayer::updateCosts( // 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]) { @@ -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]; @@ -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(); } current_ = true; @@ -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); } } @@ -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 diff --git a/nav2_costmap_2d/test/integration/inflation_tests.cpp b/nav2_costmap_2d/test/integration/inflation_tests.cpp index ad867128b1..b88c373688 100644 --- a/nav2_costmap_2d/test/integration/inflation_tests.cpp +++ b/nav2_costmap_2d/test/integration/inflation_tests.cpp @@ -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> 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>::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); @@ -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); } }