Add option to inflate unknown space (#1675)

* Add option to inflate around unknown space

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Fix bug regarding lower bound of double in worldToMapEnforceBounds

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Convert 2D caches to 1D vectors for automatic memory management and better locality

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Add general optimizations and modern syntax

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

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

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

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Add tests for inflate_unknown and inflate_around_unknown

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Remove commented out assert

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp

@@ -39,9 +39,9 @@
 /** Provides a mapping for often used cost values */
 namespace nav2_costmap_2d
 {
-static const unsigned char NO_INFORMATION = 255;
-static const unsigned char LETHAL_OBSTACLE = 254;
-static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
-static const unsigned char FREE_SPACE = 0;
+static constexpr unsigned char NO_INFORMATION = 255;
+static constexpr unsigned char LETHAL_OBSTACLE = 254;
+static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
+static constexpr unsigned char FREE_SPACE = 0;
 }
 #endif  // NAV2_COSTMAP_2D__COST_VALUES_HPP_

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -64,7 +64,7 @@ class CellData
    * @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)
+  : index_(static_cast<unsigned int>(i)), x_(x), y_(y), src_x_(sx), src_y_(sy)
   {
   }
   unsigned int index_;
@@ -77,27 +77,21 @@ class InflationLayer : public Layer
 public:
   InflationLayer();
 
-  virtual ~InflationLayer()
-  {
-    deleteKernels();
-  }
+  ~InflationLayer() override = default;
 
-  virtual void onInitialize();
-  virtual void updateBounds(
+  void onInitialize() override;
+  void updateBounds(
     double robot_x, double robot_y, double robot_yaw, double * min_x,
     double * min_y,
     double * max_x,
-    double * max_y);
-  virtual void updateCosts(
+    double * max_y) override;
+  void updateCosts(
     nav2_costmap_2d::Costmap2D & master_grid,
-    int min_i, int min_j, int max_i, int max_j);
-  virtual bool isDiscretized()
-  {
-    return true;
-  }
-  virtual void matchSize();
+    int min_i, int min_j, int max_i, int max_j) override;
+
+  void matchSize() override;
 
-  virtual void reset()
+  void reset() override
   {
     matchSize();
   }
@@ -114,15 +108,15 @@ class InflationLayer : public Layer
       cost = INSCRIBED_INFLATED_OBSTACLE;
     } else {
       // make sure cost falls off by Euclidean distance
-      double euclidean_distance = distance * resolution_;
-      double factor = exp(-1.0 * cost_scaling_factor_ * (euclidean_distance - inscribed_radius_));
-      cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
+      double factor =
+        exp(-1.0 * cost_scaling_factor_ * (distance * resolution_ - inscribed_radius_));
+      cost = static_cast<unsigned char>((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
     }
     return cost;
   }
 
 protected:
-  virtual void onFootprintChanged();
+  void onFootprintChanged() override;
 
 private:
   /**
@@ -133,11 +127,13 @@ class InflationLayer : public Layer
    * @param src_y The y coordinate of the source cell
    * @return
    */
-  inline double distanceLookup(int mx, int my, int src_x, int src_y)
+  inline double distanceLookup(
+    unsigned int mx, unsigned int my, unsigned int src_x,
+    unsigned int src_y)
   {
-    unsigned int dx = abs(mx - src_x);
-    unsigned int dy = abs(my - src_y);
-    return cached_distances_[dx][dy];
+    unsigned int dx = (mx > src_x) ? mx - src_x : src_x - mx;
+    unsigned int dy = (my > src_y) ? my - src_y : src_y - my;
+    return cached_distances_[dx * cache_length_ + dy];
   }
 
   /**
@@ -148,16 +144,18 @@ class InflationLayer : public Layer
    * @param src_y The y coordinate of the source cell
    * @return
    */
-  inline unsigned char costLookup(int mx, int my, int src_x, int src_y)
+  inline unsigned char costLookup(
+    unsigned int mx, unsigned int my, unsigned int src_x,
+    unsigned int src_y)
   {
-    unsigned int dx = abs(mx - src_x);
-    unsigned int dy = abs(my - src_y);
-    return cached_costs_[dx][dy];
+    unsigned int dx = (mx > src_x) ? mx - src_x : src_x - mx;
+    unsigned int dy = (my > src_y) ? my - src_y : src_y - my;
+    return cached_costs_[dx * cache_length_ + dy];
   }
 
   void computeCaches();
-  void deleteKernels();
-  void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char * master_grid);
+
+  int generateIntegerDistances();
 
   unsigned int cellDistance(double world_dist)
   {
@@ -169,17 +167,19 @@ class InflationLayer : public Layer
     unsigned int src_x, unsigned int src_y);
 
   double inflation_radius_, inscribed_radius_, cost_scaling_factor_;
-  bool inflate_unknown_;
+  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_;
 
-  unsigned char ** cached_costs_;
-  double ** cached_distances_;
+  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_;
 
   // Indicates that the entire costmap should be reinflated next time around.

nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp

@@ -98,8 +98,8 @@ void addObstacleLayer(
 }
 
 void addObservation(
-  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> & olayer, double x, double y, double z = 0.0,
-  double ox = 0.0, double oy = 0.0, double oz = MAX_Z)
+  std::shared_ptr<nav2_costmap_2d::ObstacleLayer> olayer, double x, double y, double z = 0.0,
+  double ox = 0.0, double oy = 0.0, double oz = MAX_Z, bool marking = true, bool clearing = true)
 {
   sensor_msgs::msg::PointCloud2 cloud;
   sensor_msgs::PointCloud2Modifier modifier(cloud);
@@ -119,7 +119,7 @@ void addObservation(
 
   // obstacle range = raytrace range = 100.0
   nav2_costmap_2d::Observation obs(p, cloud, 100.0, 100.0);
-  olayer->addStaticObservation(obs, true, true);
+  olayer->addStaticObservation(obs, marking, clearing);
 }
 
 void addInflationLayer(