From 7a6e8c67b6ab5d6699b6b175bdcdcdf836ad6792 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Thu, 15 Oct 2026 22:02:16 +0200 Subject: [PATCH] NavMap filters: incremental, in place, and sparse layer updates MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The filters update the NavMap in NavState in place instead of copying it twice each cycle. ObstacleFilter rebuilds its layer from the static map only when the map changes, and then writes just the cells that become obstacles or stop being one. InflationFilter keeps an exact navmap::DistanceField updated with the obstacle cells that come and go, and rewrites only the cells whose cost changes. The maps manager publishes sparse layer updates, only with subscribers (whole layers for a new one), and clears the recorded changes at the end of each cycle. NavMapFilter gets a virtual destructor: plugins are deleted through it. README: the obstacle filter's plugin name is ObstaclesFilter. Signed-off-by: Francisco Martín Rico --- .../easynav_navmap_maps_manager/README.md | 11 +- .../NavMapMapsManager.hpp | 2 + .../filters/InflationFilter.hpp | 24 +- .../filters/NavMapFilter.hpp | 1 + .../filters/ObstacleFilter.hpp | 14 +- .../NavMapMapsManager.cpp | 23 +- .../filters/InflationFilter.cpp | 148 +++++-- .../filters/ObstacleFilter.cpp | 81 +++- .../tests/CMakeLists.txt | 3 + .../navmap_incremental_filters_tests.cpp | 388 ++++++++++++++++++ 10 files changed, 627 insertions(+), 68 deletions(-) create mode 100644 maps_managers/easynav_navmap_maps_manager/tests/navmap_incremental_filters_tests.cpp diff --git a/maps_managers/easynav_navmap_maps_manager/README.md b/maps_managers/easynav_navmap_maps_manager/README.md index d17a327b..9a3550e0 100644 --- a/maps_managers/easynav_navmap_maps_manager/README.md +++ b/maps_managers/easynav_navmap_maps_manager/README.md @@ -63,11 +63,12 @@ With `.local.enabled`, the maps manager keeps a NavMap around the robot, #### **ObstacleFilter** -- **Plugin Name:** `easynav_navmap_maps_manager/NavMapMapsManager/ObstacleFilter` +- **Plugin Name:** `easynav_navmap_maps_manager/NavMapMapsManager/ObstaclesFilter` - **Type:** `easynav::navmap::ObstacleFilter` - **Description:** Detects occupied NavCels from the point perceptions in `NavState` and marks them as `LETHAL_OBSTACLE` in the `"obstacles"` layer, on top of the static map's `"occupancy"` layer (if any). - The points are downsampled, those within `max_range` and below `max_height` (robot frame) are grouped in 0.3 m columns, and a column is an obstacle when its highest point is more than `min_height` (plus `min_height_per_meter` for each meter from the robot) above the NavMap surface under it (along the surface's normal). So a 2D laser detects obstacles too, and the ground or a ramp, being on the surface, is not one. + The points are downsampled, those within `max_range` and below `max_height` (robot frame) are grouped in 0.3 m columns, and a column is an obstacle when its highest point is more than `min_height` (plus `min_height_per_meter` for each meter from the robot) above the NavMap surface under it (along the surface's normal). So a 2D laser detects obstacles too, and the ground or a ramp, being on the surface, is not one. + The layer is rebuilt from the static map only when the map changes; each cycle writes just the cells that become obstacles or stop being one. | Parameter | Type | Default | Description | |---|---|---:|---| @@ -85,7 +86,7 @@ With `.local.enabled`, the maps manager keeps a NavMap around the robot, - **Type:** `easynav::navmap::InflationFilter` - **Description:** Expands obstacle information from the `"obstacles"` layer into an `"inflated_obstacles"` layer, assigning graded costs depending on distance to obstacles and map boundaries (NavCels with missing neighbors). - This filter mimics the behavior of `costmap_2d::InflationLayer` but operates on the NavMap triangular mesh. + This filter mimics the behavior of `costmap_2d::InflationLayer` but operates on the NavMap triangular mesh. Distances are exact and measured along the surface (`navmap::DistanceField`), and they are updated incrementally: only the obstacle cells that appear or disappear move the field, and only the cells whose cost changes are rewritten. | Parameter | Type | Default | Description | |---|---|---:|---| @@ -95,6 +96,8 @@ With `.local.enabled`, the maps manager keeps a NavMap around the robot, | **Input Layer:** | | | Reads from `"obstacles"`. | | **Output Layer:** | | | Writes to `"inflated_obstacles"`. | +The filters update the map in `NavState` in place and record which cells they change; the maps manager publishes those changes and clears them at the end of each cycle. + ## Interfaces (Topics and Services) ### Subscriptions and Publications @@ -102,7 +105,7 @@ With `.local.enabled`, the maps manager keeps a NavMap around the robot, | Direction | Topic | Type | Purpose | QoS | |---|---|---|---|---| | Publisher | `//map` | `navmap_ros_interfaces/msg/NavMap` | Publishes the full NavMap. | depth=1 | -| Publisher | `//map_updates` | `navmap_ros_interfaces/msg/NavMapLayer` | Publishes incremental layer updates. | depth=100 | +| Publisher | `//map_updates` | `navmap_ros_interfaces/msg/NavMapLayer` | Layer updates of the filters: only the cells that changed (sparse `NavMapLayer`, or the whole layer if many changed), only when someone subscribes; a new subscriber gets the whole layers. | depth=100 | | Subscription | `//incoming_occ_map` | `nav_msgs/msg/OccupancyGrid` | Input occupancy grid to import into NavMap. | depth=1, transient_local, reliable | | Subscription | `//incoming_pc2_map` | `sensor_msgs/msg/PointCloud2` | Input point cloud to build/update NavMap. | depth=100 | diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp index e9e3e9c3..7187d261 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/NavMapMapsManager.hpp @@ -27,6 +27,7 @@ #include "nav_msgs/msg/occupancy_grid.hpp" #include "navmap_ros_interfaces/msg/nav_map.hpp" #include "navmap_ros_interfaces/msg/nav_map_layer.hpp" +#include "std_msgs/msg/header.hpp" #include "std_srvs/srv/trigger.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "pcl/point_types.h" @@ -110,6 +111,7 @@ class NavMapMapsManager : public easynav::MapsManagerBase * @brief Publisher for ::navmap::NavMap updates. */ rclcpp::Publisher::SharedPtr layer_updates_pub_; + std::size_t last_subscribers_{0}; ///< Of layer_updates_pub_: a new one gets full layers /** * @brief Subscriber for external incoming static map updates. 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 f6c668a1..5797a790 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,8 +17,12 @@ #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ #define EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__INFLATIONFILTER_HPP_ +#include +#include #include +#include +#include "navmap_core/DistanceField.hpp" #include "navmap_core/NavMap.hpp" #include "easynav_common/types/NavState.hpp" @@ -29,10 +33,20 @@ namespace easynav namespace navmap { +/** + * @class InflationFilter + * @brief "inflated_obstacles": cost by distance (along the surface) to the obstacles of the + * "obstacles" layer and, optionally, to the open edges of the NavMap. + * + * Incremental: an exact distance field (navmap::DistanceField) is updated with the obstacle cells + * that appear or disappear, and only the cells whose distance or obstacle value changed are + * rewritten (and recorded as changed). A new map or obstacle layer starts it over. + */ class InflationFilter : public NavMapFilter { public: InflationFilter(); + ~InflationFilter() override; void on_initialize() override; void update(::easynav::NavState & nav_state) override; @@ -50,10 +64,16 @@ class InflationFilter : public NavMapFilter std::string get_layer_name() override {return "inflated_obstacles";} protected: - ::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 + + // Incremental state, bound to the map (and layers) it was built on + std::unique_ptr<::navmap::DistanceField> df_; + const ::navmap::NavMap * df_map_{nullptr}; + std::uint64_t df_geometry_{0}; + const ::navmap::LayerViewBase * last_src_{nullptr}; + const ::navmap::LayerViewBase * last_dst_{nullptr}; + std::vector lethal_; // obstacle cells the distance field is seeded with }; } // namespace navmap diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp index 32d7f6eb..de9596f2 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/NavMapFilter.hpp @@ -32,6 +32,7 @@ class NavMapFilter { public: NavMapFilter(); + virtual ~NavMapFilter() = default; // plugins are deleted through this base void initialize( diff --git a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp index 20006aa8..e836e888 100644 --- a/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp +++ b/maps_managers/easynav_navmap_maps_manager/include/easynav_navmap_maps_manager/filters/ObstacleFilter.hpp @@ -17,8 +17,10 @@ #ifndef EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__OBSTACLEFILTER_HPP_ #define EASYNAV_NAVMAP_MAPS_MANAGER__FILTERS__OBSTACLEFILTER_HPP_ -#include +#include #include +#include +#include #include "navmap_core/NavMap.hpp" #include "easynav_common/types/NavState.hpp" @@ -37,6 +39,9 @@ namespace navmap * 0.3 m columns; a column is an obstacle when its highest point is more than min_height (plus * min_height_per_meter for each meter from the robot) above the NavMap surface under it. So a 2D * laser sees obstacles too, and the ground or a ramp, being on the surface, is not one. + * + * Incremental: the layer is rebuilt from the static map only when the map changes; each cycle + * writes just the cells that become obstacles or stop being one (recorded as changed). */ class ObstacleFilter : public NavMapFilter { @@ -50,7 +55,12 @@ class ObstacleFilter : public NavMapFilter std::string get_layer_name() override {return "obstacles";} private: - ::navmap::NavMap navmap_; + // Incremental state, bound to the layer it was built on + const ::navmap::LayerViewBase * last_layer_{nullptr}; + std::uint64_t last_occ_hash_{0}; + std::vector base_; // static value of each cell + std::vector<::navmap::NavCelId> marked_; // obstacle cells of the last cycle + std::vector seen_; // 1: in marked_, 2: marked this cycle double max_range_ {10.0}; ///< Points farther than this from the robot (x or y) are ignored. double min_height_ {0.1}; ///< Above the NavMap surface: lower points are the ground. double max_height_ {std::numeric_limits::quiet_NaN()}; ///< Robot frame; NaN: no limit. diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp index e065fdc1..014f1d38 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp @@ -287,15 +287,32 @@ NavMapMapsManager::update(::easynav::NavState & nav_state) map_changed_ = false; } + // Filters update the map in NavState in place, and record which cells they change + const std::size_t subscribers = layer_updates_pub_->get_subscription_count(); + const bool new_subscriber = subscribers > last_subscribers_; + last_subscribers_ = subscribers; + std_msgs::msg::Header header; + header.frame_id = RTTFBuffer::getInstance()->get_tf_info().map_frame; + header.stamp = get_node()->now(); for (const auto & filter : navmap_filters_) { filter->set_map_resolution(resolution_); filter->update(nav_state); + if (subscribers == 0 || !filter->is_adding_layer()) {continue;} const auto & navmap = nav_state.get<::navmap::NavMap>("map.navmap"); + const auto layer = navmap.layers.get(filter->get_layer_name()); + if (!layer || (!layer->has_changes() && !new_subscriber)) {continue;} + // Only the changed cells (or the whole layer if many changed, or for a new subscriber) + layer_updates_pub_->publish( + navmap_ros::to_layer_update_msg( + navmap, filter->get_layer_name(), header, new_subscriber ? 0.0f : 0.25f)); + } - if (filter->is_adding_layer() && navmap.has_layer(filter->get_layer_name())) { - auto update_msg = navmap_ros::to_msg(navmap, filter->get_layer_name()); - layer_updates_pub_->publish(update_msg); + // Every filter and the publisher have seen this cycle's changes + if (nav_state.has("map.navmap")) { + const auto & navmap = nav_state.get<::navmap::NavMap>("map.navmap"); + for (const auto & name : navmap.list_layers()) { + navmap.layers.get(name)->clear_changes(); } } } 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 c94c92a1..eda22032 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 @@ -41,6 +41,7 @@ #include #include +#include #include #include @@ -48,6 +49,7 @@ #include "easynav_common/RobotGeometry.hpp" #include "easynav_common/types/NavState.hpp" +#include "navmap_core/DistanceField.hpp" #include "navmap_core/NavMap.hpp" #include "navmap_ros/conversions.hpp" @@ -59,12 +61,38 @@ namespace easynav namespace navmap { +namespace +{ + +// Cost of a free cell at distance d (m) from the nearest obstacle (or border) +std::uint8_t cost_at(float d, float R, float r_ins, float k) +{ + using navmap_ros::FREE_SPACE; + using navmap_ros::INSCRIBED_INFLATED_OBSTACLE; + if (d <= r_ins) {return INSCRIBED_INFLATED_OBSTACLE;} + if (d > R) {return FREE_SPACE;} + const double base = static_cast(INSCRIBED_INFLATED_OBSTACLE) - 1; + const double c = std::exp(-k * static_cast(d - r_ins)) * base; + return static_cast(std::lround(std::clamp(c, 0.0, base))); +} + +// Obstacles and unknown keep their value (unknown still carries distance); the rest, by distance +std::uint8_t inflated(std::uint8_t src, float d, float R, float r_ins, float k) +{ + if (src == navmap_ros::LETHAL_OBSTACLE || src == navmap_ros::NO_INFORMATION) {return src;} + return cost_at(d, R, r_ins, k); +} + +} // namespace + InflationFilter::InflationFilter() : inflation_radius_(0.0f), cost_scaling_factor_(0.0f), inscribed_radius_(0.0f) {} +InflationFilter::~InflationFilter() = default; + bool InflationFilter::inflate_layer_u8( ::navmap::NavMap & nm, const std::string & src_layer, @@ -75,10 +103,6 @@ bool InflationFilter::inflate_layer_u8( bool inflate_borders) { using ::navmap::NavCelId; - using navmap_ros::FREE_SPACE; - using navmap_ros::INSCRIBED_INFLATED_OBSTACLE; - using navmap_ros::LETHAL_OBSTACLE; - using navmap_ros::NO_INFORMATION; if (nm.navcels.empty() || inflation_radius <= 0.0f || cost_scaling_factor <= 0.0f) { return false; @@ -95,7 +119,7 @@ bool InflationFilter::inflate_layer_u8( dst_layer, nm.navcels.size(), ::navmap::layer_type_tag()); if (!dst_view) {return false;} auto & dst = dst_view->mutable_data(); - dst.resize(nm.navcels.size(), FREE_SPACE); + dst.resize(nm.navcels.size(), navmap_ros::FREE_SPACE); const std::size_t N = nm.navcels.size(); const float R = inflation_radius; @@ -105,26 +129,16 @@ bool InflationFilter::inflate_layer_u8( // map border: an open edge is a wall, a drop or unmapped space. std::vector lethal; for (NavCelId cid = 0; cid < N; ++cid) { - if (src[cid] == LETHAL_OBSTACLE) {lethal.push_back(cid);} + if (src[cid] == navmap_ros::LETHAL_OBSTACLE) {lethal.push_back(cid);} } - const std::vector dist = nm.distance_field(lethal, inflate_borders, R); - - auto cost_from_dist = [&](float d) -> std::uint8_t { - if (d <= r_ins) {return INSCRIBED_INFLATED_OBSTACLE;} - if (d > R) {return FREE_SPACE;} - const double base = static_cast(INSCRIBED_INFLATED_OBSTACLE) - 1; - 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))); - }; + ::navmap::DistanceField df(nm, inflate_borders, R); + df.compute(lethal); 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]); + const std::uint8_t c = inflated(src[cid], df[cid], R, r_ins, cost_scaling_factor); dst[cid] = (dst_layer == src_layer) ? std::max(src[cid], c) : c; } + dst_view->mark_all_changed(); return true; } @@ -162,28 +176,90 @@ InflationFilter::on_initialize() void InflationFilter::update(::easynav::NavState & nav_state) { - if (!nav_state.has("map.navmap")) { - return; - } + using ::navmap::NavCelId; + using LayerU8 = ::navmap::LayerView; + + if (!nav_state.has("map.navmap")) {return;} + // In place: the map in NavState is updated, not copied + const auto map = nav_state.get_ptr<::navmap::NavMap>("map.navmap"); + auto & nm = *map; + const std::size_t N = nm.navcels.size(); + if (N == 0) {return;} // no map yet (e.g. a local map before its first build) + if (inflation_radius_ <= 0.0 || cost_scaling_factor_ <= 0.0) {return;} - navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); - if (navmap_.navcels.empty()) {return;} // no map yet (e.g. a local map before its first build) + const auto src = std::dynamic_pointer_cast(nm.layers.get("obstacles")); + if (!src || src->size() != N) {return;} // the obstacle filter has not run yet - const bool ok = inflate_layer_u8( - navmap_, - "obstacles", - "inflated_obstacles", - inflation_radius_, - cost_scaling_factor_, - inscribed_radius_, - inflate_borders_); + const float R = static_cast(inflation_radius_); + const float r_ins = std::clamp(static_cast(inscribed_radius_), 0.0f, R); + const float k = static_cast(cost_scaling_factor_); + auto dst = std::dynamic_pointer_cast(nm.layers.get(get_layer_name())); - if (!ok) { - RCLCPP_ERROR(get_node()->get_logger(), "InflationFilter: inflate_layer_u8() failed"); + // A new map, a new obstacle layer, or our layer gone or replaced (a new layer is all changed): + // start over + const bool rebuild = !df_ || df_map_ != &nm || df_geometry_ != nm.geometry_id() || + last_src_ != src.get() || !dst || dst.get() != last_dst_ || dst->size() != N || + dst->all_changed(); + if (rebuild) { + if (!dst || dst->size() != N) { + nm.layers.remove(get_layer_name()); + dst = nm.add_layer( + get_layer_name(), "cost by distance to obstacles and borders", "", 0); + } + lethal_.assign(N, 0); + std::vector seeds; + for (NavCelId c = 0; c < N; ++c) { + if ((*src)[c] == navmap_ros::LETHAL_OBSTACLE) {lethal_[c] = 1; seeds.push_back(c);} + } + df_ = std::make_unique<::navmap::DistanceField>(nm, inflate_borders_, R); + df_->compute(seeds); + auto & d = dst->data(); + for (NavCelId c = 0; c < N; ++c) { + d[c] = inflated((*src)[c], (*df_)[c], R, r_ins, k); + } + dst->mark_all_changed(); + df_map_ = &nm; + df_geometry_ = nm.geometry_id(); + last_src_ = src.get(); + last_dst_ = dst.get(); return; } - nav_state.set("map.navmap", navmap_); + // Only what changed: the obstacle cells that came or went move the distance field, and the + // cells whose distance or source value changed get a new cost + std::vector touched; + if (src->all_changed()) { + touched.resize(N); + for (NavCelId c = 0; c < N; ++c) { + touched[c] = c; + } + } else { + touched = src->changed(); + } + std::vector added, removed; + for (const NavCelId c : touched) { + const char now = (*src)[c] == navmap_ros::LETHAL_OBSTACLE; + if (now == lethal_[c]) {continue;} + lethal_[c] = now; + (now ? added : removed).push_back(c); + } + if (!added.empty() || !removed.empty()) {df_->update(added, removed);} + + auto refresh = [&](NavCelId c) { + const std::uint8_t v = inflated((*src)[c], (*df_)[c], R, r_ins, k); + if ((*dst)[c] != v) { + (*dst)[c] = v; + dst->mark_changed(c); + } + }; + if (!added.empty() || !removed.empty()) { + for (const NavCelId c : df_->changed()) { + refresh(c); + } + } + for (const NavCelId c : touched) { + refresh(c); + } } } // namespace navmap diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp index d3621319..ef99225e 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/ObstacleFilter.cpp @@ -16,8 +16,10 @@ #include #include +#include #include #include +#include #include "easynav_common/Parameters.hpp" #include "easynav_common/types/NavState.hpp" @@ -74,24 +76,44 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) return; } - navmap_ = nav_state.get<::navmap::NavMap>("map.navmap"); + // In place: the map in NavState is updated, not copied + const auto map = nav_state.get_ptr<::navmap::NavMap>("map.navmap"); + auto & nm = *map; + const std::size_t N = nm.navcels.size(); const auto & tf_info = RTTFBuffer::getInstance()->get_tf_info(); // The NavMap's own frame (a local NavMap is in the robot frame), the map frame by default - const std::string map_frame = !navmap_.surfaces.empty() && !navmap_.surfaces[0].frame_id.empty() ? - navmap_.surfaces[0].frame_id : tf_info.map_frame; - - // Start from the static map, if the NavMap has one (built from an occupancy grid): what the - // sensors do not see right now (behind something, far, too low) is still an obstacle. - navmap_.layer_clear(get_layer_name(), navmap_ros::FREE_SPACE); - if (navmap_.has_layer("occupancy")) { - for (std::size_t c = 0; c < navmap_.navcels.size(); ++c) { - const auto cid = static_cast<::navmap::NavCelId>(c); - const auto v = navmap_.layer_get("occupancy", cid, navmap_ros::FREE_SPACE); - if (v != navmap_ros::FREE_SPACE) { - navmap_.layer_set(get_layer_name(), cid, v); + const std::string map_frame = !nm.surfaces.empty() && !nm.surfaces[0].frame_id.empty() ? + nm.surfaces[0].frame_id : tf_info.map_frame; + + // The layer starts from the static map, if the NavMap has one (built from an occupancy grid): + // what the sensors do not see right now (behind something, far, too low) is still an obstacle. + // It is rebuilt only for a new map (or static layer); then, each cycle, only the cells seen as + // obstacles now or in the last cycle are written. + const auto occ = nm.layers.get("occupancy"); + const std::uint64_t occ_hash = occ && occ->size() == N ? occ->content_hash() : 0; + auto layer = nm.layers.get(get_layer_name()); + if (!layer || layer.get() != last_layer_ || layer->size() != N || occ_hash != last_occ_hash_ || + layer->all_changed()) + { + nm.layers.remove(get_layer_name()); + base_.assign(N, navmap_ros::FREE_SPACE); + if (occ_hash != 0) { + for (std::size_t c = 0; c < N; ++c) { + const auto cid = static_cast<::navmap::NavCelId>(c); + base_[c] = nm.layer_get("occupancy", cid, navmap_ros::FREE_SPACE); } } + nm.add_layer( + get_layer_name(), "obstacles: static map plus what the sensors see", "", + navmap_ros::FREE_SPACE)->set_data(base_); + layer = nm.layers.get(get_layer_name()); + last_layer_ = layer.get(); + last_occ_hash_ = occ_hash; + marked_.clear(); + seen_.assign(N, 0); } + auto & values = std::dynamic_pointer_cast<::navmap::LayerView>(layer)->data(); + std::vector<::navmap::NavCelId> marked; // Only the points around the robot and below its height (robot frame), downsampled first // (indices only) so that fewer points are transformed. @@ -138,15 +160,15 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) if (last_surface) {opts.hint_surface = *last_surface;} if (last_cid) {opts.hint_cid = *last_cid;} - bool ok = navmap_.locate_navcel(query, surface_idx, cid, bary, &hit, opts); + bool ok = nm.locate_navcel(query, surface_idx, cid, bary, &hit, opts); if (!ok) { ::navmap::NavMap::LocateOpts nohint; nohint.use_downward_ray = true; nohint.height_eps = 0.50f; - ok = navmap_.locate_navcel(query, surface_idx, cid, bary, &hit, nohint); + ok = nm.locate_navcel(query, surface_idx, cid, bary, &hit, nohint); if (!ok) { - ok = navmap_.locate_navcel(query, surface_idx, cid, bary, &hit); + ok = nm.locate_navcel(query, surface_idx, cid, bary, &hit); } } if (!ok) {continue;} @@ -154,19 +176,36 @@ void ObstacleFilter::update(::easynav::NavState & nav_state) last_cid = cid; // Height above the NavCel's plane (along its upward normal): on a ramp, the ramp is not one - const auto & cel = navmap_.navcels[cid]; + const auto & cel = nm.navcels[cid]; Eigen::Vector3f normal = cel.normal; if (normal.z() < 0.0f) {normal = -normal;} - const float height = normal.dot(query - navmap_.positions.at(cel.v[0])); + const float height = normal.dot(query - nm.positions.at(cel.v[0])); // Farther, a small attitude error lifts the ground more: a higher threshold const double distance = robot_known ? (query.head<2>() - robot_xy).norm() : 0.0; if (!(height > min_height_ + min_height_per_meter_ * distance)) {continue;} - navmap_.layer_set( - get_layer_name(), cid, static_cast(navmap_ros::LETHAL_OBSTACLE)); + if (seen_[cid] != 2) { + seen_[cid] = 2; + marked.push_back(cid); + } } - nav_state.set("map.navmap", navmap_); + // Obstacles now; cells that were obstacles in the last cycle and are not now go back to the + // static map. Only cells whose value changes are written (and recorded as changed). + for (const auto cid : marked) { + if (values[cid] != navmap_ros::LETHAL_OBSTACLE) { + nm.layer_set(get_layer_name(), cid, navmap_ros::LETHAL_OBSTACLE); + } + } + for (const auto cid : marked_) { + if (seen_[cid] == 2) {continue;} + seen_[cid] = 0; + if (values[cid] != base_[cid]) {nm.layer_set(get_layer_name(), cid, base_[cid]);} + } + for (const auto cid : marked) { + seen_[cid] = 1; + } + marked_.swap(marked); } diff --git a/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt index ffb49a32..efc3ca96 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt @@ -15,3 +15,6 @@ target_link_libraries(navmap_inflation_filter_tests ${PROJECT_NAME}) ament_add_gtest(navmap_local_mode_tests navmap_local_mode_tests.cpp) target_link_libraries(navmap_local_mode_tests ${PROJECT_NAME}) + +ament_add_gtest(navmap_incremental_filters_tests navmap_incremental_filters_tests.cpp) +target_link_libraries(navmap_incremental_filters_tests ${PROJECT_NAME}) diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_incremental_filters_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_incremental_filters_tests.cpp new file mode 100644 index 00000000..746e5239 --- /dev/null +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_incremental_filters_tests.cpp @@ -0,0 +1,388 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// This file is part of the project Easy Navigation (EasyNav in short) +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// The filters update the map in NavState in place and write only what changes; the maps manager +// publishes only the changed cells. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_common/types/NavState.hpp" +#include "easynav_navmap_maps_manager/NavMapMapsManager.hpp" +#include "easynav_navmap_maps_manager/filters/InflationFilter.hpp" +#include "easynav_navmap_maps_manager/filters/ObstacleFilter.hpp" +#include "easynav_sensors/types/PointPerception.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "navmap_core/NavMap.hpp" +#include "navmap_ros/conversions.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using ::navmap::NavCelId; +using ::navmap::NavMap; + +namespace +{ + +constexpr uint8_t LETHAL = navmap_ros::LETHAL_OBSTACLE; +constexpr uint8_t FREE = navmap_ros::FREE_SPACE; +constexpr uint8_t UNKNOWN = navmap_ros::NO_INFORMATION; + +// side_cells x side_cells at 0.1 m, free, with an occupied cell at (1.55, 0.55) +nav_msgs::msg::OccupancyGrid grid(int side_cells = 40) +{ + nav_msgs::msg::OccupancyGrid g; + g.header.frame_id = "map"; + g.info.resolution = 0.1; + g.info.width = side_cells; + g.info.height = side_cells; + g.info.origin.orientation.w = 1.0; + g.data.assign(side_cells * side_cells, 0); + g.data[5 * side_cells + 15] = 100; + return g; +} + +std::uint8_t at(const NavMap & nm, const std::string & layer, float x, float y) +{ + std::size_t s; + NavCelId c; + Eigen::Vector3f b, h; + EXPECT_TRUE(nm.locate_navcel(Eigen::Vector3f(x, y, 0.05f), s, c, b, &h)); + return nm.layer_get(layer, c, 123); +} + +NavCelId cell(const NavMap & nm, float x, float y) +{ + std::size_t s; + NavCelId c = 0; + Eigen::Vector3f b, h; + nm.locate_navcel(Eigen::Vector3f(x, y, 0.05f), s, c, b, &h); + return c; +} + +std::set changed(const NavMap & nm, const std::string & layer) +{ + const auto & v = nm.layers.get(layer)->changed(); + return {v.begin(), v.end()}; +} + +void clear_changes(const NavMap & nm) +{ + for (const auto & n : nm.list_layers()) { + nm.layers.get(n)->clear_changes(); + } +} + +// Exposes the inscribed radius the filter took from the robot geometry +struct Inflation : easynav::navmap::InflationFilter +{ + float inscribed() const {return static_cast(inscribed_radius_);} +}; + +class IncrementalObstaclesTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} + static int count = 0; + node_ = std::make_shared( + "incremental_obstacles_" + std::to_string(count++)); + auto tf_buffer = easynav::RTTFBuffer::getInstance(); + geometry_msgs::msg::TransformStamped tf; + tf.header.frame_id = tf_buffer->get_tf_info().map_frame; + tf.child_frame_id = tf_buffer->get_tf_info().robot_frame; + tf.transform.translation.x = 2.0; + tf.transform.translation.y = 2.0; + tf.transform.rotation.w = 1.0; + tf_buffer->setTransform(tf, "test", true); + nav_state_.set("map.navmap", navmap_ros::from_occupancy_grid(grid())); + filter_.initialize(node_, "navmap.obstacles"); + } + + // One cycle seeing a 0.5 m high column at each (x, y) (cell centers), and nothing else + const NavMap & see(const std::vector> & cols) + { + easynav::PointPerception p; + p.frame_id = "map"; + p.stamp = node_->now(); + p.valid = true; + for (const auto & [x, y] : cols) { + for (float z = 0.2f; z <= 0.5f; z += 0.1f) {p.data.push_back(pcl::PointXYZ(x, y, z));} + } + nav_state_.set("scan", p); + filter_.update(nav_state_); + return nav_state_.get("map.navmap"); + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + easynav::NavState nav_state_; + easynav::navmap::ObstacleFilter filter_; +}; + +} // namespace + +TEST_F(IncrementalObstaclesTest, AMovingObstacleChangesOnlyItsCells) +{ + const auto & nm = see({{1.03f, 2.03f}}); + EXPECT_EQ(at(nm, "obstacles", 1.03f, 2.03f), LETHAL); + EXPECT_EQ(at(nm, "obstacles", 1.55f, 0.55f), LETHAL); // the static map + EXPECT_TRUE(nm.layers.get("obstacles")->all_changed()); // first cycle: a new layer + clear_changes(nm); + + // It moves: the old cell goes back to free, the new one is marked, nothing else is written + const NavCelId old_c = cell(nm, 1.03f, 2.03f), new_c = cell(nm, 3.03f, 2.03f); + see({{3.03f, 2.03f}}); + EXPECT_EQ(at(nm, "obstacles", 1.03f, 2.03f), FREE); + EXPECT_EQ(at(nm, "obstacles", 3.03f, 2.03f), LETHAL); + EXPECT_EQ(at(nm, "obstacles", 1.55f, 0.55f), LETHAL); + EXPECT_FALSE(nm.layers.get("obstacles")->all_changed()); + EXPECT_EQ(changed(nm, "obstacles"), (std::set{old_c, new_c})); + clear_changes(nm); + + // The same scene again: nothing changes + see({{3.03f, 2.03f}}); + EXPECT_FALSE(nm.layers.get("obstacles")->has_changes()); + // Nothing seen: the obstacle goes, the static map stays + see({}); + EXPECT_EQ(changed(nm, "obstacles"), (std::set{new_c})); + EXPECT_EQ(at(nm, "obstacles", 3.03f, 2.03f), FREE); + EXPECT_EQ(at(nm, "obstacles", 1.55f, 0.55f), LETHAL); +} + +TEST_F(IncrementalObstaclesTest, SeenObstaclesOnTheStaticMapLeaveItAsItWas) +{ + const auto & nm = see({{1.55f, 0.55f}, {1.53f, 0.57f}}); // on the static obstacle + clear_changes(nm); + see({}); + EXPECT_EQ(at(nm, "obstacles", 1.55f, 0.55f), LETHAL); + EXPECT_FALSE(nm.layers.get("obstacles")->has_changes()); // was lethal, still is +} + +TEST_F(IncrementalObstaclesTest, ANewMapRebuildsTheLayer) +{ + see({{1.03f, 2.03f}}); + // A new map with another static obstacle, same geometry + auto g = grid(); + g.data[5 * 40 + 15] = 0; + g.data[30 * 40 + 30] = 100; // (3.05, 3.05) + nav_state_.set("map.navmap", navmap_ros::from_occupancy_grid(g)); + const auto & nm = see({}); + EXPECT_EQ(at(nm, "obstacles", 1.55f, 0.55f), FREE); + EXPECT_EQ(at(nm, "obstacles", 3.05f, 3.03f), LETHAL); + EXPECT_EQ(at(nm, "obstacles", 1.03f, 2.03f), FREE); + EXPECT_TRUE(nm.layers.get("obstacles")->all_changed()); + // A smaller map (other geometry) + nav_state_.set("map.navmap", navmap_ros::from_occupancy_grid(grid(30))); + const auto & small = see({{1.03f, 2.03f}}); + EXPECT_EQ(small.layer_size("obstacles"), small.navcels.size()); + EXPECT_EQ(at(small, "obstacles", 1.03f, 2.03f), LETHAL); + EXPECT_EQ(at(small, "obstacles", 1.55f, 0.55f), LETHAL); +} + +TEST(IncrementalInflation, MatchesAFullRecomputationAfterEveryChange) +{ + if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} + auto node = std::make_shared("incremental_inflation"); + node->declare_parameter("navmap.inflation.inflation_radius", 0.6); + node->declare_parameter("navmap.inflation.cost_scaling_factor", 3.0); + Inflation filter; + filter.initialize(node, "navmap.inflation"); + + // A 4 x 4 m map (its border inflates too) with a static block in the middle + auto g = grid(); + for (int j = 18; j < 22; ++j) { + for (int i = 18; i < 22; ++i) { + g.data[j * 40 + i] = 100; + } + } + NavMap base = navmap_ros::from_occupancy_grid(g); + easynav::NavState nav_state; + nav_state.set("map.navmap", base); + auto map = nav_state.get_ptr("map.navmap"); + auto obst = map->add_layer("obstacles", "", "", FREE); + const std::size_t N = map->navcels.size(); + + std::mt19937 rng(7); + std::uniform_int_distribution C(0, static_cast(N - 1)); + std::uniform_int_distribution V(0, 9); + for (int step = 0; step < 40; ++step) { + // Some cells become obstacles, unknown or free (a cluster and scattered ones) + const int n = step == 0 ? 0 : 1 + V(rng) * 3; + for (int k = 0; k < n; ++k) { + const NavCelId c = C(rng); + const int v = V(rng); + map->layer_set("obstacles", c, v < 5 ? LETHAL : (v < 7 ? UNKNOWN : FREE)); + } + const std::vector before = map->has_layer("inflated_obstacles") ? + std::dynamic_pointer_cast<::navmap::LayerView>( + map->layers.get("inflated_obstacles"))->data() : std::vector{}; + filter.update(nav_state); + + NavMap ref = *map; + ASSERT_TRUE(filter.inflate_layer_u8(ref, "obstacles", "ref", 0.6f, 3.0f, filter.inscribed(), + true)); + const auto inc = map->layers.get("inflated_obstacles"); + ASSERT_TRUE(inc); + std::set differ; + for (NavCelId c = 0; c < N; ++c) { + ASSERT_EQ(map->layer_get("inflated_obstacles", c), ref.layer_get("ref", c)) + << "step " << step << " cell " << c; + if (!before.empty() && before[c] != map->layer_get("inflated_obstacles", c)) { + differ.insert(c); + } + } + // Exactly the cells that changed are recorded (after the first, full, cycle) + if (step > 0) { + EXPECT_FALSE(inc->all_changed()) << step; + EXPECT_EQ(changed(*map, "inflated_obstacles"), differ) << step; + } + clear_changes(*map); + } +} + +TEST(IncrementalInflation, ANewMapOrObstacleLayerStartsOver) +{ + if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} + auto node = std::make_shared("incremental_inflation_new"); + node->declare_parameter("navmap.inflation.inflation_radius", 0.5); + Inflation filter; + filter.initialize(node, "navmap.inflation"); + easynav::NavState nav_state; + // No obstacle layer yet: nothing, quietly + nav_state.set("map.navmap", navmap_ros::from_occupancy_grid(grid())); + filter.update(nav_state); + EXPECT_FALSE(nav_state.get("map.navmap").has_layer("inflated_obstacles")); + + for (const int side : {40, 30, 40}) { + nav_state.set("map.navmap", navmap_ros::from_occupancy_grid(grid(side))); + auto map = nav_state.get_ptr("map.navmap"); + map->add_layer("obstacles", "", "", FREE); + map->layer_set("obstacles", cell(*map, 1.03f, 2.03f), LETHAL); + filter.update(nav_state); + NavMap ref = *map; + filter.inflate_layer_u8(ref, "obstacles", "ref", 0.5f, 3.0f, filter.inscribed(), true); + for (NavCelId c = 0; c < map->navcels.size(); ++c) { + ASSERT_EQ(map->layer_get("inflated_obstacles", c), ref.layer_get("ref", c)) + << side; + } + EXPECT_TRUE(map->layers.get("inflated_obstacles")->all_changed()); + clear_changes(*map); + } +} + +TEST(IncrementalMapsManager, PublishesOnlyTheChangedCells) +{ + if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} + auto node = std::make_shared("incremental_mapsmanager"); + node->declare_parameter("navmap.filters", std::vector{"obstacles"}); + node->declare_parameter( + "navmap.obstacles.plugin", + std::string("easynav_navmap_maps_manager/NavMapMapsManager/ObstaclesFilter")); + auto tf_buffer = easynav::RTTFBuffer::getInstance(); + geometry_msgs::msg::TransformStamped tf; + tf.header.frame_id = tf_buffer->get_tf_info().map_frame; + tf.child_frame_id = tf_buffer->get_tf_info().robot_frame; + tf.transform.translation.x = 2.0; + tf.transform.translation.y = 2.0; + tf.transform.rotation.w = 1.0; + tf_buffer->setTransform(tf, "test", true); + + auto mm = std::make_shared(); + mm->initialize(node, "navmap"); + easynav::NavState nav_state; + mm->update(nav_state); // no map of its own: it publishes its empty one first + nav_state.set("map.navmap", navmap_ros::from_occupancy_grid(grid())); + auto scan = [&](std::vector> cols) { + easynav::PointPerception p; + p.frame_id = "map"; + p.stamp = node->now(); + p.valid = true; + for (const auto & [x, y] : cols) { + for (float z = 0.2f; z <= 0.5f; z += 0.1f) { + p.data.push_back(pcl::PointXYZ(x, y, z)); + } + } + nav_state.set("scan", p); + }; + + // Nobody listens: nothing is published, but changes are still cleared every cycle + scan({{1.03f, 2.03f}}); + mm->update(nav_state); + EXPECT_FALSE(nav_state.get("map.navmap").layers.get("obstacles")->has_changes()); + + const std::string topic = "/incremental_mapsmanager/navmap/map_updates"; + std::vector got; + auto listener = std::make_shared("incremental_listener"); + auto sub = listener->create_subscription( + topic, rclcpp::QoS(100), + [&](navmap_ros_interfaces::msg::NavMapLayer::SharedPtr m) {got.push_back(*m);}); + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(listener); + auto spin = [&]() { + for (int i = 0; i < 20; ++i) { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + }; + for (int i = 0; i < 100 && node->count_subscribers(topic) == 0; ++i) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + const auto & nm = nav_state.get("map.navmap"); + const std::size_t N = nm.navcels.size(); + ASSERT_EQ(N, 3200u); + + // A new subscriber gets the whole layer, even without changes + ASSERT_GT(node->count_subscribers(topic), 0u); + mm->update(nav_state); + spin(); + ASSERT_EQ(got.size(), 1u); + EXPECT_EQ(got[0].name, "obstacles"); + EXPECT_TRUE(got[0].indices.empty()); + EXPECT_EQ(got[0].data_u8.size(), N); + EXPECT_EQ(got[0].geometry_id, nm.geometry_id()); + const auto full = got[0]; + + // No changes: nothing + got.clear(); + mm->update(nav_state); + spin(); + EXPECT_TRUE(got.empty()); + + // The obstacle moves: only its two cells + scan({{3.03f, 2.03f}}); + mm->update(nav_state); + spin(); + ASSERT_EQ(got.size(), 1u); + const std::set idx(got[0].indices.begin(), got[0].indices.end()); + EXPECT_EQ(idx, (std::set{cell(nm, 1.03f, 2.03f), cell(nm, 3.03f, 2.03f)})); + ASSERT_EQ(got[0].data_u8.size(), 2u); + // A receiver applying the full layer and then the update ends with the same layer + NavMap mirror = navmap_ros::from_occupancy_grid(grid()); + navmap_ros::from_msg(full, mirror); + navmap_ros::from_msg(got[0], mirror); + for (NavCelId c = 0; c < N; ++c) { + ASSERT_EQ(mirror.layer_get("obstacles", c), nm.layer_get("obstacles", c)); + } +}