NavMap InflationFilter: inflate from 3D distances to the real border

maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp

@@ -17,7 +17,6 @@
 #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_
 #define EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_
 
-#include <queue>
 #include <string>
 
 #include "navmap_core/NavMap.hpp"
@@ -44,7 +43,8 @@ class InflationFilter : public NavMapFilter
     const std::string & dst_layer,
     float inflation_radius,
     float cost_scaling_factor,
-    float inscribed_radius = 0.0f);
+    float inscribed_radius = 0.0f,
+    bool inflate_borders = true);
 
   bool is_adding_layer() override {return true;}
   std::string get_layer_name() override {return "inflated_obstacles";}
@@ -53,6 +53,7 @@ class InflationFilter : public NavMapFilter
   ::navmap::NavMap navmap_;
 
   double inflation_radius_, cost_scaling_factor_, inscribed_radius_;
+  bool inflate_borders_{true};  // open edges of the NavMap (walls, drops) inflate like obstacles
 };
 
 }  // namespace navmap

maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp

@@ -39,8 +39,10 @@
  *********************************************************************/
 
 
+#include <algorithm>
+#include <cmath>
 #include <string>
-#include <queue>
+#include <vector>
 
 #include "easynav_common/Parameters.hpp"
 #include "easynav_common/RobotGeometry.hpp"
@@ -69,7 +71,8 @@ bool InflationFilter::inflate_layer_u8(
   const std::string & dst_layer,
   float inflation_radius,
   float cost_scaling_factor,
-  float inscribed_radius)
+  float inscribed_radius,
+  bool inflate_borders)
 {
   using ::navmap::NavCelId;
   using navmap_ros::FREE_SPACE;
@@ -81,120 +84,47 @@ bool InflationFilter::inflate_layer_u8(
     return false;
   }
 
-  // Source layer
   auto src_view =
     std::dynamic_pointer_cast<::navmap::LayerView<std::uint8_t>>(nm.layers.get(src_layer));
   if (!src_view || src_view->size() != nm.navcels.size()) {
     return false;
   }
+  const std::vector<std::uint8_t> src = src_view->data();  // dst may alias src
 
-  // Destination layer (create if missing)
   auto dst_view = nm.layers.add_or_get<std::uint8_t>(
-    dst_layer, nm.navcels.size(),
-    ::navmap::layer_type_tag<std::uint8_t>());
+    dst_layer, nm.navcels.size(), ::navmap::layer_type_tag<std::uint8_t>());
   if (!dst_view) {return false;}
-  if (dst_view->data().size() != nm.navcels.size()) {
-    const_cast<std::vector<std::uint8_t> &>(dst_view->data()).assign(nm.navcels.size(), FREE_SPACE);
-  }
+  auto & dst = dst_view->mutable_data();
+  dst.resize(nm.navcels.size(), FREE_SPACE);
 
-  const size_t N = nm.navcels.size();
+  const std::size_t N = nm.navcels.size();
   const float R = inflation_radius;
   const float r_ins = std::clamp(inscribed_radius, 0.0f, R);
 
-  // Precompute XY centroids for each NavCel
-  std::vector<Eigen::Vector2f> C(N);
+  // Distance (3D, along the surface) to the nearest obstacle cell or, if enabled, to the
+  // map border: an open edge is a wall, a drop or unmapped space.
+  std::vector<NavCelId> lethal;
   for (NavCelId cid = 0; cid < N; ++cid) {
-    const Eigen::Vector3f cc = nm.navcel_centroid(cid);
-    C[cid] = {cc.x(), cc.y()};
-  }
-
-  // Distance map (multi-source Dijkstra)
-  const float INF = std::numeric_limits<float>::infinity();
-  std::vector<float> dist(N, INF);
-  struct Node { float d; NavCelId cid; bool operator<(const Node & o) const {return d > o.d;} };
-  std::priority_queue<Node> pq;
-
-  auto & dst = dst_view->mutable_data();        // mark dirty
-  const auto & src = src_view->data();
-
-  // Initialize destinations and seeds
-  bool any_seed = false;
-  for (NavCelId cid = 0; cid < N; ++cid) {
-    const std::uint8_t s = src[cid];
-
-    // Seed 1: lethal obstacles
-    if (s == LETHAL_OBSTACLE) {
-      dist[cid] = 0.0f;
-      pq.push({0.0f, cid});
-      dst[cid] = LETHAL_OBSTACLE;
-      any_seed = true;
-      continue;
-    }
-
-    // Seed 2: boundary triangles (missing neighbors)
-    if (nm.navcel_neighbors(cid).size() < 3) {
-      dist[cid] = 0.0f;
-      pq.push({0.0f, cid});
-      dst[cid] = LETHAL_OBSTACLE;
-      any_seed = true;
-      continue;
-    }
-
-    // Unknown handling
-    if (s == NO_INFORMATION) {
-      dst[cid] = NO_INFORMATION;
-    } else {
-      if (dst_layer != src_layer) {
-        dst[cid] = FREE_SPACE;
-      }
-    }
-  }
-
-  if (!any_seed) {
-    return true;  // nothing to inflate
+    if (src[cid] == LETHAL_OBSTACLE) {lethal.push_back(cid);}
   }
