Switch from map<double> to vector<> in using precached integer distances

Credits to original author from ros-planning/navigation#839

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -102,14 +102,14 @@ class InflationLayer : public Layer
   inline unsigned char computeCost(double distance) const
   {
     unsigned char cost = 0;
-    double euclidean_distance = distance * resolution_;
     if (distance == 0) {
       cost = LETHAL_OBSTACLE;
-    } else if (euclidean_distance <= inscribed_radius_) {
+    } else if (distance * resolution_ <= inscribed_radius_) {
       cost = INSCRIBED_INFLATED_OBSTACLE;
     } else {
       // make sure cost falls off by Euclidean distance
-      double factor = exp(-1.0 * cost_scaling_factor_ * (euclidean_distance - inscribed_radius_));
+      double factor =
+        exp(-1.0 * cost_scaling_factor_ * (distance * resolution_ - inscribed_radius_));
       cost = static_cast<unsigned char>((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
     }
     return cost;
@@ -155,6 +155,8 @@ class InflationLayer : public Layer
 
   void computeCaches();
 
+  int generateIntegerDistances();
+
   unsigned int cellDistance(double world_dist)
   {
     return layered_costmap_->getCostmap()->cellDistance(world_dist);
@@ -168,14 +170,16 @@ class InflationLayer : public Layer
   bool inflate_unknown_, inflate_around_unknown_;
   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_;
 
   double resolution_;
 
   std::vector<bool> seen_;
+  std::vector<bool> queued_;
 
   std::vector<unsigned char> cached_costs_;
   std::vector<double> cached_distances_;
+  std::vector<std::vector<int>> distance_matrix_;
   unsigned int cache_length_;
   double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
 

nav2_costmap_2d/plugins/inflation_layer.cpp

@@ -88,6 +88,7 @@ InflationLayer::onInitialize()
 
   current_ = true;
   seen_.clear();
+  queued_.clear();
   cached_distances_.clear();
   cached_costs_.clear();
   need_reinflation_ = false;
@@ -103,6 +104,7 @@ InflationLayer::matchSize()
   cell_inflation_radius_ = cellDistance(inflation_radius_);
   computeCaches();
   seen_ = std::vector<bool>(costmap->getSizeInCellsX() * costmap->getSizeInCellsY(), false);
+  queued_ = std::vector<bool>(costmap->getSizeInCellsX() * costmap->getSizeInCellsY(), false);
 }
 
 void
@@ -163,9 +165,11 @@ InflationLayer::updateCosts(
   }
 
   // make sure the inflation list is empty at the beginning of the cycle (should always be true)
-  RCLCPP_FATAL_EXPRESSION(
-    rclcpp::get_logger("nav2_costmap_2d"),
-    !inflation_cells_.empty(), "The inflation list must be empty at the beginning of inflation");
+  for (auto & dist:inflation_cells_) {
+    RCLCPP_FATAL_EXPRESSION(
+      rclcpp::get_logger("nav2_costmap_2d"),
+      !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();
@@ -175,9 +179,11 @@ InflationLayer::updateCosts(
       rclcpp::get_logger(
         "nav2_costmap_2d"), "InflationLayer::updateCosts(): seen_ vector size is wrong");
     seen_ = std::vector<bool>(size_x * size_y, false);
+    queued_ = std::vector<bool>(size_x * size_y, false);
   }
 
   std::fill(begin(seen_), end(seen_), false);
+  std::fill(begin(queued_), end(queued_), false);
 
   // We need to include in the inflation cells outside the bounding
   // box min_i...max_j, by the amount cell_inflation_radius_.  Cells
@@ -199,7 +205,7 @@ InflationLayer::updateCosts(
   // 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++) {
       int index = static_cast<int>(master_grid.getIndex(i, j));
@@ -213,11 +219,11 @@ 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
-  std::map<double, std::vector<CellData>>::iterator bin;
-  for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin) {
-    for (auto & cell : bin->second) {
+  for (const auto & dist_bin: inflation_cells_) {
+    for (const auto & current_cell: dist_bin) {
       // process all cells at distance dist_bin.first
-      unsigned int index = cell.index_;
+
+      unsigned int index = current_cell.index_;
 
       // ignore if already visited
       if (seen_[index]) {
@@ -226,10 +232,10 @@ InflationLayer::updateCosts(
 
       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);
@@ -258,7 +264,10 @@ InflationLayer::updateCosts(
     }
   }
 
-  inflation_cells_.clear();
+  for (auto & dist:inflation_cells_) {
+    dist.clear();
+    dist.reserve(200);
+  }
 }
 
 /**
@@ -275,7 +284,7 @@ InflationLayer::enqueue(
   unsigned int index, unsigned int mx, unsigned int my,
   unsigned int src_x, unsigned int src_y)
 {
-  if (!seen_[index]) {
+  if (!seen_[index] && !queued_[index]) {
     // we compute our distance table one cell further than the
     // inflation radius dictates so we can make the check below
     double distance = distanceLookup(mx, my, src_x, src_y);
@@ -286,8 +295,12 @@ InflationLayer::enqueue(
       return;
     }
 
+    const unsigned int r = cell_inflation_radius_ + 2;
+
     // push the cell data onto the inflation list and mark
-    inflation_cells_[distance].emplace_back(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);
+    queued_[index] = true;
   }
 }
 
@@ -319,6 +332,52 @@ InflationLayer::computeCaches()
       cached_costs_[i * cache_length_ + j] = computeCost(cached_distances_[i * cache_length_ + 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.emplace_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;
 }
 
 }  // namespace nav2_costmap_2d