Add layered global occupancy map server by SamuelFoo · Pull Request #36 · open-rmf/next_gen_prototype

SamuelFoo · 2026-07-09T00:46:10Z

New feature implementation

Implemented feature

This PR adds the first slice of a layered global occupancy map for the next generation prototype.

It introduces a dedicated layered map message package, a Rust layered map server, a small RViz demo, and ROS integration coverage. The path server can continue consuming a normal nav_msgs/OccupancyGrid on /map, while the layered map server composes that map from a static map and temporary observation regions.

Implementation description

This PR adds:

rmf_layered_map_msgs
- MapObservationSource
- MapRegionUpdate
- sparse region updates with TTL, obstacle, clear, and reset semantics
map_server/rmf_layered_map_server
- Rust ament_cargo package
- subscribes to /map/static
- subscribes to /map/region_updates
- publishes the composed nav_msgs/OccupancyGrid on /map
- keeps static map data separate from dynamic observations
- prunes expired observations by TTL
- lets obstacle observations win over clear observations
- supports reset updates per source and map
map_server/rmf_layered_map_server_demo
- demo publisher for a static map and temporary obstacle
- launch file that starts the layered map server, demo publisher, and RViz
- RViz config for visualizing the composed /map
map_server/rmf_layered_map_server_test
- ROS launch test that publishes a static map and region updates
- verifies composition, reset behavior, and TTL expiry

The discourse draft in discourse/3-layered-global-occupancy-map.md documents the code implemented in this PR.

Next steps

Add a Nav2 observation publisher that converts local costmap data into MapRegionUpdate messages.
Use an approximately 0.5 m global planning grid in the map/path demos.
Wire the layered map server into the path server demo so dynamic obstacles can affect newly generated plans.
Add active-plan collision checking so a plan can be invalidated after it has already been sent out.
Keep blocked-path reporting and replan debouncing separate from map composition.
Consider richer map update formats later, such as dense grid deltas, voxel observations, or semantic layers.

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.5

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

arjo129

We should batch all observations from a single robot into a single message instead of multiple messages this improves determinism on my end. I also have been a bit critical on the specification doc.

arjo129 · 2026-07-09T01:41:02Z

+
+# Quick Summary
+
+* Robots and perception sources can publish sparse 2D region updates with a time-to-live (TTL)


We may want to think about limiting it to 2D there are many cases where 2d is insufficient. For instance, overhangs. Short robots may be able to go under such overhangs but tall robots can't.

arjo129 · 2026-07-09T01:45:08Z

+* Robots and perception sources can publish sparse 2D region updates with a time-to-live (TTL)
+* A central layered map server composes a static map with active dynamic observations
+* Observations from multiple sources are stitched into one composed global grid
+* The composed output is published as `nav_msgs/OccupancyGrid` on `/map`


This is an implementation "detail". This document is meant to be read by someone who is building a robot and wants to integrate with it. OR someone who may have some better map representation. The "/map" topic is not something we should allow people to rely on.

arjo129 · 2026-07-09T01:47:26Z

+# Interface Topology Overview
+
+```mermaid
+flowchart LR
+    static["Static map server"] -- "/map/static" --> layered["Layered map server"]
+    observer["Observation source"] -- "/map/region_updates" --> layered
+    layered -- "composed /map" --> path["Path server"]
+```
+
+The layered map server owns the composed `/map` topic. When it is present, any static map publisher should be remapped to `/map/static`.


The /map/static is an implementation detail.

/map/region_updates is what we should be discussing.

arjo129 · 2026-07-09T04:52:41Z

+
+Inputs:
+
+* `/map/static`: `nav_msgs/OccupancyGrid`, transient local, reliable


Ignore the static map in your write up here this belongs in the read me for rmf_layered_map_server.

arjo129 · 2026-07-09T05:11:20Z

+# The regions should be interpreted as temporary free space.
+uint8 UPDATE_CLEAR=2
+
+# The update should remove previous temporary observations from this source.


Whats the use case for reset?

Reset is for when a source wants to replace or invalidate its previous temporary observations without asserting that any specific cells are free.

arjo129 · 2026-07-09T05:12:00Z

+
+# 2D geometric observation patches. Empty regions are valid only for
+# UPDATE_RESET.
+rmf_prototype_msgs/Region[] regions


Do we iterate in order? How do we guarantee the order by which clear and mark occur. Im trying to think of a scenario where a laser scan is involved. We would have a region that needs clearing and a region that needs updating.

I also feel region data should live together with the action that should be taken and the ttl. That way a single observation in the agent's end would make a single message improving determinism.

Addressed this by changing MapRegionUpdate to contain MapRegionPatch[] patches.

