[melodic] Speedup (~25%) inflation layer update

costmap_2d/include/costmap_2d/inflation_layer.h

@@ -168,25 +168,35 @@ class InflationLayer : public Layer
 
   void computeCaches();
   void deleteKernels();
+  int generateIntegerDistances();
   void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid);
 
   unsigned int cellDistance(double world_dist)
   {
     return layered_costmap_->getCostmap()->cellDistance(world_dist);
   }
 
+  /**
+   * @brief  Given an index of a cell in the costmap, place it into a list pending for obstacle inflation
+   * @param  index The index of the cell
+   * @param  mx The x coordinate of the cell (can be computed from the index, but saves time to store it)
+   * @param  my The y coordinate of the cell (can be computed from the index, but saves time to store it)
+   * @param  src_x The x index of the obstacle point inflation started at
+   * @param  src_y The y index of the obstacle point inflation started at
+   */
   inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
                       unsigned int src_x, unsigned int src_y);
 
   unsigned int cell_inflation_radius_;
   unsigned int cached_cell_inflation_radius_;
-  std::map<double, std::vector<CellData> > inflation_cells_;
+  std::vector<std::vector<CellData>> inflation_cells_;
 
   bool* seen_;
   int seen_size_;
 
   unsigned char** cached_costs_;
   double** cached_distances_;
+  std::vector<std::vector<int>> distance_matrix_;
   double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
 
   dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig> *dsrv_;

costmap_2d/plugins/inflation_layer.cpp

@@ -180,8 +180,10 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
     return;
 
   // make sure the inflation list is empty at the beginning of the cycle (should always be true)
-  ROS_ASSERT_MSG(inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");
-
+  for (auto& dist:inflation_cells_)
+  {
+    ROS_ASSERT_MSG(dist.empty(), "The inflation list must be empty at the beginning of inflation");
+  }
   unsigned char* master_array = master_grid.getCharMap();
   unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
 
@@ -215,9 +217,8 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
 
   // Inflation list; we append cells to visit in a list associated with its distance to the nearest obstacle
   // 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
-  std::vector<CellData>& obs_bin = inflation_cells_[0.0];
+  auto& obs_bin = inflation_cells_[0];
   for (int j = min_j; j < max_j; j++)
   {
     for (int i = min_i; i < max_i; i++)
@@ -226,22 +227,19 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
       unsigned char cost = master_array[index];
       if (cost == LETHAL_OBSTACLE)
       {
-        obs_bin.push_back(CellData(index, i, j, i, j));
+        obs_bin.emplace_back(index, 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
-  std::map<double, std::vector<CellData> >::iterator bin;
-  for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
+  for (const auto& dist_bin: inflation_cells_)
   {
-    for (int i = 0; i < bin->second.size(); ++i)
+    for (const auto& current_cell: dist_bin)
     {
       // process all cells at distance dist_bin.first
-      const CellData& cell = bin->second[i];
-
-      unsigned int index = cell.index_;
+      unsigned int index = current_cell.index_;
 
       // ignore if already visited
       if (seen_[index])
@@ -251,10 +249,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 = 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);
@@ -276,18 +274,13 @@ void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i,
     }
   }
 
-  inflation_cells_.clear();
+  for (auto& dist:inflation_cells_)
+  {
+    dist.clear();
+    dist.reserve(200);
+  }
 }
 
-/**
- * @brief  Given an index of a cell in the costmap, place it into a list pending for obstacle inflation
- * @param  grid The costmap
- * @param  index The index of the cell
- * @param  mx The x coordinate of the cell (can be computed from the index, but saves time to store it)
- * @param  my The y coordinate of the cell (can be computed from the index, but saves time to store it)
- * @param  src_x The x index of the obstacle point inflation started at
- * @param  src_y The y index of the obstacle point inflation started at
- */
 inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
                                     unsigned int src_x, unsigned int src_y)
 {
@@ -299,9 +292,10 @@ inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigne
     // we only want to put the cell in the list if it is within the inflation radius of the obstacle point
     if (distance > cell_inflation_radius_)
       return;
+    const int r = cell_inflation_radius_ + 2;
 
     // push the cell data onto the inflation list and mark
-    inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
+    inflation_cells_[distance_matrix_[mx - src_x+r][my - src_y+r]].emplace_back(index, mx, my, src_x, src_y);
   }
 }
 
@@ -338,6 +332,49 @@ void InflationLayer::computeCaches()
       cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
     }
   }
+  int max_dist = generateIntegerDistances();
+  inflation_cells_.clear();
+  inflation_cells_.resize(max_dist + 1);
+  for (auto& dist : inflation_cells_)
+  {
+      dist.reserve(200);
+  }
+}
+
+int InflationLayer::generateIntegerDistances()
+{
+  const int r = cell_inflation_radius_ + 2;
+  const int size = r * 2 + 1;
+
+  std::vector<std::pair<int, int>> points;
+
+  for (int y = -r; y <= r; y++)
+  {
+    for (int x = -r; x <= r; x++)
+    {
+      if (x * x + y * y <= r * r)
+        points.push_back({x, y});
+    }
+  }
+
+  std::sort(points.begin(), points.end(),
+            [](const std::pair<int, int> &a, const std::pair<int, int> &b) -> bool {
+              return a.first * a.first + a.second * a.second < b.first * b.first + b.second * b.second;
+            }
+  );
+
+  std::vector<std::vector<int> > distance_matrix(size, std::vector<int>(size, 0));
+  std::pair<int, int> last = {0, 0};
+  int level = 0;
+  for (auto const &p : points)
+  {
+    if (p.first * p.first + p.second * p.second != last.first * last.first + last.second * last.second)
+      level++;
+    distance_matrix[p.first + r][p.second + r] = level;
+    last = p;
+  }
+  distance_matrix_ = distance_matrix;
+  return level;
 }
 
 void InflationLayer::deleteKernels()