[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 <[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]>
ros-navigation · Jun 12, 2024 · eb560c4 · eb560c4
1 parent 60d2e61
commit eb560c4
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 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
@@ -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_;
 };

diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp
@@ -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]) {
@@ -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<CellData>();
   }
 
   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
@@ -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);
@@ -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);
         }
       }