Each patch now carries its own action, occupancy value, TTL, and regions, so a single sensor snapshot can publish clear and obstacle evidence in one ROS message. The server applies reset_source first, then sorts accepted patches so UPDATE_CLEAR is applied before UPDATE_OBSTACLE, giving deterministic behavior (clear then mark) without depending on separate message arrival order.

arjo129 · 2026-07-09T05:13:46Z

+# Coordinate frame for the regions.
+string frame_id
+
+# Time when this observation snapshot was produced.
+builtin_interfaces/Time stamp


std_msgs/msg/Header exists for this.

arjo129 · 2026-07-09T05:15:53Z

+            width, height, resolution, origin_x, origin_y, min_x, min_y, max_x, max_y,
+        );
+    }
+


Log errors for other types of regions for now.

This comment still applies.

arjo129 · 2026-07-09T05:36:36Z

@@ -0,0 +1,21 @@
+# MapObservationSource.msg


One concern I have is we would have to rely on TF like interface to look up individual odometry and sync them. It might actually be worth stamping the robot pose here just so we dont re-implment TF.

Introduce MapRegionPatch so one MapRegionUpdate can carry multiple clear and obstacle patches from the same observation snapshot. Move patch-specific action, occupancy value, TTL, and region geometry out of MapRegionUpdate, leaving the update to describe the source, optional reset_source operation, and ordered patch list. Use std_msgs/Header and robot_pose on MapObservationSource, and express TTLs as seconds so rmf_layered_map_msgs no longer depends directly on builtin_interfaces. Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

Update the layered map server and callers for MapRegionUpdate.patches: reset_source is applied before patches from the same snapshot, clear-space patches are sorted before obstacle patches for deterministic clear-then-mark behavior, older snapshots are rejected per source/map timestamp, and the demo/test helpers now populate MapRegionPatch, Header, and TTL seconds while the server package drops its unused direct builtin_interfaces dependency. Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

SamuelFoo · 2026-07-09T19:14:30Z

Addressed the review feedback:

Batched observation patches into one MapRegionUpdate
Added MapRegionPatch so action, TTL, occupancy value, and regions stay together
Apply reset_source before adding new patches from the same message
Apply clear patches before obstacle patches for deterministic clear-then-mark behavior
Reject stale older snapshots by source timestamp
Use std_msgs/Header for frame/timestamp
Added robot pose metadata to the observation source
Removed direct builtin_interfaces usage from rmf_layered_map_msgs
Log unsupported/invalid region inputs
Refocused the Discourse write-up on /map/region_updates instead of implementation details like /map/static

Ready for review!

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

arjo129

The message is looking better. There is a performance issue that we can address in a future PR.

arjo129 · 2026-07-09T07:53:29Z

+MapObservationSource source
+
+# UPDATE_OBSTACLE, UPDATE_CLEAR, or UPDATE_RESET.
+uint8 update_type


Suggested change

uint8 update_type

uint8[] update_type

arjo129 · 2026-07-10T04:25:12Z

+
+# Pose of the robot when this observation was produced. Fixed sensors or
+# synthetic map layers may leave this at its default value.
+geometry_msgs/PoseStamped robot_pose


If we use the stamp here why do we need the header? Should we just use "Pose" instead of PoseStamped since we have a header?

arjo129 · 2026-07-10T06:25:01Z

+            width, height, resolution, origin_x, origin_y, min_x, min_y, max_x, max_y,
+        );
+    }
+


This comment still applies.

arjo129 · 2026-07-10T07:18:31Z

