Demo Nav2 scan observations in the layered global map

discourse/3-layered-global-occupancy-map.md

@@ -2,8 +2,9 @@
 
 # Layered Global Map Observations
 
-This post describes the observation interface for contributing temporary map
-information to the next generation Open-RMF prototype.
+This post describes the implemented observation interface and demos for
+contributing temporary map information to the next generation Open-RMF
+prototype.
 
 The goal is to let robots and perception systems publish what they currently
 observe without tying them to a specific central map implementation. The first
@@ -19,15 +20,20 @@ observations, can be added later.
   same sensor snapshot
 * Clear-space patches have TTLs, just like obstacle patches
 * A source can reset its previous observations without using a TTL
-* Robot-mounted sources can include the robot pose at observation time
+* Robot-mounted sources include the observation-frame pose at observation time
+* The Rust map server composes a static occupancy grid and active observations
+  into `/map`
+* The Nav2 demo converts scans from three robots into clear ray sectors and occupied endpoint regions, then displays the source contributions and map
 * The observation messages live in `rmf_layered_map_msgs`, leaving
   `rmf_prototype_msgs` unchanged
 
-# Observation Topic
+# Observation Topics
 
-Observation sources publish dynamic map observations on:
+The layered map server uses these topics:
 
+* `/map/static` - static `nav_msgs/OccupancyGrid`
 * `/map/region_updates` - [`MapRegionUpdate.msg`](../rmf_layered_map_msgs/msg/MapRegionUpdate.msg)
+* `/map` - composed `nav_msgs/OccupancyGrid`
 
 The topic is an event stream. Updates are not latched because expired
 observations should not be replayed to a restarted map service as if they were
@@ -105,6 +111,28 @@ sensor-local regions with their sensor pose, while synthetic sources whose
 regions are already global can use the identity pose. The first implementation
 accepts point and axis-aligned rectangle regions.
 
+# Layered Map Server
+
+`rmf_layered_map_server` keeps the static occupancy grid separate from dynamic
+observations and publishes their composition on `/map`. It validates source
+timestamps and frames, ignores updates that are older than the latest accepted
+update from the same source, supports source resets, and removes observations
+after their TTL expires.
+
+The server applies clear patches before obstacle patches from the same update.
+During composition, obstacle observations win over clear observations so
+occupied space is not accidentally erased by another active source.
+
+# Three-Robot Nav2 Demo
+
+The launch file starts three robots in different free corners of the warehouse, cycles them through fixed Nav2 goals, and spawns one deterministic Gazebo box near each robot. Each observation node subscribes to its robot's local `sensor_msgs/LaserScan`, filters invalid or out-of-range returns, and publishes free-space ray sectors as convex polygons and occupied endpoints as point regions.
+
+Each scan adds temporary clear and obstacle patches to a rolling observation history. The map server rasterizes clear sectors before obstacle endpoints so a measured hit remains occupied. Active obstacle evidence still wins over clear evidence until its TTL expires. An update may set `reset_source` so the new scan replaces all active observations from that source instead of being added to its history. The observation-frame pose is recorded in the shared `map` frame so the map server can transform the scan-local regions before rasterizing them.
+
+The demo launches Nav2 localization, planning, and control for the fixed goal loops, but does not launch RMF planning. Robot-local RViz windows show Nav2 state, while another RViz window displays the combined global `/map`. The combined view overlays incoming region updates as colored markers grouped by source, with the same retention behavior as the map contributions.
+
+The scan-to-region conversion uses one convex sector per sampled beam instead of expanding a Bresenham line into many point regions. Scan sampling reduces the number of sectors, publication throttling limits the snapshot rate, and the observation range limits represented returns. The TTL controls how quickly stale observations expire. Each publisher logs its input beams and clear and obstacle regions so message density, update rate, and visual fidelity can be compared. The current demo remains a 2D occupancy approximation; it does not fuse probabilistic confidence or compress adjacent sectors into larger regions.
+
 # Example Flow
 
 A local costmap or LiDAR observation node can publish a replacement snapshot by:
@@ -118,22 +146,27 @@ A local costmap or LiDAR observation node can publish a replacement snapshot by:
 5. Giving each patch a TTL long enough to survive normal publication jitter but
    short enough to decay when the observation is no longer refreshed.
 
-The map service should ignore snapshots from a source if their timestamp is
-older than a newer snapshot that has already been accepted. This prevents a late
-clear/reset message from removing obstacle information that came from a newer
+The map server ignores snapshots from a source if their timestamp is older than
+a newer snapshot that has already been accepted. This prevents a late clear or
+reset message from removing obstacle information that came from a newer
 observation.
 
 # Implemented Test Coverage
 
-The first server tests cover:
+The committed server and demo tests cover:
 
 * composing obstacle regions over a static planning grid
 * point regions with non-zero map origins and non-1.0 resolutions
 * transforming robot-local regions into the global map frame
 * out-of-bounds regions, malformed point arrays, and unsupported region types
