demo: combine Nav2 scan observations

Author: SamuelFoo

map_server/README.md

@@ -11,3 +11,18 @@ ros2 launch rmf_layered_map_server_demo demo.launch.py
 ```
 
 Use `use_rviz:=False` for a headless run.
+
+## Nav2 Observations
+
+The three-robot demo converts local laser observations into point regions and
+combines them into the global `/map`. It launches three robots in warehouse
+corners, deterministic clutter for each robot to observe, the layered map
+server, and a separate RViz window for the combined map. It does not launch any
+RMF planning components.
+
+```bash
+ros2 launch rmf_layered_map_server_demo nav2_observations.launch.py
+```
+
+See [`rmf_layered_map_server_demo/README.md`](rmf_layered_map_server_demo/README.md)
+for launch arguments and the current scan-to-region limitations.

map_server/rmf_layered_map_server/README.md

@@ -43,3 +43,16 @@ ros2 launch rmf_layered_map_server_demo demo.launch.py
 ```
 
 The launch file starts RViz with a `Map` display on `/map` and the fixed frame set to `map`. The demo publisher sends a bordered static map and a temporary obstacle region with a 5 second TTL. The obstacle should appear in the composed map and then disappear when the TTL expires.
+
+## Nav2 Observation Demo
+
+To combine observations from three Nav2 robots without starting RMF planning,
+launch:
+
+```bash
+ros2 launch rmf_layered_map_server_demo nav2_observations.launch.py
+```
+
+Each robot converts valid laser endpoints into point regions. A separate RViz
+window displays the composed `/map`; see the demo package README for the
+fidelity and publication-rate tuning arguments.

map_server/rmf_layered_map_server_demo/README.md