+        let stamp_nsec = if update.source.header.stamp.sec == 0
+            && update.source.header.stamp.nanosec == 0
+        {
+            now_nsec


This should be an error and no update,

arjo129 · 2026-07-10T07:19:38Z

+
+        let default_ttl_sec = update.source.default_ttl_sec;
+        let mut patches = update.patches;
+        patches.sort_by_key(|patch| match patch.update_type {


So we always clear before marking? Mention that in the message documentation.

arjo129 · 2026-07-10T07:34:10Z

+    // Keep the ROS handles alive for as long as the server is running.
+    pub static_map_subscription: rclrs::WorkerSubscription<OccupancyGrid, LayeredMapServer>,
+    pub region_update_subscription: rclrs::WorkerSubscription<MapRegionUpdate, LayeredMapServer>,
+    pub prune_timer: Box<dyn std::any::Any + Send + Sync>,


Why not use the full type?

arjo129 · 2026-07-10T07:55:20Z

+#[derive(Clone, Debug)]
+pub struct LayeredMap {
+    static_grid: Option<OccupancyGrid>,
+    dynamic_observations: Vec<DynamicObservation>,


Im wondering if a Vec is the best data structure here. Lets think of how frequently we expect these observations to come in. If we assume a 1hz rate we would see the same obstacle multiple times. Would this not add performance overhead as we would have an evergrowing list of observations for the same overlapping region.

IMO we should rasterize the moment we receive an observation and prune the list. Theres a bit of a tradeoff depending on how the protocol is implemented.

arjo129 · 2026-07-10T08:00:58Z

+# observations.
+float64 ttl_sec
+
+# 2D geometric observation patches.


Specify the coordinate frame of the region. Is it in the robot's frame or the global frame or configurable? I recommend robot frame and then perform a transform. The rationale is we dont need to know the server's configuration properties. The less configuration we have on the server side that needs to be shared with the robot the better.

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

Define robot_pose in the observation header frame, transform supported region geometry before rasterization, and reject updates whose frame does not match the static map. Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

SamuelFoo · 2026-07-10T16:35:55Z

Addressed the latest review feedback:

Replaced PoseStamped with Pose since MapObservationSource already has a shared header
Reject updates with a zero timestamp instead of substituting the server time
Defined observation regions in the robot-local frame
Transform regions into source.header.frame_id using source.robot_pose
Reject updates whose frame does not match the static map frame
Log and ignore malformed or unsupported region types
Limit the current implementation to point and axis-aligned rectangle regions
Use the concrete WorkerTimer type instead of erasing the timer handle
Updated the demo, tests, message comments, README, and Discourse write-up for the revised frame and timestamp semantics
Added coverage for robot-local transforms, missing timestamps, frame mismatches, and unsupported regions

The "clear then mark" ordering was mentioned in the MapRegionUpdate message documentation.

The observation storage and incremental rasterization optimization will be handled in a follow-up, as discussed.

Ready for review!

arjo129 · 2026-07-13T03:17:02Z

The demo looks good. I want to hold off on a merge till we have a complete e2e showcase of replanning. I'll create an epic branch and work form there.

arjo129 · 2026-07-13T05:34:29Z

Also partially related: #5

SamuelFoo added 5 commits July 9, 2026 02:28

docs: add layered map design note

bf28e04

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

feat: add layered map observation messages

e22d587

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

feat: add layered map server

2165d74

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

feat: add layered map demo

6b8794b

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

test: add layered map server launch test

2dd7790

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

mxgrey added this to PMC Board Jul 9, 2026

github-project-automation Bot moved this to Inbox in PMC Board Jul 9, 2026

arjo129 requested changes Jul 9, 2026

View reviewed changes

arjo129 mentioned this pull request Jul 9, 2026

[Feature request]: Handle failure to plan given unforseen constraints at a local planning level #37

Open

1 task

SamuelFoo added 5 commits July 9, 2026 21:06

fix: log invalid layered map regions

a05e0d0

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

docs: explain batched map patch updates

947c6f8

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

docs: refocus layered map observation spec

e329aa3

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

SamuelFoo force-pushed the feat/layered-global-map branch from 8e575ee to e329aa3 Compare July 9, 2026 19:16

docs: clarify map patch ordering

89eab89

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

arjo129 requested changes Jul 10, 2026

View reviewed changes

SamuelFoo added 4 commits July 10, 2026 18:27

refactor: use the concrete map server timer type

2aeed1e

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

fix: reject unstamped layered map updates

0f1bf41

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

fix: reject unsupported layered map region types

4e20799

Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

fix: transform robot-local map regions

2f8c0c7

Define robot_pose in the observation header frame, transform supported region geometry before rasterization, and reject updates whose frame does not match the static map. Signed-off-by: SamuelFoo <fooenzesamuel@gmail.com>

SamuelFoo force-pushed the feat/layered-global-map branch from 6a89c2c to 2f8c0c7 Compare July 13, 2026 18:34

This was referenced Jul 13, 2026

Demo Nav2 scan observations in the layered global map SamuelFoo/next_gen_prototype#2

Open

Add layered global map composition and Nav2 observation demo SamuelFoo/next_gen_prototype#1

Closed

mxgrey moved this from Inbox to In Review in PMC Board Jul 14, 2026


		# Quick Summary

		* Robots and perception sources can publish sparse 2D region updates with a time-to-live (TTL)


		Inputs:

		* `/map/static`: `nav_msgs/OccupancyGrid`, transient local, reliable

Uh oh!

Conversation

SamuelFoo commented Jul 9, 2026

New feature implementation

Implemented feature

Implementation description

Next steps

GenAI Use

Uh oh!

arjo129 left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SamuelFoo Jul 9, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SamuelFoo commented Jul 9, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

arjo129 left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SamuelFoo commented Jul 10, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

arjo129 commented Jul 13, 2026

Uh oh!

arjo129 commented Jul 13, 2026

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

SamuelFoo Jul 9, 2026 •

edited

Loading

SamuelFoo commented Jul 9, 2026 •

edited

Loading

SamuelFoo commented Jul 10, 2026 •

edited

Loading