From bf28e04b81c8914275214f7836ee55ea9b69b66d Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 9 Jul 2026 02:26:10 +0200 Subject: [PATCH 01/15] docs: add layered map design note Signed-off-by: SamuelFoo --- discourse/3-layered-global-occupancy-map.md | 133 ++++++++++++++++++++ 1 file changed, 133 insertions(+) create mode 100644 discourse/3-layered-global-occupancy-map.md diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md new file mode 100644 index 0000000..783c5d7 --- /dev/null +++ b/discourse/3-layered-global-occupancy-map.md @@ -0,0 +1,133 @@ +[Draft for Discourse] + +# Layered Global Occupancy Map + +This post describes the first implemented slice of a layered global occupancy map for the next generation Open-RMF prototype. + +The goal of this slice is to let temporary local observations affect the global planning map without changing the path server interface. The path server can continue to consume a normal `nav_msgs/OccupancyGrid` from `/map`, while a new map server composes that grid from a static map and active observation layers. + +# Quick Summary + +* 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` +* The observation messages live in `rmf_layered_map_msgs`, leaving `rmf_prototype_msgs` unchanged +* The first implementation is a Rust package named `rmf_layered_map_server` under `map_server/` + +# Background + +The static map support in the prototype gives the path server a useful baseline: it already consumes a `nav_msgs/OccupancyGrid` from `/map` and can plan around occupied cells. This implementation preserves that contract. Instead of teaching the planner about every possible observation source, the layered map server publishes one composed `/map`. + +Local observations from robots are different from static map data. They may describe temporary objects such as pedestrians, carts, doors held open, or equipment left in the path. For that reason the central map treats robot observations as dynamic layers that expire unless they are refreshed. + +This implementation builds on the existing static map behavior. + +# 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 path server continues to subscribe to `/map`. It does not subscribe directly to local robot observations. This keeps map composition independent from multi-agent planning and allows alternative map server implementations to be swapped in later. + +Each observation stream publishes updates with a unique `source_id`, such as `robot_1/local_costmap` or `robot_2/lidar_obstacles`. The layered map server tracks these sources independently but composes their active observations into a single `OccupancyGrid`. + +# Observation Topics + +* `/map/region_updates` - [`MapRegionUpdate.msg`](../rmf_layered_map_msgs/msg/MapRegionUpdate.msg): Dynamic local observation updates + +The topic is treated as an event stream. Updates are not latched because expired observations should not be replayed to a restarted map server as if they were current. + +# Messages + +## `MapObservationSource` + +[`MapObservationSource.msg`](../rmf_layered_map_msgs/msg/MapObservationSource.msg) describes where an observation came from. + +Important fields: + +* `source_id`: stable source identifier, usually the robot namespace plus the local source name +* `robot_name`: robot that produced the observation, if the source is mounted on a robot. This can be empty for fixed sensors or synthetic layers +* `map_name`: map or level that the observation belongs to +* `frame_id`: coordinate frame for the regions +* `stamp`: time when the observation snapshot was produced +* `default_ttl`: fallback TTL for updates from this source + +## `MapRegionUpdate` + +[`MapRegionUpdate.msg`](../rmf_layered_map_msgs/msg/MapRegionUpdate.msg) describes one sparse update from a local observation source. + +Update types: + +* `UPDATE_OBSTACLE`: regions are temporary occupied space +* `UPDATE_CLEAR`: regions are temporary free space +* `UPDATE_RESET`: previous observations from the same source should be removed + +The first implementation uses `rmf_prototype_msgs/Region` for 2D geometry. This keeps the message sparse and lets the central map server rasterize regions into its composed `OccupancyGrid`. + +The map observation messages live in a dedicated `rmf_layered_map_msgs` package. This keeps `rmf_prototype_msgs` as a stable public API surface while allowing the layered-map interface to evolve in its own package. + +# Layered Map Server + +The first server implementation is a Rust package named `rmf_layered_map_server` under the `map_server/` directory. + +Inputs: + +* `/map/static`: `nav_msgs/OccupancyGrid`, transient local, reliable +* `/map/region_updates`: `rmf_layered_map_msgs/MapRegionUpdate`, reliable + +Output: + +* `/map`: composed `nav_msgs/OccupancyGrid`, transient local, reliable + +The server keeps the static grid separate from active dynamic observations: + +```text +LayeredMap + static_grid: OccupancyGrid + dynamic_observations: Vec + revision: u64 + +DynamicObservation + source_id: String + map_name: String + update_type: obstacle | clear + occupancy_value: i8 + regions: Vec + expires_at: Time +``` + +Composition rules: + +1. Start from the latest static grid. +2. Drop expired dynamic observations before composing. +3. Rasterize active clear regions to free space. +4. Rasterize active obstacle regions to occupied space. +5. Active obstacle observations win over active clear observations for the same cell. +6. Unknown static cells stay unknown unless an active clear or obstacle observation covers them. + +`UPDATE_RESET` removes observations from the same source and map without disturbing observations from other robots. For example, if `robot_1/local_costmap` resets its obstacle layer, active observations from `robot_2/local_costmap` remain in the composed grid. + +The server does not use the full `-/errors` component pattern from the broader topic taxonomy. For transient observations, TTL expiration is the primary recovery mechanism. + +# Implemented Test Coverage + +The Rust tests cover: + +* composing obstacle regions over a static map +* point regions with non-zero map origins and non-1.0 resolutions +* polygon regions, out-of-bounds regions, and malformed region point arrays +* pruning expired observations by TTL +* active obstacle observations winning over active clear observations +* reset updates removing observations from the same source and map +* multiple robot sources being stitched into one composed grid + +The `rmf_layered_map_server_demo` package includes a small publisher that sends a static map and a temporary obstacle update. Its launch file starts the layered map server and RViz on `/map`, so developers can see the dynamic layer appear and decay without needing the full Nav2 or replanning loop. + +The `rmf_layered_map_server_test` package adds a launch test for the ROS topic contract: `/map/static` and `/map/region_updates` are consumed, `/map` is published, reset updates remove active observations, and obstacles are removed after TTL expiry. From e22d587cd39d0768dcd40d7599a056c3a9019417 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 9 Jul 2026 02:26:32 +0200 Subject: [PATCH 02/15] feat: add layered map observation messages Signed-off-by: SamuelFoo --- rmf_layered_map_msgs/CMakeLists.txt | 32 +++++++++++++++++++ .../msg/MapObservationSource.msg | 21 ++++++++++++ rmf_layered_map_msgs/msg/MapRegionUpdate.msg | 31 ++++++++++++++++++ rmf_layered_map_msgs/package.xml | 27 ++++++++++++++++ 4 files changed, 111 insertions(+) create mode 100644 rmf_layered_map_msgs/CMakeLists.txt create mode 100644 rmf_layered_map_msgs/msg/MapObservationSource.msg create mode 100644 rmf_layered_map_msgs/msg/MapRegionUpdate.msg create mode 100644 rmf_layered_map_msgs/package.xml diff --git a/rmf_layered_map_msgs/CMakeLists.txt b/rmf_layered_map_msgs/CMakeLists.txt new file mode 100644 index 0000000..c889e12 --- /dev/null +++ b/rmf_layered_map_msgs/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.5) + +project(rmf_layered_map_msgs) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # we dont use add_compile_options with pedantic in message packages + # because the Python C extensions dont comply with it + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic") +endif() + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rmf_prototype_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +set(msg_files + "msg/MapObservationSource.msg" + "msg/MapRegionUpdate.msg" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + DEPENDENCIES builtin_interfaces rmf_prototype_msgs + ADD_LINTER_TESTS +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/rmf_layered_map_msgs/msg/MapObservationSource.msg b/rmf_layered_map_msgs/msg/MapObservationSource.msg new file mode 100644 index 0000000..042677a --- /dev/null +++ b/rmf_layered_map_msgs/msg/MapObservationSource.msg @@ -0,0 +1,21 @@ +# MapObservationSource.msg + +# Unique source for this stream of observations. For a robot-local node this can +# be the robot namespace plus a stable sensor or node identifier. +string source_id + +# Robot that produced this observation, if the source is tied to a robot. This +# may be empty for fixed sensors or synthetic map layers. +string robot_name + +# Map or level that the observations belong to. +string map_name + +# Coordinate frame for the regions. +string frame_id + +# Time when this observation snapshot was produced. +builtin_interfaces/Time stamp + +# Fallback time-to-live for updates from this source. +builtin_interfaces/Duration default_ttl diff --git a/rmf_layered_map_msgs/msg/MapRegionUpdate.msg b/rmf_layered_map_msgs/msg/MapRegionUpdate.msg new file mode 100644 index 0000000..68f983a --- /dev/null +++ b/rmf_layered_map_msgs/msg/MapRegionUpdate.msg @@ -0,0 +1,31 @@ +# MapRegionUpdate.msg + +# The regions should be interpreted as temporary occupied space. +uint8 UPDATE_OBSTACLE=1 + +# The regions should be interpreted as temporary free space. +uint8 UPDATE_CLEAR=2 + +# The update should remove previous temporary observations from this source. +uint8 UPDATE_RESET=3 + +# Metadata describing where the observation came from. +MapObservationSource source + +# UPDATE_OBSTACLE, UPDATE_CLEAR, or UPDATE_RESET. +uint8 update_type + +# OccupancyGrid-compatible value to write when this update is rasterized. +# +# Obstacle updates with values less than or equal to zero are treated as 100. +# Clear updates with negative values are treated as 0. +int8 occupancy_value + +# Time-to-live for this update. If this is zero or negative, source.default_ttl +# is used instead. If both are zero or negative, the map server's configured +# default TTL is used. +builtin_interfaces/Duration ttl + +# 2D geometric observation patches. Empty regions are valid only for +# UPDATE_RESET. +rmf_prototype_msgs/Region[] regions diff --git a/rmf_layered_map_msgs/package.xml b/rmf_layered_map_msgs/package.xml new file mode 100644 index 0000000..1b334e7 --- /dev/null +++ b/rmf_layered_map_msgs/package.xml @@ -0,0 +1,27 @@ + + + + rmf_layered_map_msgs + 0.0.1 + Messages for publishing layered occupancy map observations. + Arjo Chakravarty + Apache License 2.0 + + ament_cmake + rosidl_default_generators + + builtin_interfaces + rmf_prototype_msgs + + builtin_interfaces + rmf_prototype_msgs + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + From 2165d748c4880806eb4b0d55ec4d3e71516645d2 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 9 Jul 2026 02:28:52 +0200 Subject: [PATCH 03/15] feat: add layered map server Signed-off-by: SamuelFoo --- map_server/rmf_layered_map_server/.gitignore | 1 + map_server/rmf_layered_map_server/Cargo.toml | 8 + map_server/rmf_layered_map_server/package.xml | 23 + map_server/rmf_layered_map_server/src/lib.rs | 869 ++++++++++++++++++ map_server/rmf_layered_map_server/src/main.rs | 32 + 5 files changed, 933 insertions(+) create mode 100644 map_server/rmf_layered_map_server/.gitignore create mode 100644 map_server/rmf_layered_map_server/Cargo.toml create mode 100644 map_server/rmf_layered_map_server/package.xml create mode 100644 map_server/rmf_layered_map_server/src/lib.rs create mode 100644 map_server/rmf_layered_map_server/src/main.rs diff --git a/map_server/rmf_layered_map_server/.gitignore b/map_server/rmf_layered_map_server/.gitignore new file mode 100644 index 0000000..2f7896d --- /dev/null +++ b/map_server/rmf_layered_map_server/.gitignore @@ -0,0 +1 @@ +target/ diff --git a/map_server/rmf_layered_map_server/Cargo.toml b/map_server/rmf_layered_map_server/Cargo.toml new file mode 100644 index 0000000..a2dd492 --- /dev/null +++ b/map_server/rmf_layered_map_server/Cargo.toml @@ -0,0 +1,8 @@ +[package] +name = "rmf_layered_map_server" +version = "0.1.0" +edition = "2021" + +[dependencies] +rclrs = "*" +ros-env = "0.2.0" diff --git a/map_server/rmf_layered_map_server/package.xml b/map_server/rmf_layered_map_server/package.xml new file mode 100644 index 0000000..ce55aec --- /dev/null +++ b/map_server/rmf_layered_map_server/package.xml @@ -0,0 +1,23 @@ + + + + rmf_layered_map_server + 0.0.1 + A layered map server for RMF next generation prototype. + Arjo Chakravarty + Apache License 2.0 + + ament_cargo + + rclrs + rmf_layered_map_msgs + rmf_prototype_msgs + builtin_interfaces + geometry_msgs + nav_msgs + std_msgs + + + ament_cargo + + diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs new file mode 100644 index 0000000..ebeae53 --- /dev/null +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -0,0 +1,869 @@ +// 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. + +use rclrs::{IntoPrimitiveOptions, Node, PrimitiveOptions}; +use ros_env::{ + builtin_interfaces::msg::{Duration as RosDuration, Time as RosTime}, + nav_msgs::msg::OccupancyGrid, + rmf_layered_map_msgs::msg::MapRegionUpdate, + rmf_prototype_msgs::msg::Region, +}; +use std::time::Duration; + +const NANOS_PER_SECOND: i128 = 1_000_000_000; +const MAP_QOS_DEPTH: u32 = 10; + +#[derive(Clone, Copy, Debug, PartialEq, Eq)] +enum DynamicUpdateType { + Obstacle, + Clear, +} + +#[derive(Clone, Debug)] +struct DynamicObservation { + source_id: String, + map_name: String, + update_type: DynamicUpdateType, + occupancy_value: i8, + regions: Vec, + expires_at_nsec: i128, +} + +#[derive(Clone, Debug)] +pub struct LayeredMap { + static_grid: Option, + dynamic_observations: Vec, + default_ttl_nsec: i128, + revision: u64, +} + +impl LayeredMap { + pub fn new(default_ttl: Duration) -> Self { + Self { + static_grid: None, + dynamic_observations: Vec::new(), + default_ttl_nsec: duration_to_nsec(default_ttl), + revision: 0, + } + } + + pub fn revision(&self) -> u64 { + self.revision + } + + pub fn dynamic_observation_count(&self) -> usize { + self.dynamic_observations.len() + } + + pub fn set_static_map(&mut self, mut grid: OccupancyGrid) { + normalize_grid_data(&mut grid); + self.static_grid = Some(grid); + self.revision = self.revision.wrapping_add(1); + } + + pub fn ingest_region_update(&mut self, update: MapRegionUpdate, now_nsec: i128) -> bool { + if update.update_type == MapRegionUpdate::UPDATE_RESET { + let before = self.dynamic_observations.len(); + self.dynamic_observations.retain(|obs| { + obs.source_id != update.source.source_id || obs.map_name != update.source.map_name + }); + let changed = before != self.dynamic_observations.len(); + if changed { + self.revision = self.revision.wrapping_add(1); + } + return changed; + } + + let update_type = match update.update_type { + MapRegionUpdate::UPDATE_OBSTACLE => DynamicUpdateType::Obstacle, + MapRegionUpdate::UPDATE_CLEAR => DynamicUpdateType::Clear, + _ => return false, + }; + + if update.regions.is_empty() { + return false; + } + + let ttl_nsec = first_positive_duration_nsec(&update.ttl) + .or_else(|| first_positive_duration_nsec(&update.source.default_ttl)) + .unwrap_or(self.default_ttl_nsec); + if ttl_nsec <= 0 { + return false; + } + + let stamp_nsec = if is_zero_time(&update.source.stamp) { + now_nsec + } else { + time_to_nsec(&update.source.stamp) + }; + let expires_at_nsec = stamp_nsec + ttl_nsec; + if expires_at_nsec <= now_nsec { + return false; + } + + let occupancy_value = match update_type { + DynamicUpdateType::Obstacle if update.occupancy_value <= 0 => 100, + DynamicUpdateType::Obstacle => update.occupancy_value.clamp(1, 100), + DynamicUpdateType::Clear if update.occupancy_value < 0 => 0, + DynamicUpdateType::Clear => update.occupancy_value.clamp(0, 100), + }; + + self.dynamic_observations.push(DynamicObservation { + source_id: update.source.source_id, + map_name: update.source.map_name, + update_type, + occupancy_value, + regions: update.regions, + expires_at_nsec, + }); + self.revision = self.revision.wrapping_add(1); + true + } + + pub fn prune_expired(&mut self, now_nsec: i128) -> bool { + let before = self.dynamic_observations.len(); + self.dynamic_observations + .retain(|obs| obs.expires_at_nsec > now_nsec); + let changed = before != self.dynamic_observations.len(); + if changed { + self.revision = self.revision.wrapping_add(1); + } + changed + } + + pub fn compose(&self) -> Option { + let mut composed = self.static_grid.clone()?; + normalize_grid_data(&mut composed); + + for observation in self + .dynamic_observations + .iter() + .filter(|obs| obs.update_type == DynamicUpdateType::Clear) + { + rasterize_observation(&mut composed, observation); + } + + for observation in self + .dynamic_observations + .iter() + .filter(|obs| obs.update_type == DynamicUpdateType::Obstacle) + { + rasterize_observation(&mut composed, observation); + } + + Some(composed) + } +} + +impl Default for LayeredMap { + fn default() -> Self { + Self::new(Duration::from_secs(30)) + } +} + +#[derive(Clone, Debug)] +pub struct LayeredMapServerConfig { + pub static_map_topic: String, + pub region_updates_topic: String, + pub composed_map_topic: String, + pub default_ttl: Duration, + pub publish_period: Duration, +} + +impl Default for LayeredMapServerConfig { + fn default() -> Self { + Self { + static_map_topic: "/map/static".to_string(), + region_updates_topic: "/map/region_updates".to_string(), + composed_map_topic: "/map".to_string(), + default_ttl: Duration::from_secs(30), + publish_period: Duration::from_millis(250), + } + } +} + +pub struct LayeredMapServer { + node: Node, + map: LayeredMap, + map_publisher: rclrs::Publisher, + last_published_revision: Option, +} + +impl LayeredMapServer { + pub fn new( + node: Node, + config: &LayeredMapServerConfig, + ) -> Result> { + let map_publisher = + node.create_publisher::(composed_map_qos(&config.composed_map_topic))?; + + Ok(Self { + node, + map: LayeredMap::new(config.default_ttl), + map_publisher, + last_published_revision: None, + }) + } + + fn handle_static_map(&mut self, msg: OccupancyGrid) { + self.map.set_static_map(msg); + rclrs::log!( + self.node.logger(), + "Received static map, revision {}", + self.map.revision() + ); + self.publish_if_changed(); + } + + fn handle_region_update(&mut self, msg: MapRegionUpdate) { + let now_nsec = self.now_nsec(); + if self.map.ingest_region_update(msg, now_nsec) { + rclrs::log!( + self.node.logger(), + "Accepted region update, revision {}, active observations {}", + self.map.revision(), + self.map.dynamic_observation_count() + ); + self.publish_if_changed(); + } + } + + fn prune_expired(&mut self) { + let now_nsec = self.now_nsec(); + if self.map.prune_expired(now_nsec) { + rclrs::log!( + self.node.logger(), + "Pruned expired observations, revision {}, active observations {}", + self.map.revision(), + self.map.dynamic_observation_count() + ); + self.publish_if_changed(); + } + } + + fn publish_if_changed(&mut self) { + if self.last_published_revision == Some(self.map.revision()) { + return; + } + + let Some(composed) = self.map.compose() else { + return; + }; + + match self.map_publisher.publish(&composed) { + Ok(()) => { + self.last_published_revision = Some(self.map.revision()); + rclrs::log!( + self.node.logger(), + "Published composed map {}x{} @ {}m/cell", + composed.info.width, + composed.info.height, + composed.info.resolution + ); + } + Err(err) => { + rclrs::log_error!( + self.node.logger(), + "Failed to publish composed map: {:?}", + err + ); + } + } + } + + fn now_nsec(&self) -> i128 { + i128::from(self.node.get_clock().now().nsec) + } +} + +pub struct LayeredMapServerRunning { + pub worker: rclrs::Worker, + // Keep the ROS handles alive for as long as the server is running. + pub static_map_subscription: rclrs::WorkerSubscription, + pub region_update_subscription: rclrs::WorkerSubscription, + pub prune_timer: Box, +} + +pub fn start_layered_map_server( + node: Node, + config: LayeredMapServerConfig, +) -> Result> { + let static_map_topic = config.static_map_topic.clone(); + let region_updates_topic = config.region_updates_topic.clone(); + let publish_period = config.publish_period; + let worker = node.create_worker(LayeredMapServer::new(node.clone(), &config)?); + + let static_map_subscription = worker.create_subscription::( + static_map_qos(&static_map_topic), + |server: &mut LayeredMapServer, msg: OccupancyGrid| { + server.handle_static_map(msg); + }, + )?; + + let region_update_subscription = worker.create_subscription::( + region_update_qos(®ion_updates_topic), + |server: &mut LayeredMapServer, msg: MapRegionUpdate| { + server.handle_region_update(msg); + }, + )?; + + let prune_timer = + worker.create_timer_repeating(publish_period, |server: &mut LayeredMapServer| { + server.prune_expired(); + })?; + + Ok(LayeredMapServerRunning { + worker, + static_map_subscription, + region_update_subscription, + prune_timer: Box::new(prune_timer), + }) +} + +fn static_map_qos(topic: &str) -> PrimitiveOptions<'_> { + topic.keep_last(MAP_QOS_DEPTH).transient_local().reliable() +} + +fn composed_map_qos(topic: &str) -> PrimitiveOptions<'_> { + topic.keep_last(MAP_QOS_DEPTH).transient_local().reliable() +} + +fn region_update_qos(topic: &str) -> PrimitiveOptions<'_> { + topic.keep_last(MAP_QOS_DEPTH).reliable() +} + +fn normalize_grid_data(grid: &mut OccupancyGrid) { + let Some(expected_len) = grid_len(grid) else { + grid.data.clear(); + return; + }; + + if grid.data.len() < expected_len { + grid.data.resize(expected_len, -1); + } else if grid.data.len() > expected_len { + grid.data.truncate(expected_len); + } +} + +fn grid_len(grid: &OccupancyGrid) -> Option { + let width = usize::try_from(grid.info.width).ok()?; + let height = usize::try_from(grid.info.height).ok()?; + width.checked_mul(height) +} + +fn rasterize_observation(grid: &mut OccupancyGrid, observation: &DynamicObservation) { + for region in &observation.regions { + for index in rasterized_indices(grid, region) { + if let Some(cell) = grid.data.get_mut(index) { + *cell = observation.occupancy_value; + } + } + } +} + +fn rasterized_indices(grid: &OccupancyGrid, region: &Region) -> Vec { + let Some((width, height, resolution, origin_x, origin_y)) = grid_geometry(grid) else { + return Vec::new(); + }; + + if region.points.len() < 2 || region.points.len() % 2 != 0 { + return Vec::new(); + } + + if region.hint == Region::HINT_POINT || region.points.len() == 2 { + return point_index( + region.points[0], + region.points[1], + width, + height, + resolution, + origin_x, + origin_y, + ) + .into_iter() + .collect(); + } + + if region.hint == Region::HINT_AXIS_ALIGNED_RECTANGLE && region.points.len() >= 4 { + let pairs = point_pairs(region); + let (min_x, min_y, max_x, max_y) = bounds(&pairs); + return indices_in_bounds( + width, height, resolution, origin_x, origin_y, min_x, min_y, max_x, max_y, + ); + } + + let pairs = point_pairs(region); + if pairs.len() < 3 { + let (min_x, min_y, max_x, max_y) = bounds(&pairs); + return indices_in_bounds( + width, height, resolution, origin_x, origin_y, min_x, min_y, max_x, max_y, + ); + } + + let (min_x, min_y, max_x, max_y) = bounds(&pairs); + indices_in_bounds( + width, height, resolution, origin_x, origin_y, min_x, min_y, max_x, max_y, + ) + .into_iter() + .filter(|index| { + let x = index % width; + let y = index / width; + let world_x = origin_x + (x as f64 + 0.5) * resolution; + let world_y = origin_y + (y as f64 + 0.5) * resolution; + point_in_polygon(world_x, world_y, &pairs) + }) + .collect() +} + +fn grid_geometry(grid: &OccupancyGrid) -> Option<(usize, usize, f64, f64, f64)> { + if grid.info.width == 0 || grid.info.height == 0 || grid.info.resolution <= 0.0 { + return None; + } + + Some(( + usize::try_from(grid.info.width).ok()?, + usize::try_from(grid.info.height).ok()?, + f64::from(grid.info.resolution), + grid.info.origin.position.x, + grid.info.origin.position.y, + )) +} + +fn point_index( + world_x: f32, + world_y: f32, + width: usize, + height: usize, + resolution: f64, + origin_x: f64, + origin_y: f64, +) -> Option { + let cell_x = ((f64::from(world_x) - origin_x) / resolution).floor() as isize; + let cell_y = ((f64::from(world_y) - origin_y) / resolution).floor() as isize; + if cell_x < 0 || cell_y < 0 { + return None; + } + + let cell_x = usize::try_from(cell_x).ok()?; + let cell_y = usize::try_from(cell_y).ok()?; + if cell_x >= width || cell_y >= height { + return None; + } + + Some(cell_y * width + cell_x) +} + +fn point_pairs(region: &Region) -> Vec<(f64, f64)> { + region + .points + .chunks_exact(2) + .map(|point| (f64::from(point[0]), f64::from(point[1]))) + .collect() +} + +fn bounds(points: &[(f64, f64)]) -> (f64, f64, f64, f64) { + let mut min_x = f64::INFINITY; + let mut min_y = f64::INFINITY; + let mut max_x = f64::NEG_INFINITY; + let mut max_y = f64::NEG_INFINITY; + + for (x, y) in points { + min_x = min_x.min(*x); + min_y = min_y.min(*y); + max_x = max_x.max(*x); + max_y = max_y.max(*y); + } + + (min_x, min_y, max_x, max_y) +} + +fn indices_in_bounds( + width: usize, + height: usize, + resolution: f64, + origin_x: f64, + origin_y: f64, + min_x: f64, + min_y: f64, + max_x: f64, + max_y: f64, +) -> Vec { + if min_x > max_x || min_y > max_y { + return Vec::new(); + } + + let start_x = (((min_x - origin_x) / resolution).floor() as isize).max(0) as usize; + let start_y = (((min_y - origin_y) / resolution).floor() as isize).max(0) as usize; + let end_x = (((max_x - origin_x) / resolution).ceil() as isize) + .max(0) + .min(width as isize) as usize; + let end_y = (((max_y - origin_y) / resolution).ceil() as isize) + .max(0) + .min(height as isize) as usize; + + let mut indices = Vec::new(); + for y in start_y..end_y { + for x in start_x..end_x { + indices.push(y * width + x); + } + } + indices +} + +fn point_in_polygon(x: f64, y: f64, polygon: &[(f64, f64)]) -> bool { + let mut inside = false; + let mut j = polygon.len() - 1; + + for i in 0..polygon.len() { + let (xi, yi) = polygon[i]; + let (xj, yj) = polygon[j]; + let crosses = (yi > y) != (yj > y); + if crosses { + let x_intersection = (xj - xi) * (y - yi) / (yj - yi) + xi; + if x < x_intersection { + inside = !inside; + } + } + j = i; + } + + inside +} + +fn time_to_nsec(time: &RosTime) -> i128 { + i128::from(time.sec) * NANOS_PER_SECOND + i128::from(time.nanosec) +} + +fn duration_msg_to_nsec(duration: &RosDuration) -> i128 { + i128::from(duration.sec) * NANOS_PER_SECOND + i128::from(duration.nanosec) +} + +fn duration_to_nsec(duration: Duration) -> i128 { + i128::from(duration.as_secs()) * NANOS_PER_SECOND + i128::from(duration.subsec_nanos()) +} + +fn first_positive_duration_nsec(duration: &RosDuration) -> Option { + let nsec = duration_msg_to_nsec(duration); + (nsec > 0).then_some(nsec) +} + +fn is_zero_time(time: &RosTime) -> bool { + time.sec == 0 && time.nanosec == 0 +} + +#[cfg(test)] +mod tests { + use super::*; + use ros_env::{ + builtin_interfaces::msg::Duration as RosDuration, + geometry_msgs::msg::{Point, Pose, Quaternion}, + nav_msgs::msg::MapMetaData, + rmf_layered_map_msgs::msg::{MapObservationSource, MapRegionUpdate}, + rmf_prototype_msgs::msg::Region, + }; + + fn static_grid(width: u32, height: u32, value: i8) -> OccupancyGrid { + OccupancyGrid { + info: MapMetaData { + resolution: 1.0, + width, + height, + origin: Pose { + position: Point { + x: 0.0, + y: 0.0, + z: 0.0, + }, + orientation: Quaternion { + w: 1.0, + ..Default::default() + }, + }, + ..Default::default() + }, + data: vec![value; (width * height) as usize], + ..Default::default() + } + } + + fn source() -> MapObservationSource { + source_with_id("robot_1/local_costmap") + } + + fn source_with_id(source_id: &str) -> MapObservationSource { + MapObservationSource { + source_id: source_id.to_string(), + robot_name: "robot_1".to_string(), + map_name: "test_map".to_string(), + frame_id: "map".to_string(), + default_ttl: RosDuration { + sec: 10, + nanosec: 0, + }, + ..Default::default() + } + } + + fn rectangle(min_x: f32, min_y: f32, max_x: f32, max_y: f32) -> Region { + Region { + hint: Region::HINT_AXIS_ALIGNED_RECTANGLE, + points: vec![min_x, min_y, max_x, max_y], + } + } + + fn point(x: f32, y: f32) -> Region { + Region { + hint: Region::HINT_POINT, + points: vec![x, y], + } + } + + fn polygon(points: Vec) -> Region { + Region { + hint: Region::HINT_POLYGON, + points, + } + } + + fn update(update_type: u8, regions: Vec) -> MapRegionUpdate { + MapRegionUpdate { + source: source(), + update_type, + regions, + ..Default::default() + } + } + + fn update_from(source_id: &str, update_type: u8, regions: Vec) -> MapRegionUpdate { + MapRegionUpdate { + source: source_with_id(source_id), + update_type, + regions, + ..Default::default() + } + } + + #[test] + fn obstacle_regions_are_composed_over_static_map() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(5, 5, 0)); + + assert!(map.ingest_region_update( + update( + MapRegionUpdate::UPDATE_OBSTACLE, + vec![rectangle(2.0, 1.0, 3.0, 3.0)] + ), + 1_000, + )); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 5 + 2], 100); + assert_eq!(composed.data[2 * 5 + 2], 100); + assert_eq!(composed.data[0], 0); + } + + #[test] + fn point_regions_respect_map_origin_and_resolution() { + let mut static_map = static_grid(4, 4, 0); + static_map.info.resolution = 0.5; + static_map.info.origin.position.x = -1.0; + static_map.info.origin.position.y = -1.0; + + let mut map = LayeredMap::default(); + map.set_static_map(static_map); + + assert!(map.ingest_region_update( + update(MapRegionUpdate::UPDATE_OBSTACLE, vec![point(-0.25, 0.25)]), + 0, + )); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[2 * 4 + 1], 100); + assert_eq!(composed.data[0], 0); + } + + #[test] + fn polygon_regions_fill_only_cells_inside_the_polygon() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(4, 4, 0)); + + assert!(map.ingest_region_update( + update( + MapRegionUpdate::UPDATE_OBSTACLE, + vec![polygon(vec![1.0, 1.0, 3.0, 1.0, 3.0, 3.0, 1.0, 3.0])] + ), + 0, + )); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 4 + 1], 100); + assert_eq!(composed.data[1 * 4 + 2], 100); + assert_eq!(composed.data[2 * 4 + 1], 100); + assert_eq!(composed.data[2 * 4 + 2], 100); + assert_eq!(composed.data[0], 0); + assert_eq!(composed.data[3 * 4 + 3], 0); + } + + #[test] + fn out_of_bounds_regions_do_not_touch_the_grid() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + + assert!(map.ingest_region_update( + update( + MapRegionUpdate::UPDATE_OBSTACLE, + vec![rectangle(10.0, 10.0, 11.0, 11.0)] + ), + 0, + )); + + let composed = map.compose().unwrap(); + assert!(composed.data.iter().all(|cell| *cell == 0)); + } + + #[test] + fn invalid_region_points_are_ignored() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + + assert!(map.ingest_region_update( + update( + MapRegionUpdate::UPDATE_OBSTACLE, + vec![Region { + hint: Region::HINT_POLYGON, + points: vec![1.0, 1.0, 2.0], + }] + ), + 0, + )); + + let composed = map.compose().unwrap(); + assert!(composed.data.iter().all(|cell| *cell == 0)); + } + + #[test] + fn expired_observations_are_removed() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + + assert!(map.ingest_region_update( + update( + MapRegionUpdate::UPDATE_OBSTACLE, + vec![rectangle(1.0, 1.0, 2.0, 2.0)] + ), + 0, + )); + assert_eq!(map.dynamic_observation_count(), 1); + + assert!(map.prune_expired(11 * NANOS_PER_SECOND)); + assert_eq!(map.dynamic_observation_count(), 0); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 3 + 1], 0); + } + + #[test] + fn obstacles_win_over_clear_regions() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(5, 5, -1)); + + assert!(map.ingest_region_update( + update( + MapRegionUpdate::UPDATE_CLEAR, + vec![rectangle(1.0, 1.0, 4.0, 4.0)] + ), + 0, + )); + assert!(map.ingest_region_update( + update( + MapRegionUpdate::UPDATE_OBSTACLE, + vec![rectangle(2.0, 2.0, 3.0, 3.0)] + ), + 0, + )); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 5 + 1], 0); + assert_eq!(composed.data[2 * 5 + 2], 100); + assert_eq!(composed.data[0], -1); + } + + #[test] + fn reset_removes_observations_from_same_source_and_map() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + + assert!(map.ingest_region_update( + update( + MapRegionUpdate::UPDATE_OBSTACLE, + vec![rectangle(1.0, 1.0, 2.0, 2.0)] + ), + 0, + )); + + assert!(map.ingest_region_update( + MapRegionUpdate { + source: source(), + update_type: MapRegionUpdate::UPDATE_RESET, + ..Default::default() + }, + 0, + )); + + assert_eq!(map.dynamic_observation_count(), 0); + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 3 + 1], 0); + } + + #[test] + fn multiple_robot_sources_are_stitched_into_one_composed_grid() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(5, 5, 0)); + + assert!(map.ingest_region_update( + update_from( + "robot_1/local_costmap", + MapRegionUpdate::UPDATE_OBSTACLE, + vec![rectangle(1.0, 1.0, 2.0, 2.0)] + ), + 0, + )); + assert!(map.ingest_region_update( + update_from( + "robot_2/local_costmap", + MapRegionUpdate::UPDATE_OBSTACLE, + vec![rectangle(3.0, 3.0, 4.0, 4.0)] + ), + 0, + )); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 5 + 1], 100); + assert_eq!(composed.data[3 * 5 + 3], 100); + + assert!(map.ingest_region_update( + MapRegionUpdate { + source: source_with_id("robot_1/local_costmap"), + update_type: MapRegionUpdate::UPDATE_RESET, + ..Default::default() + }, + 0, + )); + + let composed = map.compose().unwrap(); + assert_eq!(map.dynamic_observation_count(), 1); + assert_eq!(composed.data[1 * 5 + 1], 0); + assert_eq!(composed.data[3 * 5 + 3], 100); + } +} diff --git a/map_server/rmf_layered_map_server/src/main.rs b/map_server/rmf_layered_map_server/src/main.rs new file mode 100644 index 0000000..e8ae638 --- /dev/null +++ b/map_server/rmf_layered_map_server/src/main.rs @@ -0,0 +1,32 @@ +// 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. + +use rclrs::{Context, CreateBasicExecutor, SpinOptions}; +use rmf_layered_map_server::{start_layered_map_server, LayeredMapServerConfig}; +use std::sync::Arc; + +fn main() -> Result<(), Box> { + let context = Context::default_from_env().unwrap(); + let mut executor = context.create_basic_executor(); + let node = Arc::new(executor.create_node("layered_map_server")?); + + let _server = start_layered_map_server(Arc::clone(&node), LayeredMapServerConfig::default())?; + + rclrs::log!( + node.logger(), + "Layered map server started. Composing /map/static and /map/region_updates into /map." + ); + executor.spin(SpinOptions::default()); + Ok(()) +} From 6b8794ba99fc6e6ee0073d280970f502d47e01ee Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 9 Jul 2026 02:29:13 +0200 Subject: [PATCH 04/15] feat: add layered map demo Signed-off-by: SamuelFoo --- map_server/README.md | 13 +++ map_server/rmf_layered_map_server/README.md | 31 +++++ .../launch/demo.launch.py | 77 +++++++++++++ .../rmf_layered_map_server_demo/package.xml | 24 ++++ .../resource/rmf_layered_map_server_demo | 1 + .../rmf_layered_map_server_demo/__init__.py | 13 +++ .../demo_publisher.py | 109 ++++++++++++++++++ .../rviz/layered_map.rviz | 104 +++++++++++++++++ .../rmf_layered_map_server_demo/setup.cfg | 4 + .../rmf_layered_map_server_demo/setup.py | 45 ++++++++ 10 files changed, 421 insertions(+) create mode 100644 map_server/README.md create mode 100644 map_server/rmf_layered_map_server/README.md create mode 100644 map_server/rmf_layered_map_server_demo/launch/demo.launch.py create mode 100644 map_server/rmf_layered_map_server_demo/package.xml create mode 100644 map_server/rmf_layered_map_server_demo/resource/rmf_layered_map_server_demo create mode 100644 map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/__init__.py create mode 100644 map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py create mode 100644 map_server/rmf_layered_map_server_demo/rviz/layered_map.rviz create mode 100644 map_server/rmf_layered_map_server_demo/setup.cfg create mode 100644 map_server/rmf_layered_map_server_demo/setup.py diff --git a/map_server/README.md b/map_server/README.md new file mode 100644 index 0000000..883cd14 --- /dev/null +++ b/map_server/README.md @@ -0,0 +1,13 @@ +# Map Server + +This folder contains the layered map server packages. + +## Demo + +The demo launches the server, a small publisher that sends a static map and a temporary obstacle region, and RViz with a `Map` display on `/map`. + +```bash +ros2 launch rmf_layered_map_server_demo demo.launch.py +``` + +Use `use_rviz:=False` for a headless run. diff --git a/map_server/rmf_layered_map_server/README.md b/map_server/rmf_layered_map_server/README.md new file mode 100644 index 0000000..0e52f20 --- /dev/null +++ b/map_server/rmf_layered_map_server/README.md @@ -0,0 +1,31 @@ +# RMF Layered Map Server + +`rmf_layered_map_server` composes a static `nav_msgs/OccupancyGrid` with temporary observation regions into the `/map` topic consumed by the path server. + +Each robot or perception node publishes updates with its own `source_id`, and the server stitches all active observations into one composed global grid. Robot-mounted sources should also include `robot_name` metadata so the source can be correlated with the robot that observed it. + +Default topics: + +- `/map/static`: static `nav_msgs/OccupancyGrid` +- `/map/region_updates`: `rmf_layered_map_msgs/MapRegionUpdate` +- `/map`: composed `nav_msgs/OccupancyGrid` + +Dynamic observations expire according to their TTL, so transient local obstacles do not remain in the global planning map forever. A LiDAR or local costmap observer can keep refreshing an obstacle while it is still seen, then let the server decay it after the TTL. + +## Visualization Smoke Test + +Build and source the workspace: + +```bash +cd /home/dell/workspaces/mapf_ws +colcon build --packages-select rmf_layered_map_msgs rmf_layered_map_server rmf_layered_map_server_demo +source install/setup.bash +``` + +Start the demo: + +```bash +ros2 launch rmf_layered_map_server_demo demo.launch.py +``` + +The launch file starts RViz with a `Map` display on `/map` and the fixed frame set to `map`. The demo publisher sends a bordered static map and a temporary obstacle region with a 5 second TTL. The obstacle should appear in the composed map and then disappear when the TTL expires. diff --git a/map_server/rmf_layered_map_server_demo/launch/demo.launch.py b/map_server/rmf_layered_map_server_demo/launch/demo.launch.py new file mode 100644 index 0000000..f1dd771 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/launch/demo.launch.py @@ -0,0 +1,77 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import EmitEvent +from launch.actions import RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + package_share = get_package_share_directory('rmf_layered_map_server_demo') + rviz_config = LaunchConfiguration('rviz_config') + use_rviz = LaunchConfiguration('use_rviz') + + declare_rviz_config = DeclareLaunchArgument( + 'rviz_config', + default_value=os.path.join(package_share, 'rviz', 'layered_map.rviz'), + description='Full path to the RViz config file to use.', + ) + declare_use_rviz = DeclareLaunchArgument( + 'use_rviz', + default_value='True', + description='Whether to start RViz.', + ) + + layered_map_server = Node( + package='rmf_layered_map_server', + executable='rmf_layered_map_server', + output='screen', + ) + demo_publisher = Node( + package='rmf_layered_map_server_demo', + executable='layered_map_demo_publisher', + output='screen', + ) + rviz = Node( + condition=IfCondition(use_rviz), + package='rviz2', + executable='rviz2', + arguments=['-d', rviz_config], + output='screen', + ) + rviz_exit_handler = RegisterEventHandler( + condition=IfCondition(use_rviz), + event_handler=OnProcessExit( + target_action=rviz, + on_exit=EmitEvent(event=Shutdown(reason='rviz exited')), + ), + ) + + return LaunchDescription([ + declare_rviz_config, + declare_use_rviz, + layered_map_server, + demo_publisher, + rviz, + rviz_exit_handler, + ]) diff --git a/map_server/rmf_layered_map_server_demo/package.xml b/map_server/rmf_layered_map_server_demo/package.xml new file mode 100644 index 0000000..6ad8738 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/package.xml @@ -0,0 +1,24 @@ + + + + rmf_layered_map_server_demo + 0.0.1 + Demo launch tools for the RMF layered map server. + Arjo Chakravarty + Apache License 2.0 + + ament_python + + rclpy + ament_index_python + geometry_msgs + nav_msgs + rmf_layered_map_msgs + rmf_layered_map_server + rmf_prototype_msgs + rviz2 + + + ament_python + + diff --git a/map_server/rmf_layered_map_server_demo/resource/rmf_layered_map_server_demo b/map_server/rmf_layered_map_server_demo/resource/rmf_layered_map_server_demo new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/resource/rmf_layered_map_server_demo @@ -0,0 +1 @@ + diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/__init__.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/__init__.py new file mode 100644 index 0000000..22d2039 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/__init__.py @@ -0,0 +1,13 @@ +# 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. diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py new file mode 100644 index 0000000..675eac0 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py @@ -0,0 +1,109 @@ +# 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 nav_msgs.msg import OccupancyGrid +import rclpy +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from rmf_layered_map_msgs.msg import MapObservationSource, MapRegionUpdate +from rmf_prototype_msgs.msg import Region + + +class LayeredMapDemoPublisher(Node): + def __init__(self): + super().__init__('layered_map_demo_publisher') + + transient_qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.RELIABLE, + ) + reliable_qos = QoSProfile( + depth=10, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.RELIABLE, + ) + + self.static_map_publisher = self.create_publisher( + OccupancyGrid, + '/map/static', + transient_qos, + ) + self.region_update_publisher = self.create_publisher( + MapRegionUpdate, + '/map/region_updates', + reliable_qos, + ) + self.timer = self.create_timer(8.0, self.publish_demo) + self.publish_demo() + + def publish_demo(self): + self.static_map_publisher.publish(static_map()) + self.region_update_publisher.publish(obstacle_update()) + self.get_logger().info('Published demo obstacle with a 5 second TTL') + + +def static_map(): + width = 10 + height = 10 + data = [0] * (width * height) + + for x in range(width): + data[x] = 100 + data[(height - 1) * width + x] = 100 + + for y in range(height): + data[y * width] = 100 + data[y * width + width - 1] = 100 + + msg = OccupancyGrid() + msg.header.frame_id = 'map' + msg.info.resolution = 1.0 + msg.info.width = width + msg.info.height = height + msg.info.origin.orientation.w = 1.0 + msg.data = data + return msg + + +def obstacle_update(): + msg = MapRegionUpdate() + msg.source = MapObservationSource() + msg.source.source_id = 'demo/temporary_obstacle' + msg.source.robot_name = 'demo_robot' + msg.source.map_name = 'demo_map' + msg.source.frame_id = 'map' + msg.source.default_ttl.sec = 5 + msg.update_type = MapRegionUpdate.UPDATE_OBSTACLE + msg.occupancy_value = 100 + msg.ttl.sec = 5 + + region = Region() + region.hint = Region.HINT_AXIS_ALIGNED_RECTANGLE + region.points = [4.0, 4.0, 6.0, 6.0] + msg.regions.append(region) + return msg + + +def main(): + rclpy.init() + node = LayeredMapDemoPublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() diff --git a/map_server/rmf_layered_map_server_demo/rviz/layered_map.rviz b/map_server/rmf_layered_map_server_demo/rviz/layered_map.rviz new file mode 100644 index 0000000..1b7b71b --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rviz/layered_map.rviz @@ -0,0 +1,104 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Layered Map1 + Splitter Ratio: 0.5 + Tree Height: 595 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 20 + Reference Frame: + Value: true + - Alpha: 1 + Binary representation: false + Binary threshold: 100 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: true + Enabled: true + Name: Layered Map + Topic: + Depth: 1 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.01 + Scale: 20 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 5 + Y: 5 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 900 + Hide Left Dock: false + Hide Right Dock: false + Views: + collapsed: false + Width: 1200 + X: 60 + Y: 60 diff --git a/map_server/rmf_layered_map_server_demo/setup.cfg b/map_server/rmf_layered_map_server_demo/setup.cfg new file mode 100644 index 0000000..f755756 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rmf_layered_map_server_demo +[install] +install_scripts=$base/lib/rmf_layered_map_server_demo diff --git a/map_server/rmf_layered_map_server_demo/setup.py b/map_server/rmf_layered_map_server_demo/setup.py new file mode 100644 index 0000000..28bb275 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/setup.py @@ -0,0 +1,45 @@ +# 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. + +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'rmf_layered_map_server_demo' + +setup( + name=package_name, + version='0.0.1', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), + (os.path.join('share', package_name, 'rviz'), glob('rviz/*.rviz')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Arjo Chakravarty', + maintainer_email='arjoc@intrinsic.ai', + description='Demo launch tools for the RMF layered map server.', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'layered_map_demo_publisher = rmf_layered_map_server_demo.demo_publisher:main', + ], + }, +) From 2dd77900cd40d2d3a29484ba48540da76efe9322 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 9 Jul 2026 02:29:30 +0200 Subject: [PATCH 05/15] test: add layered map server launch test Signed-off-by: SamuelFoo --- .../rmf_layered_map_server_test/.gitignore | 2 + .../rmf_layered_map_server_test/package.xml | 27 +++ .../resource/rmf_layered_map_server_test | 1 + .../rmf_layered_map_server_test/__init__.py | 13 ++ .../rmf_layered_map_server_test/setup.cfg | 4 + .../rmf_layered_map_server_test/setup.py | 36 ++++ .../test/test_copyright.py | 23 ++ .../test/test_flake8.py | 25 +++ .../test/test_layered_map_server.py | 204 ++++++++++++++++++ .../test/test_pep257.py | 23 ++ 10 files changed, 358 insertions(+) create mode 100644 map_server/rmf_layered_map_server_test/.gitignore create mode 100644 map_server/rmf_layered_map_server_test/package.xml create mode 100644 map_server/rmf_layered_map_server_test/resource/rmf_layered_map_server_test create mode 100644 map_server/rmf_layered_map_server_test/rmf_layered_map_server_test/__init__.py create mode 100644 map_server/rmf_layered_map_server_test/setup.cfg create mode 100644 map_server/rmf_layered_map_server_test/setup.py create mode 100644 map_server/rmf_layered_map_server_test/test/test_copyright.py create mode 100644 map_server/rmf_layered_map_server_test/test/test_flake8.py create mode 100644 map_server/rmf_layered_map_server_test/test/test_layered_map_server.py create mode 100644 map_server/rmf_layered_map_server_test/test/test_pep257.py diff --git a/map_server/rmf_layered_map_server_test/.gitignore b/map_server/rmf_layered_map_server_test/.gitignore new file mode 100644 index 0000000..3193008 --- /dev/null +++ b/map_server/rmf_layered_map_server_test/.gitignore @@ -0,0 +1,2 @@ +.pytest_cache +test/__pycache__ diff --git a/map_server/rmf_layered_map_server_test/package.xml b/map_server/rmf_layered_map_server_test/package.xml new file mode 100644 index 0000000..48bded1 --- /dev/null +++ b/map_server/rmf_layered_map_server_test/package.xml @@ -0,0 +1,27 @@ + + + + rmf_layered_map_server_test + 0.0.1 + Launch tests for the RMF layered map server. + Arjo Chakravarty + Apache License 2.0 + + ament_python + + ament_copyright + ament_flake8 + ament_pep257 + launch_testing + launch_testing_ros + python3-pytest + rclpy + nav_msgs + rmf_layered_map_msgs + rmf_layered_map_server + rmf_prototype_msgs + + + ament_python + + diff --git a/map_server/rmf_layered_map_server_test/resource/rmf_layered_map_server_test b/map_server/rmf_layered_map_server_test/resource/rmf_layered_map_server_test new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/map_server/rmf_layered_map_server_test/resource/rmf_layered_map_server_test @@ -0,0 +1 @@ + diff --git a/map_server/rmf_layered_map_server_test/rmf_layered_map_server_test/__init__.py b/map_server/rmf_layered_map_server_test/rmf_layered_map_server_test/__init__.py new file mode 100644 index 0000000..22d2039 --- /dev/null +++ b/map_server/rmf_layered_map_server_test/rmf_layered_map_server_test/__init__.py @@ -0,0 +1,13 @@ +# 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. diff --git a/map_server/rmf_layered_map_server_test/setup.cfg b/map_server/rmf_layered_map_server_test/setup.cfg new file mode 100644 index 0000000..c8eabf7 --- /dev/null +++ b/map_server/rmf_layered_map_server_test/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rmf_layered_map_server_test +[install] +install_scripts=$base/lib/rmf_layered_map_server_test diff --git a/map_server/rmf_layered_map_server_test/setup.py b/map_server/rmf_layered_map_server_test/setup.py new file mode 100644 index 0000000..455d932 --- /dev/null +++ b/map_server/rmf_layered_map_server_test/setup.py @@ -0,0 +1,36 @@ +# 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 setuptools import find_packages +from setuptools import setup + +package_name = 'rmf_layered_map_server_test' + +setup( + name=package_name, + version='0.0.1', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Arjo Chakravarty', + maintainer_email='arjoc@intrinsic.ai', + description='Launch tests for the RMF layered map server.', + license='Apache License 2.0', + tests_require=['pytest'], +) diff --git a/map_server/rmf_layered_map_server_test/test/test_copyright.py b/map_server/rmf_layered_map_server_test/test/test_copyright.py new file mode 100644 index 0000000..f87f331 --- /dev/null +++ b/map_server/rmf_layered_map_server_test/test/test_copyright.py @@ -0,0 +1,23 @@ +# 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 ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0 diff --git a/map_server/rmf_layered_map_server_test/test/test_flake8.py b/map_server/rmf_layered_map_server_test/test/test_flake8.py new file mode 100644 index 0000000..fe96a35 --- /dev/null +++ b/map_server/rmf_layered_map_server_test/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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 ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py b/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py new file mode 100644 index 0000000..718807d --- /dev/null +++ b/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py @@ -0,0 +1,204 @@ +# 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. + +import time +import unittest + +from launch import LaunchDescription +import launch_ros +import launch_testing +from nav_msgs.msg import OccupancyGrid +import pytest +import rclpy +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from rmf_layered_map_msgs.msg import MapObservationSource, MapRegionUpdate +from rmf_prototype_msgs.msg import Region + + +@pytest.mark.launch_test +def generate_test_description(): + map_server = launch_ros.actions.Node( + package='rmf_layered_map_server', + executable='rmf_layered_map_server', + output='screen', + ) + + return LaunchDescription([ + map_server, + launch_testing.actions.ReadyToTest(), + ]), { + 'map_server': map_server, + } + + +class TestLayeredMapServer(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node('test_layered_map_server_node') + self.maps = [] + + transient_qos = QoSProfile( + depth=10, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.RELIABLE, + ) + reliable_qos = QoSProfile( + depth=10, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.RELIABLE, + ) + + self.static_map_publisher = self.node.create_publisher( + OccupancyGrid, + '/map/static', + qos_profile=transient_qos, + ) + self.region_update_publisher = self.node.create_publisher( + MapRegionUpdate, + '/map/region_updates', + qos_profile=reliable_qos, + ) + self.map_subscription = self.node.create_subscription( + OccupancyGrid, + '/map', + self.maps.append, + qos_profile=transient_qos, + ) + + def tearDown(self): + self.node.destroy_node() + + def wait_for(self, predicate, timeout=5.0): + deadline = time.time() + timeout + while time.time() < deadline: + rclpy.spin_once(self.node, timeout_sec=0.1) + if predicate(): + return True + return False + + def publish_until(self, publisher, msg, predicate, timeout=12.0): + deadline = time.time() + timeout + while time.time() < deadline: + publisher.publish(msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + if predicate(): + return True + return False + + def last_cell(self, x, y): + if not self.maps: + return None + latest = self.maps[-1] + return latest.data[y * latest.info.width + x] + + def test_region_update_is_composed_reset_and_expires(self): + time.sleep(2.0) + + static = static_map() + self.assertTrue( + self.publish_until( + self.static_map_publisher, + static, + lambda: len(self.maps) > 0, + ), + 'layered map server did not publish a composed map', + ) + self.assertEqual(self.maps[-1].info.width, 5) + self.assertEqual(self.maps[-1].info.height, 5) + self.assertEqual(self.last_cell(2, 2), 0) + + update = obstacle_update(ttl_sec=30) + self.assertTrue( + self.publish_until( + self.region_update_publisher, + update, + lambda: self.last_cell(2, 2) == 100, + ), + 'layered map server did not compose the obstacle region', + ) + + reset = reset_update() + self.assertTrue( + self.publish_until( + self.region_update_publisher, + reset, + lambda: self.last_cell(2, 2) == 0, + ), + 'layered map server did not reset the obstacle region', + ) + + update = obstacle_update() + self.assertTrue( + self.publish_until( + self.region_update_publisher, + update, + lambda: self.last_cell(2, 2) == 100, + ), + 'layered map server did not compose the obstacle region', + ) + self.assertTrue( + self.wait_for(lambda: self.last_cell(2, 2) == 0, timeout=4.0), + 'layered map server did not prune the expired obstacle', + ) + + +def static_map(): + msg = OccupancyGrid() + msg.header.frame_id = 'map' + msg.info.resolution = 1.0 + msg.info.width = 5 + msg.info.height = 5 + msg.info.origin.orientation.w = 1.0 + msg.data = [0] * 25 + return msg + + +def map_source(): + msg = MapObservationSource() + msg.source_id = 'test/obstacle' + msg.robot_name = 'test_robot' + msg.map_name = 'test_map' + msg.frame_id = 'map' + msg.default_ttl.sec = 1 + return msg + + +def obstacle_update(ttl_sec=1): + msg = MapRegionUpdate() + msg.source = map_source() + msg.source.default_ttl.sec = ttl_sec + msg.update_type = MapRegionUpdate.UPDATE_OBSTACLE + msg.occupancy_value = 100 + msg.ttl.sec = ttl_sec + + region = Region() + region.hint = Region.HINT_AXIS_ALIGNED_RECTANGLE + region.points = [2.0, 2.0, 3.0, 3.0] + msg.regions.append(region) + return msg + + +def reset_update(): + msg = MapRegionUpdate() + msg.source = map_source() + msg.update_type = MapRegionUpdate.UPDATE_RESET + return msg diff --git a/map_server/rmf_layered_map_server_test/test/test_pep257.py b/map_server/rmf_layered_map_server_test/test/test_pep257.py new file mode 100644 index 0000000..1b15a96 --- /dev/null +++ b/map_server/rmf_layered_map_server_test/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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 ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0 From 4adefb138303914b85574d842cc3462046579ca5 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 9 Jul 2026 20:33:36 +0200 Subject: [PATCH 06/15] feat: batch layered map observation messages 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 --- rmf_layered_map_msgs/CMakeLists.txt | 6 ++-- .../msg/MapObservationSource.msg | 15 ++++---- rmf_layered_map_msgs/msg/MapRegionPatch.msg | 26 ++++++++++++++ rmf_layered_map_msgs/msg/MapRegionUpdate.msg | 36 ++++++------------- rmf_layered_map_msgs/package.xml | 6 ++-- 5 files changed, 52 insertions(+), 37 deletions(-) create mode 100644 rmf_layered_map_msgs/msg/MapRegionPatch.msg diff --git a/rmf_layered_map_msgs/CMakeLists.txt b/rmf_layered_map_msgs/CMakeLists.txt index c889e12..3bdd01f 100644 --- a/rmf_layered_map_msgs/CMakeLists.txt +++ b/rmf_layered_map_msgs/CMakeLists.txt @@ -12,18 +12,20 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) -find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(rmf_prototype_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) set(msg_files "msg/MapObservationSource.msg" + "msg/MapRegionPatch.msg" "msg/MapRegionUpdate.msg" ) rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} - DEPENDENCIES builtin_interfaces rmf_prototype_msgs + DEPENDENCIES geometry_msgs rmf_prototype_msgs std_msgs ADD_LINTER_TESTS ) diff --git a/rmf_layered_map_msgs/msg/MapObservationSource.msg b/rmf_layered_map_msgs/msg/MapObservationSource.msg index 042677a..10c69db 100644 --- a/rmf_layered_map_msgs/msg/MapObservationSource.msg +++ b/rmf_layered_map_msgs/msg/MapObservationSource.msg @@ -1,5 +1,8 @@ # MapObservationSource.msg +# Frame and time for this observation snapshot. +std_msgs/Header header + # Unique source for this stream of observations. For a robot-local node this can # be the robot namespace plus a stable sensor or node identifier. string source_id @@ -11,11 +14,9 @@ string robot_name # Map or level that the observations belong to. string map_name -# Coordinate frame for the regions. -string frame_id - -# Time when this observation snapshot was produced. -builtin_interfaces/Time stamp +# 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 -# Fallback time-to-live for updates from this source. -builtin_interfaces/Duration default_ttl +# Fallback time-to-live in seconds for patches from this source. +float64 default_ttl_sec diff --git a/rmf_layered_map_msgs/msg/MapRegionPatch.msg b/rmf_layered_map_msgs/msg/MapRegionPatch.msg new file mode 100644 index 0000000..07a5800 --- /dev/null +++ b/rmf_layered_map_msgs/msg/MapRegionPatch.msg @@ -0,0 +1,26 @@ +# MapRegionPatch.msg + +# The regions should be interpreted as temporary occupied space. +uint8 UPDATE_OBSTACLE=1 + +# The regions should be interpreted as temporary free space. +uint8 UPDATE_CLEAR=2 + +# UPDATE_OBSTACLE or UPDATE_CLEAR. +uint8 update_type + +# OccupancyGrid-compatible value to write when this patch is rasterized. +# +# Obstacle patches with values less than or equal to zero are treated as 100. +# Clear patches with negative values are treated as 0. +int8 occupancy_value + +# Time-to-live in seconds for the regions in this patch. If this is zero or +# negative, source.default_ttl_sec is used instead. If both are zero or +# negative, the map server's configured default TTL is used. Clear patches use +# TTL in the same way as obstacle patches because they are temporary free-space +# observations. +float64 ttl_sec + +# 2D geometric observation patches. +rmf_prototype_msgs/Region[] regions diff --git a/rmf_layered_map_msgs/msg/MapRegionUpdate.msg b/rmf_layered_map_msgs/msg/MapRegionUpdate.msg index 68f983a..e067666 100644 --- a/rmf_layered_map_msgs/msg/MapRegionUpdate.msg +++ b/rmf_layered_map_msgs/msg/MapRegionUpdate.msg @@ -1,31 +1,15 @@ # MapRegionUpdate.msg -# The regions should be interpreted as temporary occupied space. -uint8 UPDATE_OBSTACLE=1 - -# The regions should be interpreted as temporary free space. -uint8 UPDATE_CLEAR=2 - -# The update should remove previous temporary observations from this source. -uint8 UPDATE_RESET=3 - -# Metadata describing where the observation came from. +# One observation snapshot from a source. MapObservationSource source -# UPDATE_OBSTACLE, UPDATE_CLEAR, or UPDATE_RESET. -uint8 update_type - -# OccupancyGrid-compatible value to write when this update is rasterized. -# -# Obstacle updates with values less than or equal to zero are treated as 100. -# Clear updates with negative values are treated as 0. -int8 occupancy_value - -# Time-to-live for this update. If this is zero or negative, source.default_ttl -# is used instead. If both are zero or negative, the map server's configured -# default TTL is used. -builtin_interfaces/Duration ttl +# Remove active observations from this source and map before applying patches. +# This is for source shutdowns, map transitions, or snapshots that replace the +# previous snapshot from the same source. It has no TTL because it is not an +# active map observation. +bool reset_source -# 2D geometric observation patches. Empty regions are valid only for -# UPDATE_RESET. -rmf_prototype_msgs/Region[] regions +# Patches from the same observation snapshot. The map server applies clear +# patches before obstacle patches so stale free-space evidence is cleared before +# occupied space is marked. +MapRegionPatch[] patches diff --git a/rmf_layered_map_msgs/package.xml b/rmf_layered_map_msgs/package.xml index 1b334e7..2d8c458 100644 --- a/rmf_layered_map_msgs/package.xml +++ b/rmf_layered_map_msgs/package.xml @@ -10,11 +10,13 @@ ament_cmake rosidl_default_generators - builtin_interfaces + geometry_msgs rmf_prototype_msgs + std_msgs - builtin_interfaces + geometry_msgs rmf_prototype_msgs + std_msgs rosidl_default_runtime ament_lint_common From a43dc3aecfb2cb92d5d0638e9d61e6e37c806913 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 9 Jul 2026 20:34:55 +0200 Subject: [PATCH 07/15] feat: apply layered map patches deterministically 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 --- map_server/rmf_layered_map_server/package.xml | 1 - map_server/rmf_layered_map_server/src/lib.rs | 258 +++++++++++------- .../demo_publisher.py | 21 +- .../test/test_layered_map_server.py | 25 +- 4 files changed, 192 insertions(+), 113 deletions(-) diff --git a/map_server/rmf_layered_map_server/package.xml b/map_server/rmf_layered_map_server/package.xml index ce55aec..0f54ca1 100644 --- a/map_server/rmf_layered_map_server/package.xml +++ b/map_server/rmf_layered_map_server/package.xml @@ -12,7 +12,6 @@ rclrs rmf_layered_map_msgs rmf_prototype_msgs - builtin_interfaces geometry_msgs nav_msgs std_msgs diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs index ebeae53..cb30037 100644 --- a/map_server/rmf_layered_map_server/src/lib.rs +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -14,12 +14,11 @@ use rclrs::{IntoPrimitiveOptions, Node, PrimitiveOptions}; use ros_env::{ - builtin_interfaces::msg::{Duration as RosDuration, Time as RosTime}, nav_msgs::msg::OccupancyGrid, - rmf_layered_map_msgs::msg::MapRegionUpdate, + rmf_layered_map_msgs::msg::{MapRegionPatch, MapRegionUpdate}, rmf_prototype_msgs::msg::Region, }; -use std::time::Duration; +use std::{collections::HashMap, time::Duration}; const NANOS_PER_SECOND: i128 = 1_000_000_000; const MAP_QOS_DEPTH: u32 = 10; @@ -40,10 +39,17 @@ struct DynamicObservation { expires_at_nsec: i128, } +#[derive(Clone, Debug, Hash, PartialEq, Eq)] +struct ObservationSourceKey { + source_id: String, + map_name: String, +} + #[derive(Clone, Debug)] pub struct LayeredMap { static_grid: Option, dynamic_observations: Vec, + latest_source_stamps: HashMap, default_ttl_nsec: i128, revision: u64, } @@ -53,6 +59,7 @@ impl LayeredMap { Self { static_grid: None, dynamic_observations: Vec::new(), + latest_source_stamps: HashMap::new(), default_ttl_nsec: duration_to_nsec(default_ttl), revision: 0, } @@ -73,62 +80,94 @@ impl LayeredMap { } pub fn ingest_region_update(&mut self, update: MapRegionUpdate, now_nsec: i128) -> bool { - if update.update_type == MapRegionUpdate::UPDATE_RESET { - let before = self.dynamic_observations.len(); - self.dynamic_observations.retain(|obs| { - obs.source_id != update.source.source_id || obs.map_name != update.source.map_name - }); - let changed = before != self.dynamic_observations.len(); - if changed { - self.revision = self.revision.wrapping_add(1); - } - return changed; - } - - let update_type = match update.update_type { - MapRegionUpdate::UPDATE_OBSTACLE => DynamicUpdateType::Obstacle, - MapRegionUpdate::UPDATE_CLEAR => DynamicUpdateType::Clear, - _ => return false, + let reset_source = update.reset_source; + let key = ObservationSourceKey { + source_id: update.source.source_id.clone(), + map_name: update.source.map_name.clone(), + }; + let stamp_nsec = if update.source.header.stamp.sec == 0 + && update.source.header.stamp.nanosec == 0 + { + now_nsec + } else { + i128::from(update.source.header.stamp.sec) * NANOS_PER_SECOND + + i128::from(update.source.header.stamp.nanosec) }; - if update.regions.is_empty() { + if self + .latest_source_stamps + .get(&key) + .is_some_and(|latest_stamp| stamp_nsec < *latest_stamp) + { return false; } - let ttl_nsec = first_positive_duration_nsec(&update.ttl) - .or_else(|| first_positive_duration_nsec(&update.source.default_ttl)) - .unwrap_or(self.default_ttl_nsec); - if ttl_nsec <= 0 { - return false; + let mut changed = false; + + if reset_source { + let before = self.dynamic_observations.len(); + self.dynamic_observations + .retain(|obs| obs.source_id != key.source_id || obs.map_name != key.map_name); + changed |= before != self.dynamic_observations.len(); } - let stamp_nsec = if is_zero_time(&update.source.stamp) { - now_nsec - } else { - time_to_nsec(&update.source.stamp) - }; - let expires_at_nsec = stamp_nsec + ttl_nsec; - if expires_at_nsec <= now_nsec { - return false; + let default_ttl_sec = update.source.default_ttl_sec; + let mut patches = update.patches; + patches.sort_by_key(|patch| match patch.update_type { + MapRegionPatch::UPDATE_CLEAR => 0, + MapRegionPatch::UPDATE_OBSTACLE => 1, + _ => 2, + }); + + for patch in patches { + let update_type = match patch.update_type { + MapRegionPatch::UPDATE_OBSTACLE => DynamicUpdateType::Obstacle, + MapRegionPatch::UPDATE_CLEAR => DynamicUpdateType::Clear, + _ => continue, + }; + + if patch.regions.is_empty() { + continue; + } + + let ttl_nsec = positive_seconds_to_nsec(patch.ttl_sec) + .or_else(|| positive_seconds_to_nsec(default_ttl_sec)) + .unwrap_or(self.default_ttl_nsec); + if ttl_nsec <= 0 { + continue; + } + + let expires_at_nsec = stamp_nsec + ttl_nsec; + if expires_at_nsec <= now_nsec { + continue; + } + + let occupancy_value = match update_type { + DynamicUpdateType::Obstacle if patch.occupancy_value <= 0 => 100, + DynamicUpdateType::Obstacle => patch.occupancy_value.clamp(1, 100), + DynamicUpdateType::Clear if patch.occupancy_value < 0 => 0, + DynamicUpdateType::Clear => patch.occupancy_value.clamp(0, 100), + }; + + self.dynamic_observations.push(DynamicObservation { + source_id: key.source_id.clone(), + map_name: key.map_name.clone(), + update_type, + occupancy_value, + regions: patch.regions, + expires_at_nsec, + }); + changed = true; } - let occupancy_value = match update_type { - DynamicUpdateType::Obstacle if update.occupancy_value <= 0 => 100, - DynamicUpdateType::Obstacle => update.occupancy_value.clamp(1, 100), - DynamicUpdateType::Clear if update.occupancy_value < 0 => 0, - DynamicUpdateType::Clear => update.occupancy_value.clamp(0, 100), - }; + if changed || reset_source { + self.latest_source_stamps.insert(key, stamp_nsec); + } - self.dynamic_observations.push(DynamicObservation { - source_id: update.source.source_id, - map_name: update.source.map_name, - update_type, - occupancy_value, - regions: update.regions, - expires_at_nsec, - }); - self.revision = self.revision.wrapping_add(1); - true + if changed { + self.revision = self.revision.wrapping_add(1); + } + changed } pub fn prune_expired(&mut self, now_nsec: i128) -> bool { @@ -541,36 +580,27 @@ fn point_in_polygon(x: f64, y: f64, polygon: &[(f64, f64)]) -> bool { inside } -fn time_to_nsec(time: &RosTime) -> i128 { - i128::from(time.sec) * NANOS_PER_SECOND + i128::from(time.nanosec) -} - -fn duration_msg_to_nsec(duration: &RosDuration) -> i128 { - i128::from(duration.sec) * NANOS_PER_SECOND + i128::from(duration.nanosec) -} - fn duration_to_nsec(duration: Duration) -> i128 { i128::from(duration.as_secs()) * NANOS_PER_SECOND + i128::from(duration.subsec_nanos()) } -fn first_positive_duration_nsec(duration: &RosDuration) -> Option { - let nsec = duration_msg_to_nsec(duration); - (nsec > 0).then_some(nsec) -} +fn positive_seconds_to_nsec(seconds: f64) -> Option { + if !seconds.is_finite() || seconds <= 0.0 { + return None; + } -fn is_zero_time(time: &RosTime) -> bool { - time.sec == 0 && time.nanosec == 0 + Some((seconds * NANOS_PER_SECOND as f64).round() as i128) } #[cfg(test)] mod tests { use super::*; use ros_env::{ - builtin_interfaces::msg::Duration as RosDuration, geometry_msgs::msg::{Point, Pose, Quaternion}, nav_msgs::msg::MapMetaData, - rmf_layered_map_msgs::msg::{MapObservationSource, MapRegionUpdate}, + rmf_layered_map_msgs::msg::{MapObservationSource, MapRegionPatch, MapRegionUpdate}, rmf_prototype_msgs::msg::Region, + std_msgs::msg::Header, }; fn static_grid(width: u32, height: u32, value: i8) -> OccupancyGrid { @@ -603,14 +633,14 @@ mod tests { fn source_with_id(source_id: &str) -> MapObservationSource { MapObservationSource { + header: Header { + frame_id: "map".to_string(), + ..Default::default() + }, source_id: source_id.to_string(), robot_name: "robot_1".to_string(), map_name: "test_map".to_string(), - frame_id: "map".to_string(), - default_ttl: RosDuration { - sec: 10, - nanosec: 0, - }, + default_ttl_sec: 10.0, ..Default::default() } } @@ -636,11 +666,18 @@ mod tests { } } + fn patch(update_type: u8, regions: Vec) -> MapRegionPatch { + MapRegionPatch { + update_type, + regions, + ..Default::default() + } + } + fn update(update_type: u8, regions: Vec) -> MapRegionUpdate { MapRegionUpdate { source: source(), - update_type, - regions, + patches: vec![patch(update_type, regions)], ..Default::default() } } @@ -648,8 +685,7 @@ mod tests { fn update_from(source_id: &str, update_type: u8, regions: Vec) -> MapRegionUpdate { MapRegionUpdate { source: source_with_id(source_id), - update_type, - regions, + patches: vec![patch(update_type, regions)], ..Default::default() } } @@ -661,7 +697,7 @@ mod tests { assert!(map.ingest_region_update( update( - MapRegionUpdate::UPDATE_OBSTACLE, + MapRegionPatch::UPDATE_OBSTACLE, vec![rectangle(2.0, 1.0, 3.0, 3.0)] ), 1_000, @@ -684,7 +720,7 @@ mod tests { map.set_static_map(static_map); assert!(map.ingest_region_update( - update(MapRegionUpdate::UPDATE_OBSTACLE, vec![point(-0.25, 0.25)]), + update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(-0.25, 0.25)]), 0, )); @@ -700,7 +736,7 @@ mod tests { assert!(map.ingest_region_update( update( - MapRegionUpdate::UPDATE_OBSTACLE, + MapRegionPatch::UPDATE_OBSTACLE, vec![polygon(vec![1.0, 1.0, 3.0, 1.0, 3.0, 3.0, 1.0, 3.0])] ), 0, @@ -722,7 +758,7 @@ mod tests { assert!(map.ingest_region_update( update( - MapRegionUpdate::UPDATE_OBSTACLE, + MapRegionPatch::UPDATE_OBSTACLE, vec![rectangle(10.0, 10.0, 11.0, 11.0)] ), 0, @@ -739,7 +775,7 @@ mod tests { assert!(map.ingest_region_update( update( - MapRegionUpdate::UPDATE_OBSTACLE, + MapRegionPatch::UPDATE_OBSTACLE, vec![Region { hint: Region::HINT_POLYGON, points: vec![1.0, 1.0, 2.0], @@ -759,7 +795,7 @@ mod tests { assert!(map.ingest_region_update( update( - MapRegionUpdate::UPDATE_OBSTACLE, + MapRegionPatch::UPDATE_OBSTACLE, vec![rectangle(1.0, 1.0, 2.0, 2.0)] ), 0, @@ -779,17 +815,20 @@ mod tests { map.set_static_map(static_grid(5, 5, -1)); assert!(map.ingest_region_update( - update( - MapRegionUpdate::UPDATE_CLEAR, - vec![rectangle(1.0, 1.0, 4.0, 4.0)] - ), - 0, - )); - assert!(map.ingest_region_update( - update( - MapRegionUpdate::UPDATE_OBSTACLE, - vec![rectangle(2.0, 2.0, 3.0, 3.0)] - ), + MapRegionUpdate { + source: source(), + patches: vec![ + patch( + MapRegionPatch::UPDATE_CLEAR, + vec![rectangle(1.0, 1.0, 4.0, 4.0)] + ), + patch( + MapRegionPatch::UPDATE_OBSTACLE, + vec![rectangle(2.0, 2.0, 3.0, 3.0)] + ), + ], + ..Default::default() + }, 0, )); @@ -799,6 +838,33 @@ mod tests { assert_eq!(composed.data[0], -1); } + #[test] + fn newer_obstacles_survive_older_clear_updates_received_late() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(5, 5, -1)); + + let mut obstacle = update( + MapRegionPatch::UPDATE_OBSTACLE, + vec![rectangle(2.0, 2.0, 3.0, 3.0)], + ); + obstacle.source.header.stamp.sec = 2; + + let mut clear = update( + MapRegionPatch::UPDATE_CLEAR, + vec![rectangle(1.0, 1.0, 4.0, 4.0)], + ); + clear.reset_source = true; + clear.source.header.stamp.sec = 1; + + assert!(map.ingest_region_update(obstacle, 3 * NANOS_PER_SECOND)); + assert!(!map.ingest_region_update(clear, 3 * NANOS_PER_SECOND)); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 5 + 1], -1); + assert_eq!(composed.data[2 * 5 + 2], 100); + assert_eq!(composed.data[0], -1); + } + #[test] fn reset_removes_observations_from_same_source_and_map() { let mut map = LayeredMap::default(); @@ -806,7 +872,7 @@ mod tests { assert!(map.ingest_region_update( update( - MapRegionUpdate::UPDATE_OBSTACLE, + MapRegionPatch::UPDATE_OBSTACLE, vec![rectangle(1.0, 1.0, 2.0, 2.0)] ), 0, @@ -815,7 +881,7 @@ mod tests { assert!(map.ingest_region_update( MapRegionUpdate { source: source(), - update_type: MapRegionUpdate::UPDATE_RESET, + reset_source: true, ..Default::default() }, 0, @@ -834,7 +900,7 @@ mod tests { assert!(map.ingest_region_update( update_from( "robot_1/local_costmap", - MapRegionUpdate::UPDATE_OBSTACLE, + MapRegionPatch::UPDATE_OBSTACLE, vec![rectangle(1.0, 1.0, 2.0, 2.0)] ), 0, @@ -842,7 +908,7 @@ mod tests { assert!(map.ingest_region_update( update_from( "robot_2/local_costmap", - MapRegionUpdate::UPDATE_OBSTACLE, + MapRegionPatch::UPDATE_OBSTACLE, vec![rectangle(3.0, 3.0, 4.0, 4.0)] ), 0, @@ -855,7 +921,7 @@ mod tests { assert!(map.ingest_region_update( MapRegionUpdate { source: source_with_id("robot_1/local_costmap"), - update_type: MapRegionUpdate::UPDATE_RESET, + reset_source: true, ..Default::default() }, 0, diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py index 675eac0..ec1bb1e 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py @@ -16,7 +16,11 @@ import rclpy from rclpy.node import Node from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy -from rmf_layered_map_msgs.msg import MapObservationSource, MapRegionUpdate +from rmf_layered_map_msgs.msg import ( + MapObservationSource, + MapRegionPatch, + MapRegionUpdate, +) from rmf_prototype_msgs.msg import Region @@ -81,19 +85,22 @@ def static_map(): def obstacle_update(): msg = MapRegionUpdate() msg.source = MapObservationSource() + msg.source.header.frame_id = 'map' msg.source.source_id = 'demo/temporary_obstacle' msg.source.robot_name = 'demo_robot' msg.source.map_name = 'demo_map' - msg.source.frame_id = 'map' - msg.source.default_ttl.sec = 5 - msg.update_type = MapRegionUpdate.UPDATE_OBSTACLE - msg.occupancy_value = 100 - msg.ttl.sec = 5 + msg.source.default_ttl_sec = 5.0 + + patch = MapRegionPatch() + patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + patch.occupancy_value = 100 + patch.ttl_sec = 5.0 region = Region() region.hint = Region.HINT_AXIS_ALIGNED_RECTANGLE region.points = [4.0, 4.0, 6.0, 6.0] - msg.regions.append(region) + patch.regions.append(region) + msg.patches.append(patch) return msg diff --git a/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py b/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py index 718807d..29130ac 100644 --- a/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py +++ b/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py @@ -22,7 +22,11 @@ import pytest import rclpy from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy -from rmf_layered_map_msgs.msg import MapObservationSource, MapRegionUpdate +from rmf_layered_map_msgs.msg import ( + MapObservationSource, + MapRegionPatch, + MapRegionUpdate, +) from rmf_prototype_msgs.msg import Region @@ -174,31 +178,34 @@ def static_map(): def map_source(): msg = MapObservationSource() + msg.header.frame_id = 'map' msg.source_id = 'test/obstacle' msg.robot_name = 'test_robot' msg.map_name = 'test_map' - msg.frame_id = 'map' - msg.default_ttl.sec = 1 + msg.default_ttl_sec = 1.0 return msg def obstacle_update(ttl_sec=1): msg = MapRegionUpdate() msg.source = map_source() - msg.source.default_ttl.sec = ttl_sec - msg.update_type = MapRegionUpdate.UPDATE_OBSTACLE - msg.occupancy_value = 100 - msg.ttl.sec = ttl_sec + msg.source.default_ttl_sec = float(ttl_sec) + + patch = MapRegionPatch() + patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + patch.occupancy_value = 100 + patch.ttl_sec = float(ttl_sec) region = Region() region.hint = Region.HINT_AXIS_ALIGNED_RECTANGLE region.points = [2.0, 2.0, 3.0, 3.0] - msg.regions.append(region) + patch.regions.append(region) + msg.patches.append(patch) return msg def reset_update(): msg = MapRegionUpdate() msg.source = map_source() - msg.update_type = MapRegionUpdate.UPDATE_RESET + msg.reset_source = True return msg From a05e0d02780d06d5b984231c6bd6d4ab9fb91186 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 9 Jul 2026 20:35:00 +0200 Subject: [PATCH 08/15] fix: log invalid layered map regions Signed-off-by: SamuelFoo --- map_server/rmf_layered_map_server/src/lib.rs | 55 ++++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs index cb30037..db6eb35 100644 --- a/map_server/rmf_layered_map_server/src/lib.rs +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -266,6 +266,7 @@ impl LayeredMapServer { } fn handle_region_update(&mut self, msg: MapRegionUpdate) { + log_region_update_errors(&self.node, &msg); let now_nsec = self.now_nsec(); if self.map.ingest_region_update(msg, now_nsec) { rclrs::log!( @@ -382,6 +383,56 @@ fn region_update_qos(topic: &str) -> PrimitiveOptions<'_> { topic.keep_last(MAP_QOS_DEPTH).reliable() } +fn log_region_update_errors(node: &Node, update: &MapRegionUpdate) { + for patch in &update.patches { + if !matches!( + patch.update_type, + MapRegionPatch::UPDATE_OBSTACLE | MapRegionPatch::UPDATE_CLEAR + ) { + rclrs::log_error!( + node.logger(), + "Ignoring map region patch with unsupported update_type {}", + patch.update_type + ); + continue; + } + + for region in &patch.regions { + if let Some(error) = region_validation_error(region) { + rclrs::log_error!(node.logger(), "Ignoring map region: {}", error); + } + } + } +} + +fn region_validation_error(region: &Region) -> Option<&'static str> { + if region.points.len() < 2 { + return Some("region must contain at least one x/y point pair"); + } + + if region.points.len() % 2 != 0 { + return Some("region points must contain complete x/y pairs"); + } + + if !is_supported_region_hint(region.hint) { + return Some("unsupported region hint"); + } + + None +} + +fn is_supported_region_hint(hint: u8) -> bool { + matches!( + hint, + Region::HINT_UNSPECIFIED + | Region::HINT_POINT + | Region::HINT_AXIS_ALIGNED_RECTANGLE + | Region::HINT_RECTANGLE + | Region::HINT_CONVEX_POLYGON + | Region::HINT_POLYGON + ) +} + fn normalize_grid_data(grid: &mut OccupancyGrid) { let Some(expected_len) = grid_len(grid) else { grid.data.clear(); @@ -420,6 +471,10 @@ fn rasterized_indices(grid: &OccupancyGrid, region: &Region) -> Vec { return Vec::new(); } + if !is_supported_region_hint(region.hint) { + return Vec::new(); + } + if region.hint == Region::HINT_POINT || region.points.len() == 2 { return point_index( region.points[0], From 947c6f838cdc806f715c81b9d6df1a29bd2ddb91 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 9 Jul 2026 20:35:08 +0200 Subject: [PATCH 09/15] docs: explain batched map patch updates Signed-off-by: SamuelFoo --- map_server/rmf_layered_map_server/README.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/map_server/rmf_layered_map_server/README.md b/map_server/rmf_layered_map_server/README.md index 0e52f20..de12819 100644 --- a/map_server/rmf_layered_map_server/README.md +++ b/map_server/rmf_layered_map_server/README.md @@ -12,6 +12,14 @@ Default topics: Dynamic observations expire according to their TTL, so transient local obstacles do not remain in the global planning map forever. A LiDAR or local costmap observer can keep refreshing an obstacle while it is still seen, then let the server decay it after the TTL. +`MapRegionUpdate` messages contain an ordered list of `MapRegionPatch` entries. +A sensor snapshot that clears freespace and marks obstacles should publish clear +patches before obstacle patches in the same message. If the snapshot replaces +the source's previous observation state, set `reset_source` so the server removes +old observations from that `source_id` and `map_name` before adding the new +patches. Resetting has no TTL; clear and obstacle patches both use TTLs in +seconds. + ## Visualization Smoke Test Build and source the workspace: From e329aa305035bca8079cc68c5aa401a52346ae27 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 9 Jul 2026 20:35:16 +0200 Subject: [PATCH 10/15] docs: refocus layered map observation spec Signed-off-by: SamuelFoo --- discourse/3-layered-global-occupancy-map.md | 171 ++++++++++---------- 1 file changed, 84 insertions(+), 87 deletions(-) diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md index 783c5d7..cc87898 100644 --- a/discourse/3-layered-global-occupancy-map.md +++ b/discourse/3-layered-global-occupancy-map.md @@ -1,133 +1,130 @@ [Draft for Discourse] -# Layered Global Occupancy Map +# Layered Global Map Observations -This post describes the first implemented slice of a layered global occupancy map for the next generation Open-RMF prototype. +This post describes the observation interface for contributing temporary map +information to the next generation Open-RMF prototype. -The goal of this slice is to let temporary local observations affect the global planning map without changing the path server interface. The path server can continue to consume a normal `nav_msgs/OccupancyGrid` from `/map`, while a new map server composes that grid from a static map and active observation layers. +The goal is to let robots and perception systems publish what they currently +observe without tying them to a specific central map implementation. The first +prototype consumes sparse 2D occupancy regions because that is enough for the +current global planning grid, but the interface is intentionally isolated in +`rmf_layered_map_msgs` so richer representations, such as height-aware or voxel +observations, can be added later. # Quick Summary -* 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` -* The observation messages live in `rmf_layered_map_msgs`, leaving `rmf_prototype_msgs` unchanged -* The first implementation is a Rust package named `rmf_layered_map_server` under `map_server/` +* Observation sources publish sparse temporary map patches +* One message can contain both clear-space and occupied-space patches from the + same sensor snapshot +* Clear-space patches have TTLs, just like obstacle patches +* A source can reset its previous observations without using a TTL +* Robot-mounted sources can include the robot pose at observation time +* The observation messages live in `rmf_layered_map_msgs`, leaving + `rmf_prototype_msgs` unchanged -# Background +# Observation Topic -The static map support in the prototype gives the path server a useful baseline: it already consumes a `nav_msgs/OccupancyGrid` from `/map` and can plan around occupied cells. This implementation preserves that contract. Instead of teaching the planner about every possible observation source, the layered map server publishes one composed `/map`. +Observation sources publish dynamic map observations on: -Local observations from robots are different from static map data. They may describe temporary objects such as pedestrians, carts, doors held open, or equipment left in the path. For that reason the central map treats robot observations as dynamic layers that expire unless they are refreshed. +* `/map/region_updates` - [`MapRegionUpdate.msg`](../rmf_layered_map_msgs/msg/MapRegionUpdate.msg) -This implementation builds on the existing static map behavior. +The topic is an event stream. Updates are not latched because expired +observations should not be replayed to a restarted map service as if they were +current. -# 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 path server continues to subscribe to `/map`. It does not subscribe directly to local robot observations. This keeps map composition independent from multi-agent planning and allows alternative map server implementations to be swapped in later. - -Each observation stream publishes updates with a unique `source_id`, such as `robot_1/local_costmap` or `robot_2/lidar_obstacles`. The layered map server tracks these sources independently but composes their active observations into a single `OccupancyGrid`. - -# Observation Topics - -* `/map/region_updates` - [`MapRegionUpdate.msg`](../rmf_layered_map_msgs/msg/MapRegionUpdate.msg): Dynamic local observation updates - -The topic is treated as an event stream. Updates are not latched because expired observations should not be replayed to a restarted map server as if they were current. +Each observation stream should use a stable `source_id`, such as +`robot_1/local_costmap`, `robot_1/front_lidar`, or `door_sensor/west_lobby`. +The map service tracks each source independently so one robot can update or +reset its own temporary observations without disturbing observations from other +sources. # Messages ## `MapObservationSource` -[`MapObservationSource.msg`](../rmf_layered_map_msgs/msg/MapObservationSource.msg) describes where an observation came from. +[`MapObservationSource.msg`](../rmf_layered_map_msgs/msg/MapObservationSource.msg) +describes where an observation came from. Important fields: -* `source_id`: stable source identifier, usually the robot namespace plus the local source name -* `robot_name`: robot that produced the observation, if the source is mounted on a robot. This can be empty for fixed sensors or synthetic layers -* `map_name`: map or level that the observation belongs to -* `frame_id`: coordinate frame for the regions -* `stamp`: time when the observation snapshot was produced -* `default_ttl`: fallback TTL for updates from this source +* `header`: frame and timestamp for the observed regions +* `source_id`: stable source identifier, usually the robot namespace plus the + local source name +* `robot_name`: robot that produced this observation, if the source is mounted + on a robot. This can be empty for fixed sensors or synthetic map layers +* `map_name`: map or level that the observations belong to +* `robot_pose`: robot pose when the observation was produced, so consumers do + not need to reconstruct this from a separate TF-like lookup +* `default_ttl_sec`: fallback TTL in seconds for patches from this source ## `MapRegionUpdate` -[`MapRegionUpdate.msg`](../rmf_layered_map_msgs/msg/MapRegionUpdate.msg) describes one sparse update from a local observation source. +[`MapRegionUpdate.msg`](../rmf_layered_map_msgs/msg/MapRegionUpdate.msg) +describes one observation snapshot from a source. -Update types: - -* `UPDATE_OBSTACLE`: regions are temporary occupied space -* `UPDATE_CLEAR`: regions are temporary free space -* `UPDATE_RESET`: previous observations from the same source should be removed - -The first implementation uses `rmf_prototype_msgs/Region` for 2D geometry. This keeps the message sparse and lets the central map server rasterize regions into its composed `OccupancyGrid`. - -The map observation messages live in a dedicated `rmf_layered_map_msgs` package. This keeps `rmf_prototype_msgs` as a stable public API surface while allowing the layered-map interface to evolve in its own package. +Important fields: -# Layered Map Server +* `source`: metadata for the observation source +* `reset_source`: remove active observations from the same `source_id` and + `map_name` before applying the new patches +* `patches`: ordered clear-space and occupied-space patches from this snapshot -The first server implementation is a Rust package named `rmf_layered_map_server` under the `map_server/` directory. +`reset_source` is useful when a source publishes replacement snapshots, changes +maps, shuts down, or knows its previous observation state is no longer valid. It +does not have a TTL because it is a bookkeeping operation, not an active map +observation. -Inputs: +## `MapRegionPatch` -* `/map/static`: `nav_msgs/OccupancyGrid`, transient local, reliable -* `/map/region_updates`: `rmf_layered_map_msgs/MapRegionUpdate`, reliable +[`MapRegionPatch.msg`](../rmf_layered_map_msgs/msg/MapRegionPatch.msg) +describes one group of regions with the same action and TTL. -Output: +Patch types: -* `/map`: composed `nav_msgs/OccupancyGrid`, transient local, reliable +* `UPDATE_CLEAR`: regions are temporary free space +* `UPDATE_OBSTACLE`: regions are temporary occupied space -The server keeps the static grid separate from active dynamic observations: +Clear patches and obstacle patches both require a TTL because both describe +temporary observations. If a patch TTL is zero or negative, the source +`default_ttl_sec` is used. If that is also zero or negative, the map service's +configured default TTL is used. -```text -LayeredMap - static_grid: OccupancyGrid - dynamic_observations: Vec - revision: u64 +When a sensor snapshot includes both clear and occupied regions, publish them in +one `MapRegionUpdate` message. The map service applies clear patches before +obstacle patches, which gives deterministic "clear then mark" behavior without +relying on the arrival order of separate ROS messages. -DynamicObservation - source_id: String - map_name: String - update_type: obstacle | clear - occupancy_value: i8 - regions: Vec - expires_at: Time -``` +The first prototype uses `rmf_prototype_msgs/Region` for sparse 2D geometry so +robots can publish compact patches. The central map service is responsible for +rasterizing those regions into its internal representation. -Composition rules: +# Example Flow -1. Start from the latest static grid. -2. Drop expired dynamic observations before composing. -3. Rasterize active clear regions to free space. -4. Rasterize active obstacle regions to occupied space. -5. Active obstacle observations win over active clear observations for the same cell. -6. Unknown static cells stay unknown unless an active clear or obstacle observation covers them. +A local costmap or LiDAR observation node can publish a replacement snapshot by: -`UPDATE_RESET` removes observations from the same source and map without disturbing observations from other robots. For example, if `robot_1/local_costmap` resets its obstacle layer, active observations from `robot_2/local_costmap` remain in the composed grid. +1. Stamping the observation source with the sensor frame and observation time. +2. Setting `reset_source` if the new snapshot replaces the source's previous + temporary observations. +3. Adding one or more clear patches for free space seen by the sensor. +4. Adding one or more obstacle patches for occupied space seen by the sensor. +5. Giving each patch a TTL long enough to survive normal publication jitter but + short enough to decay when the observation is no longer refreshed. -The server does not use the full `-/errors` component pattern from the broader topic taxonomy. For transient observations, TTL expiration is the primary recovery mechanism. +The map service should ignore snapshots from a source if their timestamp is +older than a newer snapshot that has already been accepted. This prevents a late +clear/reset message from removing obstacle information that came from a newer +observation. # Implemented Test Coverage -The Rust tests cover: +The first server tests cover: -* composing obstacle regions over a static map +* composing obstacle regions over a static planning grid * point regions with non-zero map origins and non-1.0 resolutions * polygon regions, out-of-bounds regions, and malformed region point arrays * pruning expired observations by TTL -* active obstacle observations winning over active clear observations +* clear and obstacle patches in the same update +* late older snapshots being ignored * reset updates removing observations from the same source and map * multiple robot sources being stitched into one composed grid - -The `rmf_layered_map_server_demo` package includes a small publisher that sends a static map and a temporary obstacle update. Its launch file starts the layered map server and RViz on `/map`, so developers can see the dynamic layer appear and decay without needing the full Nav2 or replanning loop. - -The `rmf_layered_map_server_test` package adds a launch test for the ROS topic contract: `/map/static` and `/map/region_updates` are consumed, `/map` is published, reset updates remove active observations, and obstacles are removed after TTL expiry. From 89eab89357d2c6ff6dea4e0c2a5e37019cfcb2f0 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 9 Jul 2026 21:19:41 +0200 Subject: [PATCH 11/15] docs: clarify map patch ordering Signed-off-by: SamuelFoo --- discourse/3-layered-global-occupancy-map.md | 2 +- map_server/rmf_layered_map_server/README.md | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md index cc87898..42656f3 100644 --- a/discourse/3-layered-global-occupancy-map.md +++ b/discourse/3-layered-global-occupancy-map.md @@ -68,7 +68,7 @@ Important fields: * `source`: metadata for the observation source * `reset_source`: remove active observations from the same `source_id` and `map_name` before applying the new patches -* `patches`: ordered clear-space and occupied-space patches from this snapshot +* `patches`: clear-space and occupied-space patches from this snapshot `reset_source` is useful when a source publishes replacement snapshots, changes maps, shuts down, or knows its previous observation state is no longer valid. It diff --git a/map_server/rmf_layered_map_server/README.md b/map_server/rmf_layered_map_server/README.md index de12819..c52d076 100644 --- a/map_server/rmf_layered_map_server/README.md +++ b/map_server/rmf_layered_map_server/README.md @@ -12,13 +12,13 @@ Default topics: Dynamic observations expire according to their TTL, so transient local obstacles do not remain in the global planning map forever. A LiDAR or local costmap observer can keep refreshing an obstacle while it is still seen, then let the server decay it after the TTL. -`MapRegionUpdate` messages contain an ordered list of `MapRegionPatch` entries. -A sensor snapshot that clears freespace and marks obstacles should publish clear -patches before obstacle patches in the same message. If the snapshot replaces -the source's previous observation state, set `reset_source` so the server removes -old observations from that `source_id` and `map_name` before adding the new -patches. Resetting has no TTL; clear and obstacle patches both use TTLs in -seconds. +`MapRegionUpdate` messages contain a list of `MapRegionPatch` entries. A sensor +snapshot that clears freespace and marks obstacles can publish clear and obstacle +patches in the same message. The server applies clear patches before obstacle +patches. If the snapshot replaces the source's previous observation state, set +`reset_source` so the server removes old observations from that `source_id` and +`map_name` before adding the new patches. Resetting has no TTL. Clear and +obstacle patches both use TTLs in seconds. ## Visualization Smoke Test From 2aeed1e4ef9855df67ca864b1d1f538d005c7058 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Fri, 10 Jul 2026 18:14:52 +0200 Subject: [PATCH 12/15] refactor: use the concrete map server timer type Signed-off-by: SamuelFoo --- map_server/rmf_layered_map_server/src/lib.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs index db6eb35..3b38c50 100644 --- a/map_server/rmf_layered_map_server/src/lib.rs +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -332,7 +332,7 @@ pub struct LayeredMapServerRunning { // Keep the ROS handles alive for as long as the server is running. pub static_map_subscription: rclrs::WorkerSubscription, pub region_update_subscription: rclrs::WorkerSubscription, - pub prune_timer: Box, + pub prune_timer: rclrs::WorkerTimer, } pub fn start_layered_map_server( @@ -367,7 +367,7 @@ pub fn start_layered_map_server( worker, static_map_subscription, region_update_subscription, - prune_timer: Box::new(prune_timer), + prune_timer, }) } From 0f1bf4149c9a0eb5635ccb692efc178166714ef4 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Fri, 10 Jul 2026 18:17:23 +0200 Subject: [PATCH 13/15] fix: reject unstamped layered map updates Signed-off-by: SamuelFoo --- discourse/3-layered-global-occupancy-map.md | 3 +- map_server/rmf_layered_map_server/README.md | 2 ++ map_server/rmf_layered_map_server/src/lib.rs | 32 +++++++++++++------ .../demo_publisher.py | 4 ++- .../test/test_layered_map_server.py | 3 ++ .../msg/MapObservationSource.msg | 3 +- 6 files changed, 34 insertions(+), 13 deletions(-) diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md index 42656f3..8d8548a 100644 --- a/discourse/3-layered-global-occupancy-map.md +++ b/discourse/3-layered-global-occupancy-map.md @@ -48,7 +48,7 @@ describes where an observation came from. Important fields: -* `header`: frame and timestamp for the observed regions +* `header`: frame for the observation and its required, non-zero timestamp * `source_id`: stable source identifier, usually the robot namespace plus the local source name * `robot_name`: robot that produced this observation, if the source is mounted @@ -122,6 +122,7 @@ The first server tests cover: * composing obstacle regions over a static planning grid * point regions with non-zero map origins and non-1.0 resolutions +* rejecting updates without a timestamp * polygon regions, out-of-bounds regions, and malformed region point arrays * pruning expired observations by TTL * clear and obstacle patches in the same update diff --git a/map_server/rmf_layered_map_server/README.md b/map_server/rmf_layered_map_server/README.md index c52d076..abd008d 100644 --- a/map_server/rmf_layered_map_server/README.md +++ b/map_server/rmf_layered_map_server/README.md @@ -20,6 +20,8 @@ patches. If the snapshot replaces the source's previous observation state, set `map_name` before adding the new patches. Resetting has no TTL. Clear and obstacle patches both use TTLs in seconds. +Updates with a zero source timestamp are rejected. + ## Visualization Smoke Test Build and source the workspace: diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs index 3b38c50..d6e0fd1 100644 --- a/map_server/rmf_layered_map_server/src/lib.rs +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -80,19 +80,17 @@ impl LayeredMap { } pub fn ingest_region_update(&mut self, update: MapRegionUpdate, now_nsec: i128) -> bool { + if update.source.header.stamp.sec == 0 && update.source.header.stamp.nanosec == 0 { + return false; + } + let reset_source = update.reset_source; let key = ObservationSourceKey { source_id: update.source.source_id.clone(), map_name: update.source.map_name.clone(), }; - let stamp_nsec = if update.source.header.stamp.sec == 0 - && update.source.header.stamp.nanosec == 0 - { - now_nsec - } else { - i128::from(update.source.header.stamp.sec) * NANOS_PER_SECOND - + i128::from(update.source.header.stamp.nanosec) - }; + let stamp_nsec = i128::from(update.source.header.stamp.sec) * NANOS_PER_SECOND + + i128::from(update.source.header.stamp.nanosec); if self .latest_source_stamps @@ -687,7 +685,7 @@ mod tests { } fn source_with_id(source_id: &str) -> MapObservationSource { - MapObservationSource { + let mut source = MapObservationSource { header: Header { frame_id: "map".to_string(), ..Default::default() @@ -697,7 +695,9 @@ mod tests { map_name: "test_map".to_string(), default_ttl_sec: 10.0, ..Default::default() - } + }; + source.header.stamp.sec = 1; + source } fn rectangle(min_x: f32, min_y: f32, max_x: f32, max_y: f32) -> Region { @@ -843,6 +843,18 @@ mod tests { assert!(composed.data.iter().all(|cell| *cell == 0)); } + #[test] + fn updates_without_a_timestamp_are_rejected() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + + let mut unstamped = update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]); + unstamped.source.header.stamp = Default::default(); + + assert!(!map.ingest_region_update(unstamped, NANOS_PER_SECOND)); + assert_eq!(map.dynamic_observation_count(), 0); + } + #[test] fn expired_observations_are_removed() { let mut map = LayeredMap::default(); diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py index ec1bb1e..988b54d 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py @@ -55,7 +55,9 @@ def __init__(self): def publish_demo(self): self.static_map_publisher.publish(static_map()) - self.region_update_publisher.publish(obstacle_update()) + update = obstacle_update() + update.source.header.stamp = self.get_clock().now().to_msg() + self.region_update_publisher.publish(update) self.get_logger().info('Published demo obstacle with a 5 second TTL') diff --git a/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py b/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py index 29130ac..b4fac91 100644 --- a/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py +++ b/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py @@ -131,6 +131,7 @@ def test_region_update_is_composed_reset_and_expires(self): self.assertEqual(self.last_cell(2, 2), 0) update = obstacle_update(ttl_sec=30) + update.source.header.stamp = self.node.get_clock().now().to_msg() self.assertTrue( self.publish_until( self.region_update_publisher, @@ -141,6 +142,7 @@ def test_region_update_is_composed_reset_and_expires(self): ) reset = reset_update() + reset.source.header.stamp = self.node.get_clock().now().to_msg() self.assertTrue( self.publish_until( self.region_update_publisher, @@ -151,6 +153,7 @@ def test_region_update_is_composed_reset_and_expires(self): ) update = obstacle_update() + update.source.header.stamp = self.node.get_clock().now().to_msg() self.assertTrue( self.publish_until( self.region_update_publisher, diff --git a/rmf_layered_map_msgs/msg/MapObservationSource.msg b/rmf_layered_map_msgs/msg/MapObservationSource.msg index 10c69db..18f5d1c 100644 --- a/rmf_layered_map_msgs/msg/MapObservationSource.msg +++ b/rmf_layered_map_msgs/msg/MapObservationSource.msg @@ -1,6 +1,7 @@ # MapObservationSource.msg -# Frame and time for this observation snapshot. +# Frame and time for this observation snapshot. The timestamp is required. +# Consumers should reject updates with a zero timestamp. std_msgs/Header header # Unique source for this stream of observations. For a robot-local node this can From 4e20799a73eca745f39ce532b1018531d8c8f850 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Fri, 10 Jul 2026 18:18:58 +0200 Subject: [PATCH 14/15] fix: reject unsupported layered map region types Signed-off-by: SamuelFoo --- discourse/3-layered-global-occupancy-map.md | 5 +- map_server/rmf_layered_map_server/README.md | 3 + map_server/rmf_layered_map_server/src/lib.rs | 77 ++++++++++---------- rmf_layered_map_msgs/msg/MapRegionPatch.msg | 3 +- 4 files changed, 45 insertions(+), 43 deletions(-) diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md index 8d8548a..f584e05 100644 --- a/discourse/3-layered-global-occupancy-map.md +++ b/discourse/3-layered-global-occupancy-map.md @@ -97,7 +97,8 @@ relying on the arrival order of separate ROS messages. The first prototype uses `rmf_prototype_msgs/Region` for sparse 2D geometry so robots can publish compact patches. The central map service is responsible for -rasterizing those regions into its internal representation. +rasterizing those regions into its internal representation. The first +implementation accepts point and axis-aligned rectangle regions. # Example Flow @@ -123,7 +124,7 @@ The first server tests cover: * composing obstacle regions over a static planning grid * point regions with non-zero map origins and non-1.0 resolutions * rejecting updates without a timestamp -* polygon regions, out-of-bounds regions, and malformed region point arrays +* out-of-bounds regions, malformed point arrays, and unsupported region types * pruning expired observations by TTL * clear and obstacle patches in the same update * late older snapshots being ignored diff --git a/map_server/rmf_layered_map_server/README.md b/map_server/rmf_layered_map_server/README.md index abd008d..c081fda 100644 --- a/map_server/rmf_layered_map_server/README.md +++ b/map_server/rmf_layered_map_server/README.md @@ -22,6 +22,9 @@ obstacle patches both use TTLs in seconds. Updates with a zero source timestamp are rejected. +The current implementation accepts point and axis-aligned rectangle regions; +it logs and ignores other region types. + ## Visualization Smoke Test Build and source the workspace: diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs index d6e0fd1..fd72cb2 100644 --- a/map_server/rmf_layered_map_server/src/lib.rs +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -124,7 +124,12 @@ impl LayeredMap { _ => continue, }; - if patch.regions.is_empty() { + let regions: Vec<_> = patch + .regions + .into_iter() + .filter(|region| region_validation_error(region).is_none()) + .collect(); + if regions.is_empty() { continue; } @@ -152,7 +157,7 @@ impl LayeredMap { map_name: key.map_name.clone(), update_type, occupancy_value, - regions: patch.regions, + regions, expires_at_nsec, }); changed = true; @@ -403,31 +408,34 @@ fn log_region_update_errors(node: &Node, update: &MapRegionUpdate) { } } -fn region_validation_error(region: &Region) -> Option<&'static str> { +fn region_validation_error(region: &Region) -> Option { if region.points.len() < 2 { - return Some("region must contain at least one x/y point pair"); + return Some("region must contain at least one x/y point pair".to_string()); } if region.points.len() % 2 != 0 { - return Some("region points must contain complete x/y pairs"); + return Some("region points must contain complete x/y pairs".to_string()); } - if !is_supported_region_hint(region.hint) { - return Some("unsupported region hint"); + match region.hint { + Region::HINT_POINT if region.points.len() != 2 => { + Some("point region must contain exactly one x/y pair".to_string()) + } + Region::HINT_AXIS_ALIGNED_RECTANGLE if region.points.len() < 4 => { + Some("axis-aligned rectangle must contain at least two x/y pairs".to_string()) + } + Region::HINT_POINT | Region::HINT_AXIS_ALIGNED_RECTANGLE => None, + hint => Some(format!( + "unsupported region hint {}; expected a point or axis-aligned rectangle", + hint + )), } - - None } fn is_supported_region_hint(hint: u8) -> bool { matches!( hint, - Region::HINT_UNSPECIFIED - | Region::HINT_POINT - | Region::HINT_AXIS_ALIGNED_RECTANGLE - | Region::HINT_RECTANGLE - | Region::HINT_CONVEX_POLYGON - | Region::HINT_POLYGON + Region::HINT_POINT | Region::HINT_AXIS_ALIGNED_RECTANGLE ) } @@ -714,13 +722,6 @@ mod tests { } } - fn polygon(points: Vec) -> Region { - Region { - hint: Region::HINT_POLYGON, - points, - } - } - fn patch(update_type: u8, regions: Vec) -> MapRegionPatch { MapRegionPatch { update_type, @@ -785,36 +786,34 @@ mod tests { } #[test] - fn polygon_regions_fill_only_cells_inside_the_polygon() { + fn out_of_bounds_regions_do_not_touch_the_grid() { let mut map = LayeredMap::default(); - map.set_static_map(static_grid(4, 4, 0)); + map.set_static_map(static_grid(3, 3, 0)); assert!(map.ingest_region_update( update( MapRegionPatch::UPDATE_OBSTACLE, - vec![polygon(vec![1.0, 1.0, 3.0, 1.0, 3.0, 3.0, 1.0, 3.0])] + vec![rectangle(10.0, 10.0, 11.0, 11.0)] ), 0, )); let composed = map.compose().unwrap(); - assert_eq!(composed.data[1 * 4 + 1], 100); - assert_eq!(composed.data[1 * 4 + 2], 100); - assert_eq!(composed.data[2 * 4 + 1], 100); - assert_eq!(composed.data[2 * 4 + 2], 100); - assert_eq!(composed.data[0], 0); - assert_eq!(composed.data[3 * 4 + 3], 0); + assert!(composed.data.iter().all(|cell| *cell == 0)); } #[test] - fn out_of_bounds_regions_do_not_touch_the_grid() { + fn invalid_region_points_are_ignored() { let mut map = LayeredMap::default(); map.set_static_map(static_grid(3, 3, 0)); - assert!(map.ingest_region_update( + assert!(!map.ingest_region_update( update( MapRegionPatch::UPDATE_OBSTACLE, - vec![rectangle(10.0, 10.0, 11.0, 11.0)] + vec![Region { + hint: Region::HINT_POINT, + points: vec![1.0, 1.0, 2.0, 2.0], + }] ), 0, )); @@ -824,23 +823,21 @@ mod tests { } #[test] - fn invalid_region_points_are_ignored() { + fn unsupported_region_types_are_ignored() { let mut map = LayeredMap::default(); map.set_static_map(static_grid(3, 3, 0)); - assert!(map.ingest_region_update( + assert!(!map.ingest_region_update( update( MapRegionPatch::UPDATE_OBSTACLE, vec![Region { hint: Region::HINT_POLYGON, - points: vec![1.0, 1.0, 2.0], + points: vec![0.0, 0.0, 2.0, 0.0, 1.0, 2.0], }] ), 0, )); - - let composed = map.compose().unwrap(); - assert!(composed.data.iter().all(|cell| *cell == 0)); + assert_eq!(map.dynamic_observation_count(), 0); } #[test] diff --git a/rmf_layered_map_msgs/msg/MapRegionPatch.msg b/rmf_layered_map_msgs/msg/MapRegionPatch.msg index 07a5800..dcd5916 100644 --- a/rmf_layered_map_msgs/msg/MapRegionPatch.msg +++ b/rmf_layered_map_msgs/msg/MapRegionPatch.msg @@ -22,5 +22,6 @@ int8 occupancy_value # observations. float64 ttl_sec -# 2D geometric observation patches. +# 2D geometric observation patches. The first prototype supports point and +# axis-aligned rectangle regions. rmf_prototype_msgs/Region[] regions From 2f8c0c7bc311f5db4f885c119377778c149b9bdd Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Fri, 10 Jul 2026 18:19:28 +0200 Subject: [PATCH 15/15] fix: transform robot-local map regions 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 --- discourse/3-layered-global-occupancy-map.md | 23 ++- map_server/rmf_layered_map_server/README.md | 9 +- map_server/rmf_layered_map_server/src/lib.rs | 165 ++++++++++++++++-- .../demo_publisher.py | 1 + .../test/test_layered_map_server.py | 1 + .../msg/MapObservationSource.msg | 9 +- rmf_layered_map_msgs/msg/MapRegionPatch.msg | 6 +- 7 files changed, 184 insertions(+), 30 deletions(-) diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md index f584e05..2985994 100644 --- a/discourse/3-layered-global-occupancy-map.md +++ b/discourse/3-layered-global-occupancy-map.md @@ -48,14 +48,16 @@ describes where an observation came from. Important fields: -* `header`: frame for the observation and its required, non-zero timestamp +* `header`: global frame for the observation and its required, non-zero + timestamp * `source_id`: stable source identifier, usually the robot namespace plus the local source name * `robot_name`: robot that produced this observation, if the source is mounted on a robot. This can be empty for fixed sensors or synthetic map layers * `map_name`: map or level that the observations belong to -* `robot_pose`: robot pose when the observation was produced, so consumers do - not need to reconstruct this from a separate TF-like lookup +* `robot_pose`: pose of the robot-local observation frame in `header.frame_id` + when the observation was produced, so consumers do not need to reconstruct + it from a separate TF-like lookup * `default_ttl_sec`: fallback TTL in seconds for patches from this source ## `MapRegionUpdate` @@ -96,15 +98,19 @@ obstacle patches, which gives deterministic "clear then mark" behavior without relying on the arrival order of separate ROS messages. The first prototype uses `rmf_prototype_msgs/Region` for sparse 2D geometry so -robots can publish compact patches. The central map service is responsible for -rasterizing those regions into its internal representation. The first -implementation accepts point and axis-aligned rectangle regions. +robots can publish compact patches. Region coordinates are robot-local. The map +service transforms them through `source.robot_pose` into +`source.header.frame_id` before rasterizing them. Fixed sensors can publish +sensor-local regions with their sensor pose, while synthetic sources whose +regions are already global can use the identity pose. The first implementation +accepts point and axis-aligned rectangle regions. # Example Flow A local costmap or LiDAR observation node can publish a replacement snapshot by: -1. Stamping the observation source with the sensor frame and observation time. +1. Stamping the observation source with the global frame and observation time, + and including the pose of the robot-local observation frame. 2. Setting `reset_source` if the new snapshot replaces the source's previous temporary observations. 3. Adding one or more clear patches for free space seen by the sensor. @@ -123,8 +129,9 @@ The first server tests cover: * composing obstacle regions over a static planning grid * point regions with non-zero map origins and non-1.0 resolutions -* rejecting updates without a timestamp +* transforming robot-local regions into the global map frame * out-of-bounds regions, malformed point arrays, and unsupported region types +* rejecting updates without a timestamp * pruning expired observations by TTL * clear and obstacle patches in the same update * late older snapshots being ignored diff --git a/map_server/rmf_layered_map_server/README.md b/map_server/rmf_layered_map_server/README.md index c081fda..3b9c7bc 100644 --- a/map_server/rmf_layered_map_server/README.md +++ b/map_server/rmf_layered_map_server/README.md @@ -20,10 +20,11 @@ patches. If the snapshot replaces the source's previous observation state, set `map_name` before adding the new patches. Resetting has no TTL. Clear and obstacle patches both use TTLs in seconds. -Updates with a zero source timestamp are rejected. - -The current implementation accepts point and axis-aligned rectangle regions; -it logs and ignores other region types. +Patch regions are expressed in the robot-local observation frame. The server +uses `source.robot_pose` to transform them into `source.header.frame_id`, which +must match the static occupancy grid frame. Updates need a non-zero source +timestamp. The current implementation accepts point and axis-aligned rectangle +regions. It logs and ignores other region types. ## Visualization Smoke Test diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs index fd72cb2..f5529ce 100644 --- a/map_server/rmf_layered_map_server/src/lib.rs +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -14,6 +14,7 @@ use rclrs::{IntoPrimitiveOptions, Node, PrimitiveOptions}; use ros_env::{ + geometry_msgs::msg::Pose, nav_msgs::msg::OccupancyGrid, rmf_layered_map_msgs::msg::{MapRegionPatch, MapRegionUpdate}, rmf_prototype_msgs::msg::Region, @@ -33,6 +34,7 @@ enum DynamicUpdateType { struct DynamicObservation { source_id: String, map_name: String, + frame_id: String, update_type: DynamicUpdateType, occupancy_value: i8, regions: Vec, @@ -80,7 +82,7 @@ impl LayeredMap { } pub fn ingest_region_update(&mut self, update: MapRegionUpdate, now_nsec: i128) -> bool { - if update.source.header.stamp.sec == 0 && update.source.header.stamp.nanosec == 0 { + if source_validation_error(&update, self.static_frame_id()).is_some() { return false; } @@ -110,6 +112,8 @@ impl LayeredMap { } let default_ttl_sec = update.source.default_ttl_sec; + let frame_id = update.source.header.frame_id; + let robot_pose = update.source.robot_pose; let mut patches = update.patches; patches.sort_by_key(|patch| match patch.update_type { MapRegionPatch::UPDATE_CLEAR => 0, @@ -128,6 +132,7 @@ impl LayeredMap { .regions .into_iter() .filter(|region| region_validation_error(region).is_none()) + .map(|region| transform_region(region, &robot_pose)) .collect(); if regions.is_empty() { continue; @@ -155,6 +160,7 @@ impl LayeredMap { self.dynamic_observations.push(DynamicObservation { source_id: key.source_id.clone(), map_name: key.map_name.clone(), + frame_id: frame_id.clone(), update_type, occupancy_value, regions, @@ -187,25 +193,30 @@ impl LayeredMap { pub fn compose(&self) -> Option { let mut composed = self.static_grid.clone()?; normalize_grid_data(&mut composed); + let frame_id = composed.header.frame_id.clone(); for observation in self .dynamic_observations .iter() - .filter(|obs| obs.update_type == DynamicUpdateType::Clear) + .filter(|obs| obs.frame_id == frame_id && obs.update_type == DynamicUpdateType::Clear) { rasterize_observation(&mut composed, observation); } - for observation in self - .dynamic_observations - .iter() - .filter(|obs| obs.update_type == DynamicUpdateType::Obstacle) - { + for observation in self.dynamic_observations.iter().filter(|obs| { + obs.frame_id == frame_id && obs.update_type == DynamicUpdateType::Obstacle + }) { rasterize_observation(&mut composed, observation); } Some(composed) } + + fn static_frame_id(&self) -> Option<&str> { + self.static_grid + .as_ref() + .map(|grid| grid.header.frame_id.as_str()) + } } impl Default for LayeredMap { @@ -269,7 +280,7 @@ impl LayeredMapServer { } fn handle_region_update(&mut self, msg: MapRegionUpdate) { - log_region_update_errors(&self.node, &msg); + log_region_update_errors(&self.node, &msg, self.map.static_frame_id()); let now_nsec = self.now_nsec(); if self.map.ingest_region_update(msg, now_nsec) { rclrs::log!( @@ -386,7 +397,16 @@ fn region_update_qos(topic: &str) -> PrimitiveOptions<'_> { topic.keep_last(MAP_QOS_DEPTH).reliable() } -fn log_region_update_errors(node: &Node, update: &MapRegionUpdate) { +fn log_region_update_errors(node: &Node, update: &MapRegionUpdate, expected_frame: Option<&str>) { + if let Some(error) = source_validation_error(update, expected_frame) { + rclrs::log_error!( + node.logger(), + "Ignoring map region update from '{}': {}", + update.source.source_id, + error + ); + } + for patch in &update.patches { if !matches!( patch.update_type, @@ -408,6 +428,56 @@ fn log_region_update_errors(node: &Node, update: &MapRegionUpdate) { } } +fn source_validation_error( + update: &MapRegionUpdate, + expected_frame: Option<&str>, +) -> Option { + let stamp = &update.source.header.stamp; + if stamp.sec == 0 && stamp.nanosec == 0 { + return Some("source timestamp must be non-zero".to_string()); + } + + let frame_id = update.source.header.frame_id.as_str(); + if frame_id.is_empty() { + return Some("source frame must not be empty".to_string()); + } + + if let Some(expected) = expected_frame { + if !expected.is_empty() && expected != frame_id { + return Some(format!( + "source frame '{}' does not match map frame '{}'", + frame_id, expected + )); + } + } + + let pose = &update.source.robot_pose; + if ![ + pose.position.x, + pose.position.y, + pose.position.z, + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ] + .into_iter() + .all(f64::is_finite) + { + return Some("source pose must contain finite values".to_string()); + } + + let orientation_norm = pose.orientation.x * pose.orientation.x + + pose.orientation.y * pose.orientation.y + + pose.orientation.z * pose.orientation.z + + pose.orientation.w * pose.orientation.w; + if orientation_norm <= f64::EPSILON { + return Some("source pose must contain a valid orientation".to_string()); + } + + None +} + fn region_validation_error(region: &Region) -> Option { if region.points.len() < 2 { return Some("region must contain at least one x/y point pair".to_string()); @@ -432,13 +502,49 @@ fn region_validation_error(region: &Region) -> Option { } } -fn is_supported_region_hint(hint: u8) -> bool { +fn is_rasterizable_region_hint(hint: u8) -> bool { matches!( hint, - Region::HINT_POINT | Region::HINT_AXIS_ALIGNED_RECTANGLE + Region::HINT_POINT | Region::HINT_AXIS_ALIGNED_RECTANGLE | Region::HINT_CONVEX_POLYGON ) } +fn transform_region(mut region: Region, pose: &Pose) -> Region { + let points = point_pairs(®ion); + let points = if region.hint == Region::HINT_AXIS_ALIGNED_RECTANGLE { + let (min_x, min_y, max_x, max_y) = bounds(&points); + region.hint = Region::HINT_CONVEX_POLYGON; + vec![ + (min_x, min_y), + (max_x, min_y), + (max_x, max_y), + (min_x, max_y), + ] + } else { + points + }; + + let (cos_yaw, sin_yaw) = planar_rotation(pose); + region.points = points + .into_iter() + .flat_map(|(x, y)| { + let map_x = pose.position.x + cos_yaw * x - sin_yaw * y; + let map_y = pose.position.y + sin_yaw * x + cos_yaw * y; + [map_x as f32, map_y as f32] + }) + .collect(); + region +} + +fn planar_rotation(pose: &Pose) -> (f64, f64) { + let q = &pose.orientation; + let norm = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; + let sin_yaw = 2.0 * (q.w * q.z + q.x * q.y) / norm; + let cos_yaw = 1.0 - 2.0 * (q.y * q.y + q.z * q.z) / norm; + let yaw = sin_yaw.atan2(cos_yaw); + (yaw.cos(), yaw.sin()) +} + fn normalize_grid_data(grid: &mut OccupancyGrid) { let Some(expected_len) = grid_len(grid) else { grid.data.clear(); @@ -477,7 +583,7 @@ fn rasterized_indices(grid: &OccupancyGrid, region: &Region) -> Vec { return Vec::new(); } - if !is_supported_region_hint(region.hint) { + if !is_rasterizable_region_hint(region.hint) { return Vec::new(); } @@ -666,6 +772,10 @@ mod tests { fn static_grid(width: u32, height: u32, value: i8) -> OccupancyGrid { OccupancyGrid { + header: Header { + frame_id: "map".to_string(), + ..Default::default() + }, info: MapMetaData { resolution: 1.0, width, @@ -705,6 +815,7 @@ mod tests { ..Default::default() }; source.header.stamp.sec = 1; + source.robot_pose.orientation.w = 1.0; source } @@ -785,6 +896,24 @@ mod tests { assert_eq!(composed.data[0], 0); } + #[test] + fn robot_local_regions_are_transformed_into_the_map_frame() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(5, 5, 0)); + + let mut update = update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 0.0)]); + update.source.robot_pose.position.x = 2.25; + update.source.robot_pose.position.y = 1.25; + update.source.robot_pose.orientation.z = std::f64::consts::FRAC_1_SQRT_2; + update.source.robot_pose.orientation.w = std::f64::consts::FRAC_1_SQRT_2; + + assert!(map.ingest_region_update(update, 0)); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[2 * 5 + 2], 100); + assert_eq!(composed.data[0], 0); + } + #[test] fn out_of_bounds_regions_do_not_touch_the_grid() { let mut map = LayeredMap::default(); @@ -852,6 +981,18 @@ mod tests { assert_eq!(map.dynamic_observation_count(), 0); } + #[test] + fn updates_in_a_different_global_frame_are_rejected() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + + let mut update = update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]); + update.source.header.frame_id = "odom".to_string(); + + assert!(!map.ingest_region_update(update, NANOS_PER_SECOND)); + assert_eq!(map.dynamic_observation_count(), 0); + } + #[test] fn expired_observations_are_removed() { let mut map = LayeredMap::default(); diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py index 988b54d..3fdf855 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.py @@ -88,6 +88,7 @@ def obstacle_update(): msg = MapRegionUpdate() msg.source = MapObservationSource() msg.source.header.frame_id = 'map' + msg.source.robot_pose.orientation.w = 1.0 msg.source.source_id = 'demo/temporary_obstacle' msg.source.robot_name = 'demo_robot' msg.source.map_name = 'demo_map' diff --git a/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py b/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py index b4fac91..235f0ac 100644 --- a/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py +++ b/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py @@ -182,6 +182,7 @@ def static_map(): def map_source(): msg = MapObservationSource() msg.header.frame_id = 'map' + msg.robot_pose.orientation.w = 1.0 msg.source_id = 'test/obstacle' msg.robot_name = 'test_robot' msg.map_name = 'test_map' diff --git a/rmf_layered_map_msgs/msg/MapObservationSource.msg b/rmf_layered_map_msgs/msg/MapObservationSource.msg index 18f5d1c..a35cd00 100644 --- a/rmf_layered_map_msgs/msg/MapObservationSource.msg +++ b/rmf_layered_map_msgs/msg/MapObservationSource.msg @@ -1,6 +1,6 @@ # MapObservationSource.msg -# Frame and time for this observation snapshot. The timestamp is required. +# Global frame and time for this observation snapshot. The timestamp is required. # Consumers should reject updates with a zero timestamp. std_msgs/Header header @@ -15,9 +15,10 @@ string robot_name # Map or level that the observations belong to. string map_name -# 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 +# Pose of the robot-local observation frame in header.frame_id when this +# snapshot was produced. Fixed sensors can use their sensor pose. Sources whose +# regions are already in header.frame_id should use the identity pose. +geometry_msgs/Pose robot_pose # Fallback time-to-live in seconds for patches from this source. float64 default_ttl_sec diff --git a/rmf_layered_map_msgs/msg/MapRegionPatch.msg b/rmf_layered_map_msgs/msg/MapRegionPatch.msg index dcd5916..14dc00b 100644 --- a/rmf_layered_map_msgs/msg/MapRegionPatch.msg +++ b/rmf_layered_map_msgs/msg/MapRegionPatch.msg @@ -22,6 +22,8 @@ int8 occupancy_value # observations. float64 ttl_sec -# 2D geometric observation patches. The first prototype supports point and -# axis-aligned rectangle regions. +# 2D geometric observation patches in the robot-local observation frame. The +# map service transforms them into source.header.frame_id using +# source.robot_pose. The first prototype supports point and axis-aligned +# rectangle regions. rmf_prototype_msgs/Region[] regions