Demo obstacle-triggered replanning with the layered global map

[SamuelFoo]

New feature implementation

Implemented feature

This PR integrates the layered global occupancy map with the obstacle replanning flow from open-rmf/next_gen_prototype#39.

Two robots publish local scan observations into the combined /map. After a long obstacle is spawned, the plan executor detects blocked active routes and requests new plans using the updated global map.

Implementation description

This PR adds:

• simple-room and warehouse obstacle replanning scenarios
• active-route collision checks with debounce and cooldown
• conservative occupancy-grid downsampling for the 1.0 m PiBT planner
• staged multi-robot Nav2 startup
• handling for superseded goals and unchanged targets from new plans
• obstacle retention and robot-body scan filtering
• robot-colored poses, goals, observations, and Nav2 paths in RViz
• automated obstacle spawning and replan verification

The demo documentation describes both scenarios, launch arguments, visualization, and expected replanning sequence.

ros2 launch rmf_layered_map_server_demo replan_obstacle.launch.py

simple_replan_compressed.mp4

ros2 launch rmf_layered_map_server_demo replan_obstacle.launch.py scenario:=warehouse

warehouse_replan_compressed.mp4

Next steps

• Rasterize incoming region updates into per-source occupancy layers instead of retaining a stack of region messages.
• Apply TTL expiry, clearing, and reset_source behavior directly to those layers.
• Evaluate memory usage and map composition performance with higher update rates.

GenAI Use

We follow OSRA's policy on GenAI tools

• I used a GenAI tool in this PR.
• I did not use GenAI

Generated-by: GPT-5

@arjo129 ready for review!

[SamuelFoo]

Here is a summary of issues addressed:

• Startup: Nav2 could autostart before Gazebo odometry and TF were ready, leaving lifecycle nodes inactive. Startup is now staged (da1ab1b).

• If a replan kept the same immediate target, the bridge treated it as unchanged and sent no new Nav2 goal. If the old goal was then aborted or cancelled, the bridge could continue suppressing the replacement because its coordinates were unchanged, leaving the robot stuck. Targets are now resent whenever the PlanId changes (12a7ae9).

For example:

1. The old plan sends the robot toward waypoint A.
2. An obstacle blocks the current route to A.
3. RMF creates a new plan that still targets A but reaches it using a different route.
4. Previously, the bridge considered A a duplicate and did not send a goal associated with the new plan. If the old goal failed, the robot had no replacement command.

• Installing a new plan could cancel the previous Nav2 goal. A delayed abort from that superseded goal could be mistaken for a new blockage, triggering another CODE_PATH_BLOCKED, another replan, and another cancellation. This feedback loop could leave the robot repeatedly replanning without progressing. Superseded goals are now marked explicitly, and stale aborts and cleanup events are ignored (f0da686).

• An unchanged obstacle could cause repeated blockage reports for successive plans, making the system continuously replan without progressing. The blockage monitor now reports once per PlanId, requires 300 ms persistence, and enforces a two-second cross-plan cooldown (c1e4920).