|
| 1 | +[Draft for Discourse] |
| 2 | + |
| 3 | +# Layered Global Map Follow-ups |
| 4 | + |
| 5 | +This post collects the next pieces to build after the first layered global occupancy map implementation. |
| 6 | + |
| 7 | +# Quick Summary |
| 8 | + |
| 9 | +* Use an approximately `0.5 m` global planning grid in the demos |
| 10 | +* Downsample local Nav2 costmaps before publishing global map observations |
| 11 | +* Add a Nav2 observation publisher that emits `MapRegionUpdate` messages |
| 12 | +* Visualize replanning behavior in the demo |
| 13 | +* Add active-plan collision checking so paths can be invalidated after publication |
| 14 | +* Keep blocked-path reporting and replan debouncing separate from map composition |
| 15 | + |
| 16 | +# Planning Resolution |
| 17 | + |
| 18 | +Use a global planning map cell size around `0.5 m`. This matches the rough size of the robots used in the current demos and keeps the MAPF problem smaller than a dense local Nav2 costmap. |
| 19 | + |
| 20 | +Downsample local robot costmaps to the global planning resolution before publishing region updates. In practice this means a cluster of fine local costmap cells becomes one or more coarser `Region` updates on `/map/region_updates`. |
| 21 | + |
| 22 | +Keep resampling policy outside the layered map server. The demo and map-loading configuration should use a static map resolution close to the desired MAPF planning resolution, initially `0.5 m`. |
| 23 | + |
| 24 | +# Nav2 Observation Node |
| 25 | + |
| 26 | +Add a node that reads a robot's Nav2 local costmap and publishes `MapRegionUpdate` messages. |
| 27 | + |
| 28 | +The first version should: |
| 29 | + |
| 30 | +1. Subscribe to a local `nav2_msgs/Costmap`, such as `~/local_costmap/costmap_raw`. |
| 31 | +2. Threshold lethal and inflated cells into a binary obstacle mask. |
| 32 | +3. Optionally subtract cells already occupied by the static map. |
| 33 | +4. Downsample to the global planning resolution, initially around `0.5 m`. |
| 34 | +5. Group adjacent cells into connected components. |
| 35 | +6. Publish components as axis-aligned rectangle regions with a short TTL. |
| 36 | + |
| 37 | +Keep the observation interface generic enough for robots with richer perception stacks to publish `MapRegionUpdate` messages without using this Nav2-specific node. |
| 38 | + |
| 39 | +# Replanning |
| 40 | + |
| 41 | +Avoid triggering global replanning directly from every map update. Local observations may arrive frequently, and many will not affect the current traffic plan. |
| 42 | + |
| 43 | +Handle blocked-path reporting separately. The existing `~/plan/error` topic already includes `PlanError.CODE_PATH_BLOCKED`, which can be used by a Nav2 traffic bridge or plan executor when a robot can no longer progress through its current safe zone. That follow-up can debounce replan requests and ask the path server to plan again using the latest composed map. |
| 44 | + |
| 45 | +Add an active-plan collision check after a path has been sent out. The check should compare the robot's current plan or safe zone against the latest composed map and publish `CODE_PATH_BLOCKED` when the remaining path is no longer traversable. |
| 46 | + |
| 47 | +# Testing and Visualization |
| 48 | + |
| 49 | +Make replanning visible before considering the loop complete. A useful test scenario is: |
| 50 | + |
| 51 | +1. Start with the static map from the map-loading work in `open-rmf/next_gen_prototype#22`. |
| 52 | +2. Send two robots through the path server and visualize their published plans. |
| 53 | +3. Publish a temporary obstacle region with a TTL onto `/map/region_updates`. |
| 54 | +4. Confirm in RViz or the web visualizer that the composed `/map` includes the temporary obstacle. |
| 55 | +5. Trigger a blocked-path report for the affected robot and confirm that the next plan avoids the obstacle. |
| 56 | +6. Let the TTL expire and confirm that the obstacle disappears from `/map`. |
| 57 | + |
| 58 | +Add an integration test for the whole loop by publishing an obstacle, reporting a blocked path, and checking that the next path differs from the original path. |
| 59 | + |
| 60 | +# Suggested PR Split |
| 61 | + |
| 62 | +## PR 1: Demo and Path Server Wiring |
| 63 | + |
| 64 | +Update the demo so the static map is remapped to `/map/static`, the layered map server is launched, and the path server continues to consume `/map`. |
| 65 | + |
| 66 | +The acceptance test should use an approximately `0.5 m` planning grid and show that adding a dynamic obstacle changes the next path produced by the path server. |
| 67 | + |
| 68 | +## PR 2: Nav2 Observation Publisher |
| 69 | + |
| 70 | +Add a Rust observation node that converts a local Nav2 costmap into `MapRegionUpdate` messages. |
| 71 | + |
| 72 | +The node should downsample the local costmap to the global planning resolution before publishing regions. The demo should show an obstacle observed by a robot appearing in the layered global map and then expiring when it is no longer refreshed. |
| 73 | + |
| 74 | +## PR 3: Active-plan Collision Checking |
| 75 | + |
| 76 | +Add a check in the plan executor or Nav2 traffic bridge that compares the remaining active plan or safe zone against the latest composed map. |
| 77 | + |
| 78 | +When the remaining route is blocked, the component should publish `PlanError.CODE_PATH_BLOCKED` and let the existing replan flow request a new path. |
| 79 | + |
| 80 | +# Future Extensions |
| 81 | + |
| 82 | +* dense grid or delta-grid update messages |
| 83 | +* voxel or 3D observation components |
| 84 | +* semantic map layers |
| 85 | +* a map revision/status topic for consumers that need to correlate plans with a specific composed map version |
| 86 | +* smarter region compression for irregular obstacle shapes |
0 commit comments