fix: correctly free memory in inflation layer

costmap_2d/include/costmap_2d/inflation_layer.h

@@ -56,18 +56,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_(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_;
 };

costmap_2d/plugins/inflation_layer.cpp

@@ -223,6 +223,7 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
   // 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];
+  obs_bin.reserve(200);
   for (int j = min_j; j < max_j; j++)
   {
     for (int i = min_i; i < max_i; i++)
@@ -231,19 +232,27 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
       unsigned char cost = master_array[index];
       if (cost == LETHAL_OBSTACLE)
       {
-        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_)
   {
-    for (const auto& current_cell: dist_bin)
+    dist_bin.reserve(200);
+    for (std::size_t i = 0; i < dist_bin.size(); ++i)
     {
       // process all cells at distance dist_bin.first
-      unsigned int index = current_cell.index_;
+      // unsigned int index = current_cell.index_;
+      auto& current_cell = dist_bin[i];
+
+      unsigned int mx = current_cell.x_;
+      unsigned int my = current_cell.y_;
+      unsigned int sx = current_cell.src_x_;
+      unsigned int sy = current_cell.src_y_;
+      unsigned int index = master_grid.getIndex(mx, my);
 
       // ignore if already visited
       if (seen_[index])
@@ -253,11 +262,6 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
 
       seen_[index] = true;
 
-      unsigned int mx = current_cell.x_;
-      unsigned int my = current_cell.y_;
-      unsigned int sx = current_cell.src_x_;
-      unsigned int sy = current_cell.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];
@@ -276,12 +280,10 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
       if (my < size_y - 1)
         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>();
   }
 }
 
@@ -299,7 +301,7 @@ inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigne
     const 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);
+    inflation_cells_[distance_matrix_[mx - src_x + r][my - src_y + r]].emplace_back(mx, my, src_x, src_y);
   }
 }
 
@@ -339,10 +341,6 @@ void InflationLayer::computeCaches()
   int max_dist = generateIntegerDistances();
   inflation_cells_.clear();
   inflation_cells_.resize(max_dist + 1);
-  for (auto& dist : inflation_cells_)
-  {
-      dist.reserve(200);
-  }
 }
 
 int InflationLayer::generateIntegerDistances()

costmap_2d/test/inflation_tests.cpp

@@ -1,10 +1,10 @@
 /*
  * Copyright (c) 2013, Willow Garage, Inc.
  * All rights reserved.
- * 
+ *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
- * 
+ *
  *     * Redistributions of source code must retain the above copyright
  *       notice, this list of conditions and the following disclaimer.
  *     * Redistributions in binary form must reproduce the above copyright
@@ -13,7 +13,7 @@
  *     * Neither the name of the Willow Garage, Inc. nor the names of its
  *       contributors may be used to endorse or promote products derived from
  *       this software without specific prior written permission.
- * 
+ *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@@ -50,16 +50,16 @@ std::vector<Point> setRadii(LayeredCostmap& layers, double length, double width,
   std::vector<Point> polygon;
   Point p;
   p.x = width;
-  p.y = length; 
+  p.y = length;
   polygon.push_back(p);
   p.x = width;
-  p.y = -length; 
+  p.y = -length;
   polygon.push_back(p);
   p.x = -width;
-  p.y = -length; 
+  p.y = -length;
   polygon.push_back(p);
   p.x = -width;
-  p.y = length; 
+  p.y = length;
   polygon.push_back(p);
   layers.setFootprint(polygon);
 
@@ -75,16 +75,16 @@ void validatePointInflation(unsigned int mx, unsigned int my, Costmap2D* costmap
   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 (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 = hypot(dx, dy);
@@ -106,26 +106,22 @@ void validatePointInflation(unsigned int mx, unsigned int my, Costmap2D* costmap
 
         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);
         }
       }
@@ -143,7 +139,7 @@ TEST(costmap, testAdjacentToObstacleCanStillMove){
   //               circumscribed radius = 3.1
   std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);
 
-  ObstacleLayer* olayer = addObstacleLayer(layers, tf);  
+  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
   InflationLayer* ilayer = addInflationLayer(layers, tf);
   layers.setFootprint(polygon);
 
@@ -169,7 +165,7 @@ TEST(costmap, testInflationShouldNotCreateUnknowns){
   //               circumscribed radius = 3.1
   std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);
 
-  ObstacleLayer* olayer = addObstacleLayer(layers, tf);  
+  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
   InflationLayer* ilayer = addInflationLayer(layers, tf);
   layers.setFootprint(polygon);
 
@@ -194,7 +190,7 @@ TEST(costmap, testCostFunctionCorrectness){
   //               circumscribed radius = 8.0
   std::vector<Point> polygon = setRadii(layers, 5.0, 6.25, 10.5);
 
-  ObstacleLayer* olayer = addObstacleLayer(layers, tf);  
+  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
   InflationLayer* ilayer = addInflationLayer(layers, tf);
   layers.setFootprint(polygon);
 
@@ -291,7 +287,7 @@ TEST(costmap, testInflation){
   std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
 
   addStaticLayer(layers, tf);
-  ObstacleLayer* olayer = addObstacleLayer(layers, tf);  
+  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
   InflationLayer* ilayer = addInflationLayer(layers, tf);
   layers.setFootprint(polygon);
 
@@ -317,7 +313,7 @@ TEST(costmap, testInflation){
   // It and its 2 neighbors makes 3 obstacles
   ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)51);
 
-  // @todo Rewrite 
+  // @todo Rewrite
   // Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells
   addObservation(olayer, 2, 0);
   layers.updateMap(0,0,0);
@@ -355,7 +351,7 @@ TEST(costmap, testInflation2){
   std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
 
   addStaticLayer(layers, tf);
-  ObstacleLayer* olayer = addObstacleLayer(layers, tf);  
+  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
   InflationLayer* ilayer = addInflationLayer(layers, tf);
   layers.setFootprint(polygon);
 
@@ -367,7 +363,7 @@ TEST(costmap, testInflation2){
 
   Costmap2D* costmap = layers.getCostmap();
   //printMap(*costmap);
-  ASSERT_EQ(costmap->getCost(2, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);  
+  ASSERT_EQ(costmap->getCost(2, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
   ASSERT_EQ(costmap->getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
 }
 
@@ -382,7 +378,7 @@ TEST(costmap, testInflation3){
   // 1 2 3
   std::vector<Point> polygon = setRadii(layers, 1, 1.75, 3);
 
-  ObstacleLayer* olayer = addObstacleLayer(layers, tf);  
+  ObstacleLayer* olayer = addObstacleLayer(layers, tf);
   InflationLayer* ilayer = addInflationLayer(layers, tf);
   layers.setFootprint(polygon);