[nav2_costmap_2s]: free correctly the memory in the InflationLayer

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

nav2_costmap_2d/plugins/inflation_layer.cpp

@@ -230,6 +230,7 @@ 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));
@@ -243,7 +244,8 @@ 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
@@ -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;
@@ -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