Demo obstacle-triggered replanning with the layered global map

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

@@ -24,6 +24,7 @@ observations, can be added later.
 * 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 replanning demo fuses two robots' scans and replans when an updated map blocks an active route
 * The observation messages live in `rmf_layered_map_msgs`, leaving
   `rmf_prototype_msgs` unchanged
 
@@ -133,6 +134,12 @@ The demo launches Nav2 localization, planning, and control for the fixed goal lo
 
 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.
 
+# Two-Robot Replanning Demo
+
+The replanning launch starts with a simple room by default and retains the warehouse layout as a regression scenario. It fuses both robots' scans into `/map`, spawns a blocking bar, and displays robot-colored poses, goals, observations, and Nav2 paths alongside the green RMF plans.
+
+The plan executor checks each remaining route against the composed map and publishes `CODE_PATH_BLOCKED` after a short debounce. The path server replans on a conservatively downsampled `1.0 m` grid, while the Nav2 bridge ignores superseded aborts and resends unchanged targets when a new plan requires them.
+
 # Example Flow
 
 A local costmap or LiDAR observation node can publish a replacement snapshot by:

map_server/README.md

@@ -12,4 +12,4 @@ 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.
+See [`rmf_layered_map_server_demo/README.md`](rmf_layered_map_server_demo/README.md) for the three-robot observation demo and two-robot replanning demo.

map_server/rmf_layered_map_server_demo/README.md

@@ -1,6 +1,6 @@
 # Layered Map Server Demos
 
-This package contains two demonstrations of the layered map server.
+This package contains three demonstrations of the layered map server.
 
 ## TTL Smoke Test
 
@@ -31,3 +31,46 @@ By default, the demo opens three robot-local RViz windows and one combined-map v
 * 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.
+
+## Two-Robot Obstacle Replanning Demo
+
+This demo connects the layered map server to the path server, plan executor, and Nav2 traffic bridge. It exercises the `CODE_PATH_BLOCKED` replanning flow from [PR #39](https://github.com/open-rmf/next_gen_prototype/pull/39).
+
+The default scenario is a simple room with no aisles:
+
+```bash
+ros2 launch rmf_layered_map_server_demo replan_obstacle.launch.py
+```
+
+Two robots start with non-overlapping scan regions and separate goals. Once their initial plans are active, the demo spawns a red bar across both scan regions. The bar meets the right wall to form a dead end and leaves a detour around its left end.
+
+The global RViz view shows robot-colored poses, goals, region contributions, and Nav2 paths alongside the green RMF plans.
+
+Use the warehouse scenario to reproduce the original two-aisle case:
+
+```bash
+ros2 launch rmf_layered_map_server_demo replan_obstacle.launch.py \
+  scenario:=warehouse
+```
+
+This scenario keeps the original robot poses and 15 m shelf-connected bar.
+
+Run only one scenario per ROS/Gazebo domain because both use the same robot and map topic names. For concurrent runs, set different `ROS_DOMAIN_ID` and `GZ_PARTITION` values.
+
+The expected sequence in the global RViz window is:
+
+1. Both initial plans appear on the static map.
+2. The long bar is spawned and its detected portions enter the combined `/map`.
+3. After a 300 ms debounce, the plan executor publishes `PlanError.CODE_PATH_BLOCKED` for the blocked route.
+4. The path server publishes a new plan using the updated map.
+
+The path server conservatively downsamples the `0.1 m` simple-room map and the `0.03 m` warehouse map to its `1.0 m` PiBT planning resolution. Any planning cell containing an occupied source cell remains occupied. Replan reports have a two-second cooldown to prevent oscillation.
+
+Useful launch arguments:
+
+* `use_nav2_rviz:=True` opens the robot-local Nav2 views.
+* `use_global_rviz:=False` runs without the combined RViz view.
+* `spawn_delay_sec:=1.0` controls the delay after initial plans are received.
+* `scenario_timeout_sec:=180.0` controls how long the demo waits for replanning.
+* `obstacle_memory_sec:=-1.0` retains obstacle endpoints for the scanner's lifetime. Use `0.0` to disable memory or a positive value for finite retention.
+* `self_filter_radius:=0.22` excludes scan returns inside the robot body.

map_server/rmf_layered_map_server_demo/launch/replan_obstacle.launch.py

@@ -0,0 +1,39 @@
+# 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.
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+
+from rmf_layered_map_server_demo.replan_obstacle_launch import (
+    generate_replan_launch_description,
+)
+
+
+def _launch_scenario(context):
+    scenario = LaunchConfiguration('scenario').perform(context)
+    return generate_replan_launch_description(scenario).entities
+
+
+def generate_launch_description():
+    """Launch a replanning scenario."""
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'scenario',
+            default_value='simple',
+            choices=['simple', 'warehouse'],
+            description='Replanning environment to launch.',
+        ),
+        OpaqueFunction(function=_launch_scenario),
+    ])

map_server/rmf_layered_map_server_demo/maps/single_room.yaml

@@ -0,0 +1,7 @@
+image: single_room.png
+mode: trinary
+resolution: 0.1
+origin: [-10.0, -8.0, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.1

map_server/rmf_layered_map_server_demo/package.xml

@@ -13,13 +13,20 @@
   <exec_depend>rclpy</exec_depend>
   <exec_depend>ament_index_python</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>lifecycle_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>nav2_common</exec_depend>
   <exec_depend>nav2_msgs</exec_depend>
   <exec_depend>rmf_layered_map_msgs</exec_depend>
   <exec_depend>rmf_layered_map_server</exec_depend>
+  <exec_depend>rmf_nav2_traffic</exec_depend>
+  <exec_depend>rmf_path_server</exec_depend>
+  <exec_depend>rmf_path_visualizer</exec_depend>
+  <exec_depend>rmf_plan_executor</exec_depend>
+  <exec_depend>rmf_simple_destination_server</exec_depend>
   <exec_depend>rmf_prototype_msgs</exec_depend>
   <exec_depend>rviz2</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>

map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py

@@ -23,14 +23,16 @@
 from rmf_prototype_msgs.msg import Region
 from visualization_msgs.msg import Marker, MarkerArray
 
+from .replan_scenarios import ROBOT_COLORS
+
 
 SOURCE_COLORS = (
-    (0.96, 0.26, 0.21),
+    ROBOT_COLORS['robot0'][:3],
+    ROBOT_COLORS['robot1'][:3],
     (0.18, 0.80, 0.44),
-    (0.20, 0.60, 0.98),
-    (1.00, 0.65, 0.15),
     (0.61, 0.35, 0.71),
     (0.10, 0.74, 0.80),
+    (0.96, 0.26, 0.21),
 )
 CLEAR_COLOR_BLEND = 0.5
 CLEAR_MARKER_Z = 0.04
@@ -183,10 +185,15 @@ def apply_update(self, update):
         if latest_stamp is not None and stamp_nsec < latest_stamp:
             return MarkerArray()
 
-        color = self.source_colors.setdefault(
-            key,
-            SOURCE_COLORS[len(self.source_colors) % len(SOURCE_COLORS)],
-        )
+        color = self.source_colors.get(key)
+        if color is None:
+            robot_color = ROBOT_COLORS.get(update.source.robot_name)
+            color = (
+                robot_color[:3]
+                if robot_color is not None
+                else SOURCE_COLORS[len(self.source_colors) % len(SOURCE_COLORS)]
+            )
+            self.source_colors[key] = color
         marker_array = MarkerArray()
         self.markers_by_source[key] = [
             marker