NavMap filters: incremental, in place, and sparse layer updates

maps_managers/easynav_navmap_maps_manager/README.md

@@ -63,11 +63,12 @@ With `<plugin>.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 `<plugin>.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,14 +96,16 @@ With `<plugin>.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
 
 | Direction | Topic | Type | Purpose | QoS |
 |---|---|---|---|---|
 | Publisher | `<node_fqn>/<plugin>/map` | `navmap_ros_interfaces/msg/NavMap` | Publishes the full NavMap. | depth=1 |
-| Publisher | `<node_fqn>/<plugin>/map_updates` | `navmap_ros_interfaces/msg/NavMapLayer` | Publishes incremental layer updates. | depth=100 |
+| Publisher | `<node_fqn>/<plugin>/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 | `<node_fqn>/<plugin>/incoming_occ_map` | `nav_msgs/msg/OccupancyGrid` | Input occupancy grid to import into NavMap. | depth=1, transient_local, reliable |
 | Subscription | `<node_fqn>/<plugin>/incoming_pc2_map` | `sensor_msgs/msg/PointCloud2` | Input point cloud to build/update NavMap. | depth=100 |
 

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<navmap_ros_interfaces::msg::NavMapLayer>::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.

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 <cstdint>
+#include <memory>
 #include <string>
+#include <vector>
 
+#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<char> lethal_;  // obstacle cells the distance field is seeded with
 };
 
 }  // namespace navmap

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(

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 <string>
+#include <cstdint>
 #include <limits>
+#include <string>
+#include <vector>
 
 #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<uint8_t> base_;                 // static value of each cell
+  std::vector<::navmap::NavCelId> marked_;    // obstacle cells of the last cycle
+  std::vector<uint8_t> 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<double>::quiet_NaN()};  ///< Robot frame; NaN: no limit.

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