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)); + } +}