Implement a layered global occupancy map

[arjo129]

Before proceeding, is there an existing issue or discussion for this?

• I have done a search for similar issues and discussions.

Description

Our current prototype for Nav2 integration relies on a one-directional flow of action:

A challenge arises when local planning fails to find a valid path for a robot, often due to unexpected obstacles blocking all progress. To resolve this, the robot must utilize its onboard sensors to dynamically update a centralized map.

Dotted lines represent new message interfaces that need to be specified

With this extended architecture, the Nav2-native robot can update the map server. If an agent can no longer progress within its safe operating envelope, a system-wide replan is triggered. This requires a mechanism to notify the fleet adapter of the blocked path, allowing global multi-agent planning to adapt in real time.

Key Considerations

• Time-to-Live (TTL): Local observations must incorporate a time-to-live mechanism. This ensures that transient obstacles, such as pedestrians or temporary equipment, do not permanently degrade the routing efficiency of the central fleet manager.
• Obstacle Representation: We must determine how to efficiently represent and transmit obstacle data (e.g., occupancy grid updates versus costmap layers) to minimize communication latency between Nav2 nodes and the central Open-RMF map server.
• Map Representation: Different planners may require alternatives to standard 2D occupancy grids.
• Replan Triggers: We must define the specific conditions that should necessitate a full, system-wide replanning cycle.

Immediate Actions:

• Propose a standardized set of messages to extend the perception system.
• Implement a lightweight local observation node that can be easily deployed onto a Nav2-powered Autonomous Mobile Robot (AMR).
• Implement a layered costmap approach to serving a map.

[SamuelFoo]

Hi, here is a first-pass design.

I think the first version can focus on a small feedback loop where robots publish local observations, the central map server merges them into temporary dynamic layers that expire by TTL, and global replanning is requested only when a robot reports that its current plan is blocked. I would start with the Nav2 local costmap from the simulation and treat richer perception inputs as later extensions.

flowchart LR
    costmap["Nav2 local costmap"] --> obs["Local observation node"]
    obs -- "region update with TTL on /map/region_updates" --> map["Central layered map server"]
    static["Static map"] --> map
    map -- "composed map on /map" --> planner

    robot["Robot navigation stack"] --> blocked["Blocked-path signal"]
    blocked --> fleet["Fleet adapter"]
    fleet -- "replan request" --> planner

    planner["Path server"] --> plan["Updated robot plan"]

Loading

Local observation node

Each Nav2 AMR could get a lightweight node beside its navigation stack. For the first implementation, this node could read the Nav2 local costmap from the simulation and publish region updates to the map server. Starting with rmf_prototype_msgs/Region keeps the observation interface sparse, reuses an existing dependency, and avoids tying the design to one specific sensor or obstacle detector. This also leaves room for robots that do not expose a dedicated perception node, as long as they can publish the agreed region update interface.

I would start with two observation messages:

• ObservationSource: source_id, robot_name, map_name, stamp, and a default_ttl.
• RegionUpdate: source metadata, one or more rmf_prototype_msgs/Region values, TTL, and a layer type.

The local node can derive these regions from the Nav2 local costmap at first. A de facto input topic could be /map/region_updates for these robot observation updates. Full or partial nav_msgs/OccupancyGrid publication can still be useful as a fallback or debugging path.

Central layered map server

The map server could keep the static map separate from dynamic layers such as:

• Observed obstacles
• Recently cleared space

Dynamic cells could expire through TTL so temporary obstacles do not stay in the global route model forever.

The first public output could be a composed nav_msgs/OccupancyGrid on /map, matching the usual ROS convention and giving the path server a single topic to consume. Internally, the server can still keep the layers separate so richer APIs can be exposed later.

Blocked-path signal

Local observations may arrive frequently, and many of them will not change the global plan in a meaningful way. To avoid noisy or expensive replanning, I would keep map updates separate from replan requests: robots can publish observations continuously, while a blocked-path message tells the fleet adapter when the current plan has failed beyond threshold.

That message could include the robot name, current plan ID, timestamp, blocked pose, progress along the plan, reason, and expected blockage duration.

Possible trigger conditions include repeated Nav2 planning failures, no controller progress for some timeout, or a local costmap showing that the robot's safe operating envelope is blocked. The fleet adapter can then debounce repeated requests and ask the path server to replan only when needed.

Tradeoffs

• Sparse updates: region messages avoid sending a dense grid when a small obstacle update only covers a few meaningful cells.
• Costmap conversion: the observation node needs to convert local costmap cells into regions, which may simplify detailed obstacle shapes.
• Map composition: the map server may still need to rasterize those regions into a composed nav_msgs/OccupancyGrid for current path server consumers.

Alternatives

• Full or partial occupancy grid updates: closer to the Nav2 costmap representation and useful as a fallback or debugging path, but full grids can be large if several robots publish frequently.
• Navigation meshes: useful for planners that want topology instead of cells, but probably not necessary for the first implementation.
• Voxels or layered 3D maps: useful for future 3D support, but they multiply bandwidth and storage costs. This seems better left until a 2D version works end to end.

Initial implementation

1. Target the xiyu/nav2 branch for the draft implementation.
2. Let the path server consume the composed occupancy map.
3. Add a central layered map server with static and dynamic TTL layers.
4. Add a local Nav2 observation node that publishes region updates from the Nav2 local costmap.
5. Add a blocked-path message and debounce logic for replanning.

For the stacked/layered occupancy grid work, we will likely need #22.

[arjo129]

Thanks for this proposal a couple of comments Id have:

• It should be clarified if we plan on following the whole error reporting mechanism in https://github.com/open-rmf/next_gen_prototype/blob/epic/next-gen-base/discourse/1-topics-interfaces.md . IMO, this is not necessary for the type of transient observations that come from a robot.
• Your point about blocked path signals is good. Id leave it as a "separate problem".
• Target epic/next-gen-base

[arjo129]

Another point I realise we should discuss is support for 3d. 2D lidars work fine in simulation but the real world is 3d: with overhangs, drops and other things. One approach is to have the option to encode such obstacles using voxels. For our first implementation we don't need to support this but it is something to be mindful of. As far as we are concerned the external messages should focus on broad high level support. What the internal representation of the occupancy grid is should be left as an implementation detail which different users can support.

[arjo129]

Your point about blocked path signals is good. Id leave it as a "separate problem".

Opened issue #37 regarding this.