+  const std::vector<float> dist = nm.distance_field(lethal, inflate_borders, R);
 
-  // Distance-to-cost conversion
   auto cost_from_dist = [&](float d) -> std::uint8_t {
-      if (d <= 0.0f) {return LETHAL_OBSTACLE;}
       if (d <= r_ins) {return INSCRIBED_INFLATED_OBSTACLE;}
       if (d > R) {return FREE_SPACE;}
-
-      const float x = d - r_ins;
       const double base = static_cast<int>(INSCRIBED_INFLATED_OBSTACLE) - 1;
-      double c = std::exp(-cost_scaling_factor * static_cast<double>(x)) * base;
-      if (c < 0.0) {c = 0.0;}
-      if (c > base) {c = base;}
-      return static_cast<std::uint8_t>(std::lround(c));
+      const double c = std::exp(-cost_scaling_factor * static_cast<double>(d - r_ins)) * base;
+      return static_cast<std::uint8_t>(std::lround(std::clamp(c, 0.0, base)));
     };
 
-  // Bounded Dijkstra expansion
-  while (!pq.empty()) {
-    const auto [du, u] = pq.top(); pq.pop();
-    if (du != dist[u]) {continue;}
-    if (du > R) {continue;}
-
-    if (dst[u] != NO_INFORMATION && dst[u] != LETHAL_OBSTACLE) {
-      const std::uint8_t c = cost_from_dist(du);
-      if (c > dst[u]) {dst[u] = c;}  // monotonic accumulation
-    }
-
-    // Neighbor relaxation
-    for (NavCelId v : nm.navcel_neighbors(u)) {
-      const size_t vidx = static_cast<size_t>(v);
-      if (vidx >= N) {continue;}
-
-      // Unknown cells carry the distance (as in costmap_2d) but keep their value.
-      const float step = (C[u] - C[v]).norm();
-      if (step <= 0.0f) {continue;}
-      const float alt = du + step;
-      if (alt < dist[v] && alt <= R) {
-        dist[v] = alt;
-        pq.push({alt, v});
-      }
+  for (NavCelId cid = 0; cid < N; ++cid) {
+    if (src[cid] == LETHAL_OBSTACLE || src[cid] == NO_INFORMATION) {
+      dst[cid] = src[cid];  // obstacles and unknown keep their value (unknown carries distance)
+      continue;
     }
+    const std::uint8_t c = cost_from_dist(dist[cid]);
+    dst[cid] = (dst_layer == src_layer) ? std::max(src[cid], c) : c;
   }
-
   return true;
 }
 
@@ -214,16 +144,20 @@ InflationFilter::on_initialize()
     *node, plugin_name_ + ".cost_scaling_factor",
     cost_scaling_factor_);
 
+  easynav::declare_parameter_if_absent(
+    *node, plugin_name_ + ".inflate_borders", inflate_borders_);
+
   node->get_parameter(plugin_name_ + ".inflation_radius", inflation_radius_);
   node->get_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_);
+  node->get_parameter(plugin_name_ + ".inflate_borders", inflate_borders_);
   // The robot's: "system_node.robot_geometry" (the filter's own is deprecated).
   inscribed_radius_ = easynav::get_robot_geometry(
     *node, {"", plugin_name_ + ".inscribed_radius", ""}).inscribed_radius;
 
   RCLCPP_INFO(
     node->get_logger(),
-    "InflationFilter (NavMap): radius=%.3f cost_scaling=%.3f inscribed=%.3f",
-    inflation_radius_, cost_scaling_factor_, inscribed_radius_);
+    "InflationFilter (NavMap): radius=%.3f cost_scaling=%.3f inscribed=%.3f borders=%s",
+    inflation_radius_, cost_scaling_factor_, inscribed_radius_, inflate_borders_ ? "yes" : "no");
 }
 
 void InflationFilter::update(::easynav::NavState & nav_state)
@@ -240,7 +174,8 @@ void InflationFilter::update(::easynav::NavState & nav_state)
     "inflated_obstacles",
     inflation_radius_,
     cost_scaling_factor_,
