diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp index f2b511dc..f6c668a1 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/InflationFilter.hpp +++ b/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 #include #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 diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp index ac759c2b..08a6687c 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp @@ -39,8 +39,10 @@ *********************************************************************/ +#include +#include #include -#include +#include #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>(nm.layers.get(src_layer)); if (!src_view || src_view->size() != nm.navcels.size()) { return false; } + const std::vector src = src_view->data(); // dst may alias src - // Destination layer (create if missing) auto dst_view = nm.layers.add_or_get( - dst_layer, nm.navcels.size(), - ::navmap::layer_type_tag()); + dst_layer, nm.navcels.size(), ::navmap::layer_type_tag()); if (!dst_view) {return false;} - if (dst_view->data().size() != nm.navcels.size()) { - const_cast &>(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 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 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::infinity(); - std::vector dist(N, INF); - struct Node { float d; NavCelId cid; bool operator<(const Node & o) const {return d > o.d;} }; - std::priority_queue 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 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(INSCRIBED_INFLATED_OBSTACLE) - 1; - double c = std::exp(-cost_scaling_factor * static_cast(x)) * base; - if (c < 0.0) {c = 0.0;} - if (c > base) {c = base;} - return static_cast(std::lround(c)); + const double c = std::exp(-cost_scaling_factor * static_cast(d - r_ins)) * base; + return static_cast(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(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"); diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_inflation_filter_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_inflation_filter_tests.cpp index 9411a35f..121622da 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/navmap_inflation_filter_tests.cpp +++ b/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 #include +#include #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 & f) +{ + const std::size_t s = nm.create_surface("map"); + const auto base = static_cast(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(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("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("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::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("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("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("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("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(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("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("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("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); +}