-* rejecting updates without a timestamp
+* rejecting updates without a timestamp or in a different global frame
 * pruning expired observations by TTL
 * clear and obstacle patches in the same update
 * late older snapshots being ignored
 * reset updates removing observations from the same source and map
 * multiple robot sources being stitched into one composed grid
+* converting laser beams into clear sectors and occupied endpoints
+* filtering invalid and out-of-range laser returns
+* preserving original beam angles when scan points are sampled
+* converting point and rectangle updates into source-colored markers
+* retaining markers until TTL expiry and replacing them on source reset

map_server/README.md

@@ -11,3 +11,5 @@ ros2 launch rmf_layered_map_server_demo demo.launch.py
 ```
 
 Use `use_rviz:=False` for a headless run.
+
+See [`rmf_layered_map_server_demo/README.md`](rmf_layered_map_server_demo/README.md) for the three-robot Nav2 observation demo.

map_server/rmf_layered_map_server/src/lib.rs

@@ -494,9 +494,14 @@ fn region_validation_error(region: &Region) -> Option<String> {
         Region::HINT_AXIS_ALIGNED_RECTANGLE if region.points.len() < 4 => {
             Some("axis-aligned rectangle must contain at least two x/y pairs".to_string())
         }
-        Region::HINT_POINT | Region::HINT_AXIS_ALIGNED_RECTANGLE => None,
+        Region::HINT_CONVEX_POLYGON if region.points.len() < 6 => {
+            Some("convex polygon must contain at least three x/y pairs".to_string())
+        }
+        Region::HINT_POINT
+        | Region::HINT_AXIS_ALIGNED_RECTANGLE
+        | Region::HINT_CONVEX_POLYGON => None,
         hint => Some(format!(
-            "unsupported region hint {}; expected a point or axis-aligned rectangle",
+            "unsupported region hint {}; expected a point, axis-aligned rectangle, or convex polygon",
             hint
         )),
     }
@@ -833,6 +838,13 @@ mod tests {
         }
     }
 
+    fn convex_polygon(points: Vec<f32>) -> Region {
+        Region {
+            hint: Region::HINT_CONVEX_POLYGON,
+            points,
+        }
+    }
+
     fn patch(update_type: u8, regions: Vec<Region>) -> MapRegionPatch {
         MapRegionPatch {
             update_type,
@@ -896,6 +908,24 @@ mod tests {
         assert_eq!(composed.data[0], 0);
     }
 
+    #[test]
+    fn convex_clear_regions_are_composed_over_static_map() {
+        let mut map = LayeredMap::default();
+        map.set_static_map(static_grid(5, 5, 100));
+
+        assert!(map.ingest_region_update(
+            update(
+                MapRegionPatch::UPDATE_CLEAR,
+                vec![convex_polygon(vec![0.0, 0.0, 4.0, 0.0, 4.0, 4.0])]
+            ),
+            0,
+        ));
+
+        let composed = map.compose().unwrap();
+        assert_eq!(composed.data[1 * 5 + 3], 0);
+        assert_eq!(composed.data[3 * 5 + 1], 100);
+    }
+
     #[test]
     fn robot_local_regions_are_transformed_into_the_map_frame() {
         let mut map = LayeredMap::default();

map_server/rmf_layered_map_server_demo/README.md

@@ -0,0 +1,33 @@
+# Layered Map Server Demos
+
+This package contains two demonstrations of the layered map server.
+
+## TTL Smoke Test
+
+This demo publishes a synthetic static grid and a temporary rectangle with a five-second TTL:
+
+```bash
+ros2 launch rmf_layered_map_server_demo demo.launch.py
+```
+
+Use `use_rviz:=False` for a headless run.
+
+## Three-Robot Nav2 Observation Demo
+
+This demo combines laser observations from three moving Nav2 robots in the global `/map`:
+
+```bash
+ros2 launch rmf_layered_map_server_demo nav2_observations.launch.py
+```
+
+![Three robot laser observations in the combined global map](docs/images/nav2_observations.png)
+
+By default, the demo opens three robot-local RViz windows and one combined-map view, moves the robots between fixed goals, spawns demo obstacles, and retains scans for ten seconds.
+
+* Set `use_nav2_rviz:=False` or `use_global_rviz:=False` to disable either RViz view.
+* Set `move_robots:=False` to disable robot movement.
+* Set `spawn_clutter:=False` to use the unmodified warehouse world.
+* Set `reset_source:=True` to show only the latest scan from each robot.
+* Use `map` and `params_file` to override the warehouse map and shared Nav2 parameters.
+* `beam_stride:=1` and `publish_period_sec:=0.5` control scan sampling.
+* `max_observation_range:=2.5` and `ttl_sec:=10.0` control range and retention.