-    inscribed_radius_);
+    inscribed_radius_,
+    inflate_borders_);
 
   if (!ok) {
     RCLCPP_ERROR(get_node()->get_logger(), "InflationFilter: inflate_layer_u8() failed");

maps_managers/easynav_navmap_maps_manager/tests/navmap_inflation_filter_tests.cpp

@@ -12,7 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <cmath>
 #include <cstdint>
+#include <functional>
 
 #include "gtest/gtest.h"
 #include "nav_msgs/msg/occupancy_grid.hpp"
@@ -162,3 +164,145 @@ TEST_F(NavmapInflationFilterTest, InflatingTwiceGivesTheSameLayer)
       nm_, "obstacles", "inflated_obstacles", kRadius, kScaling, kInscribed));
   EXPECT_EQ(cost_at(2.45f, 3.0f), first);
 }
+
+namespace
+{
+
+// nx x ny cells of `res` m at z = f(x, y) + z0, as a new surface, with an empty "obstacles" layer.
+void add_grid(
+  ::navmap::NavMap & nm, int nx, int ny, float res, float x0, float y0,
+  const std::function<float(float, float)> & f)
+{
+  const std::size_t s = nm.create_surface("map");
+  const auto base = static_cast<uint32_t>(nm.positions.size());
+  for (int j = 0; j <= ny; ++j) {
+    for (int i = 0; i <= nx; ++i) {
+      const float x = x0 + i * res, y = y0 + j * res;
+      nm.add_vertex({x, y, f(x, y)});
+    }
+  }
+  auto v = [&](int i, int j) {return base + static_cast<uint32_t>(j * (nx + 1) + i);};
+  for (int j = 0; j < ny; ++j) {
+    for (int i = 0; i < nx; ++i) {
+      nm.add_navcel_to_surface(s, nm.add_navcel(v(i, j), v(i + 1, j), v(i + 1, j + 1)));
+      nm.add_navcel_to_surface(s, nm.add_navcel(v(i, j), v(i + 1, j + 1), v(i, j + 1)));
+    }
+  }
+}
+
+void finish(::navmap::NavMap & nm)
+{
+  nm.rebuild_geometry_accels();
+  nm.add_layer<std::uint8_t>("obstacles", "", "", FREE_SPACE);
+}
+
+std::uint8_t cost_in(const ::navmap::NavMap & nm, float x, float y, float z)
+{
+  std::size_t sidx;
+  ::navmap::NavCelId cid;
+  Eigen::Vector3f bary, hit;
+  EXPECT_TRUE(nm.locate_navcel({x, y, z}, sidx, cid, bary, &hit));
+  return nm.layer_get<std::uint8_t>("inflated_obstacles", cid, 123);
+}
+
+::navmap::NavCelId cell_in(const ::navmap::NavMap & nm, float x, float y, float z)
+{
+  std::size_t sidx;
+  ::navmap::NavCelId cid;
+  Eigen::Vector3f bary, hit;
+  EXPECT_TRUE(nm.locate_navcel({x, y, z}, sidx, cid, bary, &hit));
+  return cid;
+}
+
+// Same cost formula as the filter, for a given distance.
+std::uint8_t expected_cost(float d, float r_ins, float radius, float scaling)
+{
+  if (d <= r_ins) {return INSCRIBED_INFLATED_OBSTACLE;}
+  if (d > radius) {return FREE_SPACE;}
+  const double base = INSCRIBED_INFLATED_OBSTACLE - 1;
+  return static_cast<std::uint8_t>(std::lround(std::exp(-scaling * (d - r_ins)) * base));
+}
+
+}  // namespace
+
+TEST(NavmapInflationBorders, BorderCellsAreNotLethalAndCostByTheirDistance)
+{
+  ::navmap::NavMap nm;
+  add_grid(nm, 40, 40, 0.1f, 0.0f, 0.0f, [](float, float) {return 0.0f;});
+  finish(nm);
+  easynav::navmap::InflationFilter f;
+  ASSERT_TRUE(f.inflate_layer_u8(nm, "obstacles", "inflated_obstacles", 1.0f, 3.0f, 0.3f));
+  for (::navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) {
+    EXPECT_NE(nm.layer_get<std::uint8_t>("inflated_obstacles", c), LETHAL_OBSTACLE);
+  }
+  // Cell centroid ~0.5 m from the x = 0 border: between inscribed and free.
+  const auto cid = cell_in(nm, 0.53f, 2.03f, 0.0f);
+  const float d = nm.navcel_centroid(cid).x();
+  EXPECT_NEAR(nm.layer_get<std::uint8_t>("inflated_obstacles", cid),
+    expected_cost(d, 0.3f, 1.0f, 3.0f), 1);
+  EXPECT_EQ(cost_in(nm, 2.0f, 2.0f, 0.0f), FREE_SPACE);  // 2 m from every border
+}
+
+TEST(NavmapInflationBorders, NarrowCorridorKeepsItsTrueWidth)
+{
+  // A 1.2 m wide corridor (ramp between two side walls). Robot inscribed radius 0.5 m:
+  // the centre line, 0.6 m from both edges, must stay passable.
+  ::navmap::NavMap nm;
+  add_grid(nm, 100, 3, 0.4f, 0.0f, 0.0f, [](float, float) {return 0.0f;});
+  finish(nm);
+  easynav::navmap::InflationFilter f;
+  ASSERT_TRUE(f.inflate_layer_u8(nm, "obstacles", "inflated_obstacles", 2.0f, 1.5f, 0.5f));
+  const auto centre = cell_in(nm, 20.13f, 0.6f, 0.0f);
+  EXPECT_LT(nm.layer_get<std::uint8_t>("inflated_obstacles", centre), INSCRIBED_INFLATED_OBSTACLE);
+}
+
+TEST(NavmapInflationBorders, DisabledBordersDoNotInflate)
+{
+  ::navmap::NavMap nm;
+  add_grid(nm, 40, 40, 0.1f, 0.0f, 0.0f, [](float, float) {return 0.0f;});
+  finish(nm);
+  easynav::navmap::InflationFilter f;
+  ASSERT_TRUE(
+    f.inflate_layer_u8(nm, "obstacles", "inflated_obstacles", 1.0f, 3.0f, 0.3f, false));
+  for (::navmap::NavCelId c = 0; c < nm.navcels.size(); ++c) {
+    EXPECT_EQ(nm.layer_get<std::uint8_t>("inflated_obstacles", c), FREE_SPACE);
+  }
+}
+
+TEST(NavmapInflationBorders, SlopeDistanceIsMeasuredAlongTheSurface)
+{
+  // Same obstacle and XY offset on a flat floor and on a 35 deg ramp: the ramp is farther.
+  const float k = std::tan(35.0f * static_cast<float>(M_PI) / 180.0f);
+  for (const float slope : {0.0f, k}) {
+    ::navmap::NavMap nm;
+    add_grid(nm, 60, 60, 0.1f, 0.0f, 0.0f, [slope](float x, float) {return slope * x;});
+    finish(nm);
+    nm.layer_set<std::uint8_t>("obstacles", cell_in(nm, 3.03f, 3.03f, slope * 3.03f),
+      LETHAL_OBSTACLE);
+    easynav::navmap::InflationFilter f;
+    ASSERT_TRUE(
+      f.inflate_layer_u8(nm, "obstacles", "inflated_obstacles", 1.5f, 3.0f, 0.3f, false));
+    const auto probe = cell_in(nm, 3.63f, 3.03f, slope * 3.63f);
+    const auto obst = cell_in(nm, 3.03f, 3.03f, slope * 3.03f);
+    const float d3 = (nm.navcel_centroid(probe) - nm.navcel_centroid(obst)).norm();
+    EXPECT_NEAR(
+      nm.layer_get<std::uint8_t>("inflated_obstacles", probe), expected_cost(d3, 0.3f, 1.5f, 3.0f),
+      1)
+      << "slope " << slope;
+  }
+}
+
+TEST(NavmapInflationBorders, AnObstacleOnOneFloorDoesNotInflateTheFloorAbove)
+{
+  ::navmap::NavMap nm;
+  add_grid(nm, 40, 40, 0.1f, 0.0f, 0.0f, [](float, float) {return 0.0f;});
+  add_grid(nm, 40, 40, 0.1f, 0.0f, 0.0f, [](float, float) {return 2.5f;});
+  finish(nm);
+  nm.layer_set<std::uint8_t>("obstacles", cell_in(nm, 2.03f, 2.03f, 0.0f), LETHAL_OBSTACLE);
+  easynav::navmap::InflationFilter f;
+  ASSERT_TRUE(
+    f.inflate_layer_u8(nm, "obstacles", "inflated_obstacles", 1.0f, 3.0f, 0.3f, false));
+  EXPECT_EQ(cost_in(nm, 2.13f, 2.03f, 0.0f), INSCRIBED_INFLATED_OBSTACLE);  // floor 0
+  EXPECT_EQ(cost_in(nm, 2.03f, 2.03f, 2.5f), FREE_SPACE);                     // floor 1 above
+  EXPECT_EQ(cost_in(nm, 2.13f, 2.03f, 2.5f), FREE_SPACE);
+}