@@ -0,0 +1,50 @@
+# Layered Map Server Demos
+
+This package contains two demonstrations of the layered map server.
+
+## TTL smoke test
+
+The small smoke test 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
+```
+
+## Three-robot Nav2 observation demo
+
+The Nav2 demo starts three stationary robots in free corners of the warehouse
+map and spawns one deterministic box near each robot. Each robot's observation
+node subscribes to its `sensor_msgs/LaserScan`, represents valid scan endpoints
+as point `Region` messages, and publishes a replacing `MapRegionUpdate`
+snapshot. The layered map server transforms and combines all three sources over
+the static map.
+
+No RMF planning components or navigation goals are required. By default the
+Nav2 launch opens three robot-local RViz windows, and the demo opens another
+RViz window showing the combined `/map`:
+
+```bash
+ros2 launch rmf_layered_map_server_demo nav2_observations.launch.py
+```
+
+Use `use_nav2_rviz:=False` to open only the combined-map view. Use
+`spawn_clutter:=False` to run against the unmodified warehouse world.
+
+The initial implementation deliberately keeps the scan-to-region conversion
+simple so its cost and information loss are visible. `beam_stride` samples
+fewer endpoints, while `publish_period_sec` throttles snapshots. For example:
+
+```bash
+ros2 launch rmf_layered_map_server_demo nav2_observations.launch.py \
+  beam_stride:=4 publish_period_sec:=1.0
+```
+
+Each publisher logs the number of input beams and output regions. This makes it
+possible to compare message density and global-map update rate against the
+visual fidelity in RViz.
+
+The prototype publishes occupied endpoints only. It does not yet turn clearing
+rays into free-space regions or compress adjacent endpoints into larger region
+primitives. `reset_source` removes the previous snapshot before each new scan,
+and TTL expiry removes observations if a robot stops publishing.

map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py

@@ -0,0 +1,199 @@
+# Copyright 2026 Open Source Robotics Foundation
+#
+# 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    EmitEvent,
+    ExecuteProcess,
+    RegisterEventHandler,
+    TimerAction,
+)
+from launch.conditions import IfCondition
+from launch.event_handlers import OnProcessExit
+from launch.events import Shutdown
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+ROBOTS = (
+    ('robot0', -12.0, -21.0, 0.7854),
+    ('robot1', 10.0, -21.0, 2.3562),
+    ('robot2', -12.0, 21.0, -0.7854),
+)
+
+
+def robots_argument():
+    """Format the three warehouse-corner robot poses for Nav2 bringup."""
+    return '; '.join(
+        f'{name}={{x: {x}, y: {y}, yaw: {yaw}}}'
+        for name, x, y, yaw in ROBOTS
+    )
+
+
+def generate_launch_description():
+    demo_share = get_package_share_directory('rmf_layered_map_server_demo')
+    nav2_share = get_package_share_directory('sp_demo_nav2_bringup')
+
+    map_file = LaunchConfiguration('map')
+    params_file = LaunchConfiguration('params_file')
+    use_nav2_rviz = LaunchConfiguration('use_nav2_rviz')
+    use_global_rviz = LaunchConfiguration('use_global_rviz')
+    spawn_clutter = LaunchConfiguration('spawn_clutter')
+    beam_stride = LaunchConfiguration('beam_stride')
+    publish_period_sec = LaunchConfiguration('publish_period_sec')
+    ttl_sec = LaunchConfiguration('ttl_sec')
+    max_observation_range = LaunchConfiguration('max_observation_range')
+
+    declarations = [
+        DeclareLaunchArgument(
+            'map',
+            default_value=os.path.join(nav2_share, 'maps', 'warehouse.yaml'),
+            description='Static map used by each Nav2 robot.',
+        ),
+        DeclareLaunchArgument(
+            'params_file',
+            default_value=os.path.join(
+                nav2_share, 'params', 'nav2_multirobot_params_all.yaml'
+            ),
+            description='Nav2 parameters shared by the three robots.',
+        ),
+        DeclareLaunchArgument(
+            'use_nav2_rviz',
+            default_value='True',
+            description='Whether to open one local Nav2 RViz window per robot.',
+        ),
+        DeclareLaunchArgument(
+            'use_global_rviz',
+            default_value='True',
+            description='Whether to open the combined global-map RViz window.',
+        ),
+        DeclareLaunchArgument(
+            'spawn_clutter',
+            default_value='True',
+            description='Whether to spawn one deterministic obstacle near each robot.',
+        ),
+        DeclareLaunchArgument(
+            'beam_stride',
+            default_value='1',
+            description='Publish every Nth valid laser return as a point region.',
+        ),
+        DeclareLaunchArgument(
+            'publish_period_sec',
+            default_value='0.5',
+            description='Minimum period between region snapshots from each robot.',
+        ),
+        DeclareLaunchArgument(
+            'ttl_sec',
+            default_value='1.0',
+            description='Lifetime of each robot observation snapshot.',
+        ),
+        DeclareLaunchArgument(
+            'max_observation_range',
+            default_value='2.5',
+            description='Maximum laser return distance represented globally.',
+        ),
+    ]
+
+    # ParseMultiRobotPose in the Nav2 demo reads sys.argv directly. Launch it as
+    # a managed child process so the robot list reaches that parser.
+    nav2_simulation = ExecuteProcess(
+        cmd=[
+            'ros2',
+            'launch',
+            'sp_demo_nav2_bringup',
+            'cloned_multi_tb3_simulation_launch.py',
+            f'robots:={robots_argument()}',
+            ['map:=', map_file],
+            ['params_file:=', params_file],
+            ['use_rviz:=', use_nav2_rviz],
+            'use_navigation:=False',
+        ],
+        output='screen',
+    )
+
+    layered_map_server = Node(
+        package='rmf_layered_map_server',
+        executable='rmf_layered_map_server',
+        output='screen',
+        remappings=[('/map/static', '/robot0/inner/map')],
+    )
+
+    observation_nodes = []
+    for robot_name, _, _, _ in ROBOTS:
+        observation_nodes.append(Node(
+            package='rmf_layered_map_server_demo',
+            executable='scan_region_publisher',
+            namespace=f'{robot_name}/inner',
+            output='screen',
+            parameters=[{
+                'robot_name': robot_name,
+                'map_frame': 'map',
+                'map_name': 'warehouse',
+                'scan_topic': f'/{robot_name}/inner/scan',
+                'beam_stride': ParameterValue(beam_stride, value_type=int),
+                'publish_period_sec': ParameterValue(
+                    publish_period_sec, value_type=float
+                ),
+                'ttl_sec': ParameterValue(ttl_sec, value_type=float),
+                'max_observation_range': ParameterValue(
+                    max_observation_range, value_type=float
+                ),
+            }],
+            remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')],
+        ))
+
+    clutter_spawner = TimerAction(
+        period=5.0,
+        actions=[Node(
+            condition=IfCondition(spawn_clutter),
+            package='rmf_layered_map_server_demo',
+            executable='layered_map_demo_clutter_spawner',
+            output='screen',
+        )],
+    )
+
+    global_rviz = Node(
+        condition=IfCondition(use_global_rviz),
+        package='rviz2',
+        executable='rviz2',
+        name='layered_global_map_rviz',
+        arguments=[
+            '-d',
+            os.path.join(demo_share, 'rviz', 'nav2_observations.rviz'),
+        ],
+        parameters=[{'use_sim_time': True}],
+        output='screen',
+    )
+    global_rviz_exit_handler = RegisterEventHandler(
+        condition=IfCondition(use_global_rviz),
+        event_handler=OnProcessExit(
+            target_action=global_rviz,
+            on_exit=EmitEvent(event=Shutdown(reason='global RViz exited')),
+        ),
+    )
+
+    return LaunchDescription([
+        *declarations,
+        nav2_simulation,
+        layered_map_server,
+        *observation_nodes,
+        clutter_spawner,
+        global_rviz,
+        global_rviz_exit_handler,
+    ])

map_server/rmf_layered_map_server_demo/package.xml

@@ -12,11 +12,19 @@
   <exec_depend>rclpy</exec_depend>
   <exec_depend>ament_index_python</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>gz_tools_vendor</exec_depend>
+  <exec_depend>launch</exec_depend>
+  <exec_depend>launch_ros</exec_depend>
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>rmf_layered_map_msgs</exec_depend>
   <exec_depend>rmf_layered_map_server</exec_depend>
   <exec_depend>rmf_prototype_msgs</exec_depend>
   <exec_depend>rviz2</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>sp_demo_nav2_bringup</exec_depend>
+  <exec_depend>tf2_ros_py</exec_depend>
+
+  <test_depend>python3-pytest</test_depend>
 
   <export>
     <build_type>ament_python</build_type>