Do no use iterators to traverse cells vectors (as they get increased)

ros-planning · Nov 3, 2016 · f8d5aa4 · f8d5aa4
1 parent da46325
commit f8d5aa4
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 24 deletions.
diff --git a/costmap_2d/plugins/inflation_layer.cpp b/costmap_2d/plugins/inflation_layer.cpp
@@ -232,10 +232,12 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
   std::map<double, std::vector<CellData> >::iterator bin;
   for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
   {
-    for (std::vector<CellData>::iterator cell = bin->second.begin(); cell != bin->second.end(); ++cell)
+    for (int i = 0; i < bin->second.size(); ++i)
     {
       // process all cells at distance dist_bin.first
-      unsigned int index = cell->index_;
+      const CellData& cell = bin->second[i];
+
+      unsigned int index = cell.index_;
 
       // ignore if already visited
       if (seen_[index])
@@ -245,10 +247,10 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
 
       seen_[index] = true;
 
-      unsigned int mx = cell->x_;
-      unsigned int my = cell->y_;
-      unsigned int sx = cell->src_x_;
-      unsigned int sy = cell->src_y_;
+      unsigned int mx = cell.x_;
+      unsigned int my = cell.y_;
+      unsigned int sx = cell.src_x_;
+      unsigned int sy = cell.src_y_;
 
       // assign the cost associated with the distance from an obstacle to the cell
       unsigned char cost = costLookup(mx, my, sx, sy);

diff --git a/costmap_2d/test/inflation_tests.cpp b/costmap_2d/test/inflation_tests.cpp
@@ -79,45 +79,46 @@ void validatePointInflation(unsigned int mx, unsigned int my, Costmap2D* costmap
   m[0].push_back(initial);
   for (std::map<double, std::vector<CellData> >::iterator bin = m.begin(); bin != m.end(); ++bin)
   {
-    for (std::vector<CellData>::iterator cell = bin->second.begin(); cell != bin->second.end(); ++cell)
+    for (int i = 0; i < bin->second.size(); ++i)
     {
-      if (!seen[cell->index_])
+      const CellData& cell = bin->second[i];
+      if (!seen[cell.index_])
       {
-        seen[cell->index_] = true;
-        unsigned int dx = abs(cell->x_ - cell->src_x_);
-        unsigned int dy = abs(cell->y_ - cell->src_y_);
+        seen[cell.index_] = true;
+        unsigned int dx = abs(cell.x_ - cell.src_x_);
+        unsigned int dy = abs(cell.y_ - cell.src_y_);
         double dist = hypot(dx, dy);
 
         unsigned char expected_cost = ilayer->computeCost(dist);
-        ASSERT_TRUE(costmap->getCost(cell->x_, cell->y_) >= expected_cost);
+        ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost);
 
         if (dist > inflation_radius)
         {
           continue;
         }
 
-        if (cell->x_ > 0)
+        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(costmap->getIndex(cell.x_-1, cell.y_),
+                        cell.x_-1, cell.y_, cell.src_x_, cell.src_y_);
           m[dist].push_back(data);
         }
-        if (cell->y_ > 0)
+        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(costmap->getIndex(cell.x_, cell.y_-1),
+                        cell.x_, cell.y_-1, cell.src_x_, cell.src_y_);
           m[dist].push_back(data);
         }
-        if (cell->x_ < costmap->getSizeInCellsX() - 1)
+        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(costmap->getIndex(cell.x_+1, cell.y_),
+                        cell.x_+1, cell.y_, cell.src_x_, cell.src_y_);
           m[dist].push_back(data);
         }
-        if (cell->y_ < costmap->getSizeInCellsY() - 1)
+        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(costmap->getIndex(cell.x_, cell.y_+1),
+                        cell.x_, cell.y_+1, cell.src_x_, cell.src_y_);
           m[dist].push_back(data);
         }
       }