From bf28e04b81c8914275214f7836ee55ea9b69b66d Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 9 Jul 2026 02:26:10 +0200 Subject: [PATCH 01/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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/38] 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 From 16153e9456babbe30d8e18861c590cb763501f0a Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Mon, 13 Jul 2026 20:54:34 +0200 Subject: [PATCH 16/38] nav2: support localization-only multi-robot bringup Signed-off-by: SamuelFoo --- .../launch/bringup_launch.py | 8 +++++ .../cloned_multi_tb3_simulation_launch.py | 9 +++++ .../launch/tb3_simulation_launch.py | 36 ++++++++++++++++++- 3 files changed, 52 insertions(+), 1 deletion(-) diff --git a/nav2_integration/sp_demo_nav2_bringup/launch/bringup_launch.py b/nav2_integration/sp_demo_nav2_bringup/launch/bringup_launch.py index eee19f6..352987a 100644 --- a/nav2_integration/sp_demo_nav2_bringup/launch/bringup_launch.py +++ b/nav2_integration/sp_demo_nav2_bringup/launch/bringup_launch.py @@ -49,6 +49,7 @@ def generate_launch_description(): use_respawn = LaunchConfiguration('use_respawn') log_level = LaunchConfiguration('log_level') use_localization = LaunchConfiguration('use_localization') + use_navigation = LaunchConfiguration('use_navigation') # Map fully qualified names to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative @@ -105,6 +106,11 @@ def generate_launch_description(): description='Whether to enable localization or not' ) + declare_use_navigation_cmd = DeclareLaunchArgument( + 'use_navigation', default_value='True', + description='Whether to enable the Nav2 planning and control stack' + ) + declare_use_sim_time_cmd = DeclareLaunchArgument( 'use_sim_time', default_value='false', @@ -186,6 +192,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(launch_dir, 'navigation_launch.py') ), + condition=IfCondition(use_navigation), launch_arguments={ 'namespace': namespace, 'use_sim_time': use_sim_time, @@ -217,6 +224,7 @@ def generate_launch_description(): ld.add_action(declare_use_respawn_cmd) ld.add_action(declare_log_level_cmd) ld.add_action(declare_use_localization_cmd) + ld.add_action(declare_use_navigation_cmd) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) diff --git a/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index 55b7b3b..1c793c0 100644 --- a/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -63,6 +63,7 @@ def generate_launch_description(): rviz_config_file = LaunchConfiguration('rviz_config') use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') use_rviz = LaunchConfiguration('use_rviz') + use_navigation = LaunchConfiguration('use_navigation') log_settings = LaunchConfiguration('log_settings', default='true') # Declare the launch arguments @@ -108,6 +109,12 @@ def generate_launch_description(): 'use_rviz', default_value='True', description='Whether to start RVIZ' ) + declare_use_navigation_cmd = DeclareLaunchArgument( + 'use_navigation', + default_value='True', + description='Whether to enable the Nav2 planning and control stack', + ) + # Start Gazebo with plugin providing the robot spawning service world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') world_sdf_xacro = ExecuteProcess( @@ -166,6 +173,7 @@ def generate_launch_description(): 'use_simulator': 'False', 'headless': 'False', 'use_robot_state_pub': use_robot_state_pub, + 'use_navigation': use_navigation, 'x_pose': TextSubstitution(text=str(init_pose['x'])), 'y_pose': TextSubstitution(text=str(init_pose['y'])), 'z_pose': TextSubstitution(text=str(init_pose['z'])), @@ -199,6 +207,7 @@ def generate_launch_description(): ld.add_action(declare_autostart_cmd) ld.add_action(declare_rviz_config_file_cmd) ld.add_action(declare_use_robot_state_pub_cmd) + ld.add_action(declare_use_navigation_cmd) # initial localization node for robot_name in robots_list: diff --git a/nav2_integration/sp_demo_nav2_bringup/launch/tb3_simulation_launch.py b/nav2_integration/sp_demo_nav2_bringup/launch/tb3_simulation_launch.py index db1964f..2983f82 100644 --- a/nav2_integration/sp_demo_nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_integration/sp_demo_nav2_bringup/launch/tb3_simulation_launch.py @@ -27,7 +27,7 @@ OpaqueFunction, RegisterEventHandler, ) -from launch.conditions import IfCondition +from launch.conditions import IfCondition, UnlessCondition from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -39,6 +39,7 @@ def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') launch_dir = os.path.join(bringup_dir, 'launch') + prototype_bringup_dir = get_package_share_directory('sp_demo_nav2_bringup') sim_dir = get_package_share_directory('nav2_minimal_tb3_sim') # Create the launch configuration variables @@ -51,6 +52,7 @@ def generate_launch_description(): autostart = LaunchConfiguration('autostart') use_composition = LaunchConfiguration('use_composition') use_respawn = LaunchConfiguration('use_respawn') + use_navigation = LaunchConfiguration('use_navigation') # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration('rviz_config_file') @@ -122,6 +124,12 @@ def generate_launch_description(): description='Whether to respawn if a node crashes. Applied when composition is disabled.', ) + declare_use_navigation_cmd = DeclareLaunchArgument( + 'use_navigation', + default_value='True', + description='Whether to enable the Nav2 planning and control stack', + ) + declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), @@ -194,6 +202,29 @@ def generate_launch_description(): bringup_cmd = IncludeLaunchDescription( PythonLaunchDescriptionSource(os.path.join(launch_dir, 'bringup_launch.py')), + condition=IfCondition(use_navigation), + launch_arguments={ + 'namespace': namespace, + 'use_namespace': use_namespace, + 'slam': slam, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'autostart': autostart, + 'use_composition': use_composition, + 'use_respawn': use_respawn, + 'use_navigation': use_navigation, + }.items(), + ) + localization_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + prototype_bringup_dir, + 'launch', + 'bringup_launch.py', + ) + ), + condition=UnlessCondition(use_navigation), launch_arguments={ 'namespace': namespace, 'use_namespace': use_namespace, @@ -204,6 +235,7 @@ def generate_launch_description(): 'autostart': autostart, 'use_composition': use_composition, 'use_respawn': use_respawn, + 'use_navigation': 'False', }.items(), ) # The SDF file for the world is a xacro file because we wanted to @@ -272,6 +304,7 @@ def generate_launch_description(): ld.add_action(declare_robot_name_cmd) ld.add_action(declare_robot_sdf_cmd) ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_use_navigation_cmd) ld.add_action(world_sdf_xacro) ld.add_action(remove_temp_sdf_file) @@ -283,5 +316,6 @@ def generate_launch_description(): ld.add_action(start_robot_state_publisher_cmd) ld.add_action(rviz_cmd) ld.add_action(bringup_cmd) + ld.add_action(localization_cmd) return ld From 1fa278fd746642980b3a824b79a6a7b87af34816 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Mon, 13 Jul 2026 20:54:52 +0200 Subject: [PATCH 17/38] demo: combine Nav2 scan observations Signed-off-by: SamuelFoo --- .../launch/nav2_observations.launch.py | 203 ++++++++++++++++++ .../rmf_layered_map_server_demo/package.xml | 10 + .../demo_clutter_spawner.py | 109 ++++++++++ .../scan_conversion.py | 46 ++++ .../scan_region_publisher.py | 195 +++++++++++++++++ .../rviz/nav2_observations.rviz | 104 +++++++++ .../rmf_layered_map_server_demo/setup.py | 4 + .../test/test_copyright.py | 23 ++ .../test/test_pep257.py | 23 ++ .../test/test_scan_conversion.py | 64 ++++++ 10 files changed, 781 insertions(+) create mode 100644 map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py create mode 100644 map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_clutter_spawner.py create mode 100644 map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_conversion.py create mode 100644 map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py create mode 100644 map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz create mode 100644 map_server/rmf_layered_map_server_demo/test/test_copyright.py create mode 100644 map_server/rmf_layered_map_server_demo/test/test_pep257.py create mode 100644 map_server/rmf_layered_map_server_demo/test/test_scan_conversion.py diff --git a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py new file mode 100644 index 0000000..96efb58 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py @@ -0,0 +1,203 @@ +# 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, + EmitEvent, + ExecuteProcess, + RegisterEventHandler, + TimerAction, +) +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 +from launch_ros.parameter_descriptions import ParameterValue + + +ROBOTS = ( + ('robot0', -12.0, -21.0, 0.7854), + ('robot1', 10.0, -21.0, 2.3562), + ('robot2', -12.0, 21.0, -0.7854), +) + + +def robots_argument(): + """Format the three warehouse-corner robot poses for Nav2 bringup.""" + return '; '.join( + f'{name}={{x: {x}, y: {y}, yaw: {yaw}}}' + for name, x, y, yaw in ROBOTS + ) + + +def generate_launch_description(): + demo_share = get_package_share_directory('rmf_layered_map_server_demo') + nav2_share = get_package_share_directory('sp_demo_nav2_bringup') + + map_file = LaunchConfiguration('map') + params_file = LaunchConfiguration('params_file') + use_nav2_rviz = LaunchConfiguration('use_nav2_rviz') + use_global_rviz = LaunchConfiguration('use_global_rviz') + spawn_clutter = LaunchConfiguration('spawn_clutter') + beam_stride = LaunchConfiguration('beam_stride') + publish_period_sec = LaunchConfiguration('publish_period_sec') + ttl_sec = LaunchConfiguration('ttl_sec') + max_observation_range = LaunchConfiguration('max_observation_range') + + declarations = [ + DeclareLaunchArgument( + 'map', + default_value=os.path.join(nav2_share, 'maps', 'warehouse.yaml'), + description='Static map used by each Nav2 robot.', + ), + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join( + nav2_share, 'params', 'nav2_multirobot_params_all.yaml' + ), + description='Nav2 parameters shared by the three robots.', + ), + DeclareLaunchArgument( + 'use_nav2_rviz', + default_value='True', + description='Whether to open one local Nav2 RViz window per robot.', + ), + DeclareLaunchArgument( + 'use_global_rviz', + default_value='True', + description='Whether to open the combined global-map RViz window.', + ), + DeclareLaunchArgument( + 'spawn_clutter', + default_value='True', + description='Whether to spawn one deterministic obstacle near each robot.', + ), + DeclareLaunchArgument( + 'beam_stride', + default_value='1', + description='Publish every Nth valid laser return as a point region.', + ), + DeclareLaunchArgument( + 'publish_period_sec', + default_value='0.5', + description='Minimum period between region snapshots from each robot.', + ), + DeclareLaunchArgument( + 'ttl_sec', + default_value='1.0', + description='Lifetime of each robot observation snapshot.', + ), + DeclareLaunchArgument( + 'max_observation_range', + default_value='2.5', + description='Maximum laser return distance represented globally.', + ), + ] + + # ParseMultiRobotPose in the Nav2 demo reads sys.argv directly. + # Launch a managed child process so the robot list reaches that parser. + nav2_simulation = ExecuteProcess( + cmd=[ + 'ros2', + 'launch', + 'sp_demo_nav2_bringup', + 'cloned_multi_tb3_simulation_launch.py', + f'robots:={robots_argument()}', + ['map:=', map_file], + ['params_file:=', params_file], + ['use_rviz:=', use_nav2_rviz], + 'use_navigation:=False', + ], + output='screen', + ) + + layered_map_server = Node( + package='rmf_layered_map_server', + executable='rmf_layered_map_server', + output='screen', + remappings=[('/map/static', '/robot0/inner/map')], + ) + + observation_nodes = [] + for robot_name, _, _, _ in ROBOTS: + observation_nodes.append( + Node( + package='rmf_layered_map_server_demo', + executable='scan_region_publisher', + namespace=f'{robot_name}/inner', + output='screen', + parameters=[{ + 'robot_name': robot_name, + 'map_frame': 'map', + 'map_name': 'warehouse', + 'scan_topic': f'/{robot_name}/inner/scan', + 'beam_stride': ParameterValue(beam_stride, value_type=int), + 'publish_period_sec': ParameterValue( + publish_period_sec, value_type=float + ), + 'ttl_sec': ParameterValue(ttl_sec, value_type=float), + 'max_observation_range': ParameterValue( + max_observation_range, value_type=float + ), + }], + remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], + ) + ) + + clutter_spawner = TimerAction( + period=5.0, + actions=[ + Node( + condition=IfCondition(spawn_clutter), + package='rmf_layered_map_server_demo', + executable='layered_map_demo_clutter_spawner', + output='screen', + ), + ], + ) + + global_rviz = Node( + condition=IfCondition(use_global_rviz), + package='rviz2', + executable='rviz2', + name='layered_global_map_rviz', + arguments=[ + '-d', + os.path.join(demo_share, 'rviz', 'nav2_observations.rviz'), + ], + parameters=[{'use_sim_time': True}], + output='screen', + ) + global_rviz_exit_handler = RegisterEventHandler( + condition=IfCondition(use_global_rviz), + event_handler=OnProcessExit( + target_action=global_rviz, + on_exit=EmitEvent(event=Shutdown(reason='global RViz exited')), + ), + ) + + return LaunchDescription([ + *declarations, + nav2_simulation, + layered_map_server, + *observation_nodes, + clutter_spawner, + global_rviz, + global_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 index 6ad8738..90cd0c7 100644 --- a/map_server/rmf_layered_map_server_demo/package.xml +++ b/map_server/rmf_layered_map_server_demo/package.xml @@ -12,11 +12,21 @@ rclpy ament_index_python geometry_msgs + gz_tools_vendor + launch + launch_ros nav_msgs rmf_layered_map_msgs rmf_layered_map_server rmf_prototype_msgs rviz2 + sensor_msgs + sp_demo_nav2_bringup + tf2_ros_py + + ament_copyright + ament_pep257 + python3-pytest ament_python diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_clutter_spawner.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_clutter_spawner.py new file mode 100644 index 0000000..42fa6b9 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_clutter_spawner.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. + +import subprocess + +import rclpy +from rclpy.node import Node + + +DEMO_OBSTACLES = ( + ('layered_map_obstacle_0', -11.0, -20.0), + ('layered_map_obstacle_1', 9.0, -20.0), + ('layered_map_obstacle_2', -11.0, 20.0), +) + + +def box_sdf(name, x, y): + """Return a small static box model for a deterministic laser target.""" + return ( + "" + f"" + f'{x} {y} 0.5 0 0 0' + 'true' + "" + "" + '0.3 0.3 1.0' + '' + "" + '0.3 0.3 1.0' + '' + '' + '' + '' + ) + + +class DemoClutterSpawner(Node): + """Retry deterministic Gazebo box creation until the world is ready.""" + + def __init__(self): + super().__init__('layered_map_demo_clutter_spawner') + self.world_name = self.declare_parameter('world_name', 'warehouse').value + self.pending = list(DEMO_OBSTACLES) + self.timer = self.create_timer(2.0, self.spawn_pending) + + def spawn_pending(self): + for obstacle in list(self.pending): + name, x, y = obstacle + request = f'sdf: "{box_sdf(name, x, y)}"' + try: + result = subprocess.run( + [ + 'gz', + 'service', + '-s', + f'/world/{self.world_name}/create', + '--reqtype', + 'gz.msgs.EntityFactory', + '--reptype', + 'gz.msgs.Boolean', + '--timeout', + '1000', + '--req', + request, + ], + capture_output=True, + check=False, + text=True, + timeout=3.0, + ) + except (OSError, subprocess.TimeoutExpired) as error: + self.get_logger().warning(f'Waiting for Gazebo: {error}') + return + + if result.returncode != 0: + detail = result.stderr.strip() or result.stdout.strip() + self.get_logger().warning(f'Waiting for Gazebo: {detail}') + return + + self.pending.remove(obstacle) + self.get_logger().info(f'Spawned {name} at ({x}, {y})') + + if not self.pending: + self.get_logger().info('All layered-map demo obstacles are ready') + self.timer.cancel() + rclpy.shutdown() + + +def main(): + rclpy.init() + node = DemoClutterSpawner() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_conversion.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_conversion.py new file mode 100644 index 0000000..5cae001 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_conversion.py @@ -0,0 +1,46 @@ +# 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 math import cos, isfinite, sin + + +def scan_obstacle_points( + ranges, + angle_min, + angle_increment, + range_min, + range_max, + max_observation_range, + beam_stride, +): + """Convert valid laser returns into points in the scan frame.""" + if beam_stride < 1: + raise ValueError('beam_stride must be at least one') + + effective_max = range_max + if max_observation_range > 0.0: + effective_max = min(effective_max, max_observation_range) + + points = [] + for index in range(0, len(ranges), beam_stride): + distance = ranges[index] + if not isfinite(distance): + continue + if distance < range_min or distance > effective_max: + continue + + angle = angle_min + index * angle_increment + points.append((distance * cos(angle), distance * sin(angle))) + + return points diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py new file mode 100644 index 0000000..fba5e2e --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py @@ -0,0 +1,195 @@ +# 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 rclpy +from rclpy.node import Node +from rclpy.qos import ( + qos_profile_sensor_data, + QoSProfile, + ReliabilityPolicy, +) +from rclpy.time import Time +from rmf_layered_map_msgs.msg import ( + MapObservationSource, + MapRegionPatch, + MapRegionUpdate, +) +from rmf_prototype_msgs.msg import Region +from sensor_msgs.msg import LaserScan +from tf2_ros import Buffer, TransformException, TransformListener + +from .scan_conversion import scan_obstacle_points + + +class ScanRegionPublisher(Node): + """Publish one robot's laser endpoints as a replaceable region snapshot.""" + + def __init__(self): + super().__init__('scan_region_publisher') + + self.robot_name = self.declare_parameter('robot_name', '').value + self.map_frame = self.declare_parameter('map_frame', 'map').value + self.map_name = self.declare_parameter('map_name', 'warehouse').value + self.scan_topic = self.declare_parameter('scan_topic', 'scan').value + self.ttl_sec = self.declare_parameter('ttl_sec', 1.0).value + self.publish_period_sec = self.declare_parameter( + 'publish_period_sec', 0.5 + ).value + self.max_observation_range = self.declare_parameter( + 'max_observation_range', 2.5 + ).value + self.beam_stride = self.declare_parameter('beam_stride', 1).value + + if not self.robot_name: + raise ValueError('robot_name must not be empty') + if self.ttl_sec <= 0.0: + raise ValueError('ttl_sec must be positive') + if self.publish_period_sec < 0.0: + raise ValueError('publish_period_sec must not be negative') + if self.beam_stride < 1: + raise ValueError('beam_stride must be at least one') + + self.source_id = f'{self.robot_name}/scan' + self.last_publish_stamp_sec = None + self.publish_count = 0 + self.transform_failure_count = 0 + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + reliable_qos = QoSProfile( + depth=10, + reliability=ReliabilityPolicy.RELIABLE, + ) + self.region_update_publisher = self.create_publisher( + MapRegionUpdate, + '/map/region_updates', + reliable_qos, + ) + self.scan_subscription = self.create_subscription( + LaserScan, + self.scan_topic, + self.publish_scan, + qos_profile_sensor_data, + ) + + self.get_logger().info( + f'Converting {self.scan_topic} into region updates for ' + f'{self.robot_name} (stride={self.beam_stride}, ' + f'period={self.publish_period_sec:.2f}s)' + ) + + def publish_scan(self, scan): + stamp_sec = scan.header.stamp.sec + scan.header.stamp.nanosec / 1e9 + if scan.header.stamp.sec == 0 and scan.header.stamp.nanosec == 0: + self.get_logger().warning('Ignoring scan with a zero timestamp') + return + + if self.last_publish_stamp_sec is not None: + elapsed = stamp_sec - self.last_publish_stamp_sec + if 0.0 <= elapsed < self.publish_period_sec: + return + + try: + transform = self.tf_buffer.lookup_transform( + self.map_frame, + scan.header.frame_id, + Time.from_msg(scan.header.stamp), + ) + except TransformException: + # Gazebo can deliver a scan before the matching TF sample. + # These demo robots are stationary, so the latest transform is equivalent. + try: + transform = self.tf_buffer.lookup_transform( + self.map_frame, + scan.header.frame_id, + Time(), + ) + except TransformException as error: + self.transform_failure_count += 1 + if self.transform_failure_count == 1 or ( + self.transform_failure_count % 20 == 0 + ): + self.get_logger().warning( + f'Cannot transform {scan.header.frame_id} to ' + f'{self.map_frame}: {error}' + ) + return + + self.transform_failure_count = 0 + points = scan_obstacle_points( + scan.ranges, + scan.angle_min, + scan.angle_increment, + scan.range_min, + scan.range_max, + self.max_observation_range, + self.beam_stride, + ) + update = self.make_update(transform, points) + self.region_update_publisher.publish(update) + self.last_publish_stamp_sec = stamp_sec + self.publish_count += 1 + + if self.publish_count == 1 or self.publish_count % 20 == 0: + self.get_logger().info( + f'Published {len(points)} obstacle regions from ' + f'{len(scan.ranges)} laser beams' + ) + + def make_update(self, transform, points): + update = MapRegionUpdate() + update.reset_source = True + update.source = MapObservationSource() + # The Rust map server currently uses a system-time clock. + # Keep TTL and update ordering in that clock while using the scan stamp for TF. + update.source.header.stamp = self.get_clock().now().to_msg() + update.source.header.frame_id = self.map_frame + update.source.source_id = self.source_id + update.source.robot_name = self.robot_name + update.source.map_name = self.map_name + update.source.default_ttl_sec = self.ttl_sec + + translation = transform.transform.translation + rotation = transform.transform.rotation + update.source.robot_pose.position.x = translation.x + update.source.robot_pose.position.y = translation.y + update.source.robot_pose.position.z = translation.z + update.source.robot_pose.orientation = rotation + + if not points: + return update + + patch = MapRegionPatch() + patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + patch.occupancy_value = 100 + patch.ttl_sec = self.ttl_sec + for x, y in points: + region = Region() + region.hint = Region.HINT_POINT + region.points = [x, y] + patch.regions.append(region) + update.patches.append(patch) + return update + + +def main(): + rclpy.init() + node = ScanRegionPublisher() + 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/nav2_observations.rviz b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz new file mode 100644 index 0000000..f8c22bc --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz @@ -0,0 +1,104 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Combined Global 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: 60 + 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: Combined Global 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: 55 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 900 + Hide Left Dock: false + Hide Right Dock: false + Views: + collapsed: false + Width: 1200 + X: 180 + Y: 90 diff --git a/map_server/rmf_layered_map_server_demo/setup.py b/map_server/rmf_layered_map_server_demo/setup.py index 28bb275..8349ab2 100644 --- a/map_server/rmf_layered_map_server_demo/setup.py +++ b/map_server/rmf_layered_map_server_demo/setup.py @@ -39,7 +39,11 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'layered_map_demo_clutter_spawner = ' + 'rmf_layered_map_server_demo.demo_clutter_spawner:main', 'layered_map_demo_publisher = rmf_layered_map_server_demo.demo_publisher:main', + 'scan_region_publisher = ' + 'rmf_layered_map_server_demo.scan_region_publisher:main', ], }, ) diff --git a/map_server/rmf_layered_map_server_demo/test/test_copyright.py b/map_server/rmf_layered_map_server_demo/test/test_copyright.py new file mode 100644 index 0000000..f87f331 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/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_demo/test/test_pep257.py b/map_server/rmf_layered_map_server_demo/test/test_pep257.py new file mode 100644 index 0000000..1b15a96 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/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 diff --git a/map_server/rmf_layered_map_server_demo/test/test_scan_conversion.py b/map_server/rmf_layered_map_server_demo/test/test_scan_conversion.py new file mode 100644 index 0000000..d848bcd --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_scan_conversion.py @@ -0,0 +1,64 @@ +# 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 math import inf, nan, pi + +import pytest + +from rmf_layered_map_server_demo.scan_conversion import scan_obstacle_points + + +def test_scan_filters_invalid_and_out_of_range_returns(): + points = scan_obstacle_points( + [nan, inf, 0.05, 1.0, 3.0], + angle_min=0.0, + angle_increment=pi / 2.0, + range_min=0.1, + range_max=10.0, + max_observation_range=2.5, + beam_stride=1, + ) + + assert len(points) == 1 + assert points[0][0] == pytest.approx(0.0, abs=1e-6) + assert points[0][1] == pytest.approx(-1.0) + + +def test_scan_stride_preserves_original_beam_angles(): + points = scan_obstacle_points( + [1.0, 1.0, 1.0, 1.0], + angle_min=0.0, + angle_increment=pi / 2.0, + range_min=0.1, + range_max=5.0, + max_observation_range=0.0, + beam_stride=2, + ) + + assert len(points) == 2 + assert points[0] == pytest.approx((1.0, 0.0), abs=1e-6) + assert points[1] == pytest.approx((-1.0, 0.0), abs=1e-6) + + +def test_scan_rejects_invalid_stride(): + with pytest.raises(ValueError, match='beam_stride'): + scan_obstacle_points( + [], + angle_min=0.0, + angle_increment=0.1, + range_min=0.1, + range_max=5.0, + max_observation_range=2.5, + beam_stride=0, + ) From 6bc186fa7c86e3ae73748a99efe779289379ad11 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Mon, 13 Jul 2026 20:55:03 +0200 Subject: [PATCH 18/38] docs: describe Nav2 observation demo Signed-off-by: SamuelFoo --- discourse/3-layered-global-occupancy-map.md | 70 ++++++++++++++++--- map_server/README.md | 2 + .../rmf_layered_map_server_demo/README.md | 21 ++++++ 3 files changed, 83 insertions(+), 10 deletions(-) create mode 100644 map_server/rmf_layered_map_server_demo/README.md diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md index 2985994..60c0d46 100644 --- a/discourse/3-layered-global-occupancy-map.md +++ b/discourse/3-layered-global-occupancy-map.md @@ -2,8 +2,9 @@ # Layered Global Map Observations -This post describes the observation interface for contributing temporary map -information to the next generation Open-RMF prototype. +This post describes the implemented observation interface and demos for +contributing temporary map information to the next generation Open-RMF +prototype. 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 @@ -19,15 +20,21 @@ observations, can be added later. 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 +* Robot-mounted sources include the observation-frame pose at observation time +* The Rust map server composes a static occupancy grid and active observations + into `/map` +* The Nav2 demo converts scans from three robots into point regions and displays + the combined map * The observation messages live in `rmf_layered_map_msgs`, leaving `rmf_prototype_msgs` unchanged -# Observation Topic +# Observation Topics -Observation sources publish dynamic map observations on: +The layered map server uses these topics: +* `/map/static` - static `nav_msgs/OccupancyGrid` * `/map/region_updates` - [`MapRegionUpdate.msg`](../rmf_layered_map_msgs/msg/MapRegionUpdate.msg) +* `/map` - composed `nav_msgs/OccupancyGrid` 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 @@ -105,6 +112,47 @@ 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. +# Layered Map Server + +`rmf_layered_map_server` keeps the static occupancy grid separate from dynamic +observations and publishes their composition on `/map`. It validates source +timestamps and frames, ignores updates that are older than the latest accepted +update from the same source, supports source resets, and removes observations +after their TTL expires. + +The server applies clear patches before obstacle patches from the same update. +During composition, obstacle observations win over clear observations so +occupied space is not accidentally erased by another active source. + +# Three-Robot Nav2 Demo + +The launch file starts three stationary robots in different free corners of the +warehouse and spawns one deterministic Gazebo box near each robot. Each +observation node subscribes to its robot's local `sensor_msgs/LaserScan`, +filters invalid or out-of-range returns, and publishes the remaining scan +endpoints as point regions. + +Each scan is a replacement snapshot: `reset_source` removes the source's +preceding points before the new points are added, and a short TTL removes the +source if it stops publishing. The observation-frame pose is recorded in the +shared `map` frame so the map server can transform the scan-local regions before +rasterizing them. + +The demo launches Nav2 localization but does not launch Nav2 planning or control +components, RMF planning, or navigation goals. Three robot-local RViz windows +are enabled by default, and another RViz window displays the combined global +`/map`. + +The scan-to-region conversion is deliberately simple so its information loss +and publication cost are visible. `beam_stride` reduces the number of point +regions, `publish_period_sec` throttles replacement snapshots, and +`max_observation_range` limits represented returns. `ttl_sec` controls how +quickly stale snapshots expire. Each publisher logs its number of input beams +and output regions so message density, update rate, and visual fidelity can be +compared. The current demo publishes occupied endpoints only; it does not +convert raycast clearing into free-space regions or compress adjacent points +into larger regions. + # Example Flow A local costmap or LiDAR observation node can publish a replacement snapshot by: @@ -118,22 +166,24 @@ A local costmap or LiDAR observation node can publish a replacement snapshot by: 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 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 +The map server ignores snapshots from a source if their timestamp is older than +a newer snapshot that has already been accepted. This prevents a late clear or +reset message from removing obstacle information that came from a newer observation. # Implemented Test Coverage -The first server tests cover: +The committed server and demo tests cover: * composing obstacle regions over a static planning grid * point regions with non-zero map origins and non-1.0 resolutions * 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 +* rejecting updates without a timestamp or in a different global frame * pruning expired observations by TTL * 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 +* filtering invalid and out-of-range laser returns +* preserving original beam angles when scan points are sampled diff --git a/map_server/README.md b/map_server/README.md index 883cd14..ee71057 100644 --- a/map_server/README.md +++ b/map_server/README.md @@ -11,3 +11,5 @@ ros2 launch rmf_layered_map_server_demo demo.launch.py ``` Use `use_rviz:=False` for a headless run. + +See [`rmf_layered_map_server_demo/README.md`](rmf_layered_map_server_demo/README.md) for the three-robot Nav2 observation demo. diff --git a/map_server/rmf_layered_map_server_demo/README.md b/map_server/rmf_layered_map_server_demo/README.md new file mode 100644 index 0000000..c95c7fb --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/README.md @@ -0,0 +1,21 @@ +# Layered Map Server Demos + +This package contains two demonstrations of the layered map server. + +## TTL Smoke Test + +This demo publishes a synthetic static grid and a temporary rectangle with a five-second TTL: + +```bash +ros2 launch rmf_layered_map_server_demo demo.launch.py +``` + +## Three-Robot Nav2 Observation Demo + +This demo combines laser observations from three stationary Nav2 robots in the global `/map`: + +```bash +ros2 launch rmf_layered_map_server_demo nav2_observations.launch.py +``` + +Use `use_nav2_rviz:=False` to open only the combined-map view. Use `spawn_clutter:=False` to run against the unmodified warehouse world. From 924bf110f7129fd1e2d6d0a7b8817642e4cfd9e4 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Wed, 15 Jul 2026 22:47:33 +0200 Subject: [PATCH 19/38] demo: move robots through Nav2 goals Signed-off-by: SamuelFoo --- .../launch/nav2_observations.launch.py | 33 +++- .../rmf_layered_map_server_demo/package.xml | 2 + .../nav2_goal_publisher.py | 147 ++++++++++++++++++ .../scan_region_publisher.py | 41 ++--- .../rmf_layered_map_server_demo/setup.py | 2 + .../test/test_nav2_goal_publisher.py | 58 +++++++ .../cloned_multi_tb3_simulation_launch.py | 9 +- .../launch/tb3_simulation_launch.py | 40 +++-- .../sp_demo_nav2_bringup/package.xml | 1 + .../params/nav2_multirobot_params_all.yaml | 2 +- 10 files changed, 299 insertions(+), 36 deletions(-) create mode 100644 map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/nav2_goal_publisher.py create mode 100644 map_server/rmf_layered_map_server_demo/test/test_nav2_goal_publisher.py diff --git a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py index 96efb58..efeb3c1 100644 --- a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py +++ b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py @@ -37,6 +37,12 @@ ('robot2', -12.0, 21.0, -0.7854), ) +ROBOT_GOALS = { + 'robot0': ((-12.0, -16.0, 1.5708), (-12.0, -21.0, -1.5708)), + 'robot1': ((10.0, -16.0, 1.5708), (10.0, -21.0, -1.5708)), + 'robot2': ((-12.0, 16.0, -1.5708), (-12.0, 21.0, 1.5708)), +} + def robots_argument(): """Format the three warehouse-corner robot poses for Nav2 bringup.""" @@ -54,6 +60,7 @@ def generate_launch_description(): params_file = LaunchConfiguration('params_file') use_nav2_rviz = LaunchConfiguration('use_nav2_rviz') use_global_rviz = LaunchConfiguration('use_global_rviz') + move_robots = LaunchConfiguration('move_robots') spawn_clutter = LaunchConfiguration('spawn_clutter') beam_stride = LaunchConfiguration('beam_stride') publish_period_sec = LaunchConfiguration('publish_period_sec') @@ -83,6 +90,11 @@ def generate_launch_description(): default_value='True', description='Whether to open the combined global-map RViz window.', ), + DeclareLaunchArgument( + 'move_robots', + default_value='True', + description='Whether to cycle the robots through example Nav2 goals.', + ), DeclareLaunchArgument( 'spawn_clutter', default_value='True', @@ -122,7 +134,7 @@ def generate_launch_description(): ['map:=', map_file], ['params_file:=', params_file], ['use_rviz:=', use_nav2_rviz], - 'use_navigation:=False', + ['use_navigation:=', move_robots], ], output='screen', ) @@ -135,6 +147,7 @@ def generate_launch_description(): ) observation_nodes = [] + goal_nodes = [] for robot_name, _, _, _ in ROBOTS: observation_nodes.append( Node( @@ -159,6 +172,23 @@ def generate_launch_description(): remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], ) ) + goal_nodes.append( + Node( + condition=IfCondition(move_robots), + package='rmf_layered_map_server_demo', + executable='nav2_goal_publisher', + namespace=f'{robot_name}/inner', + output='screen', + parameters=[{ + 'use_sim_time': True, + 'waypoints': [ + value + for waypoint in ROBOT_GOALS[robot_name] + for value in waypoint + ], + }], + ) + ) clutter_spawner = TimerAction( period=5.0, @@ -197,6 +227,7 @@ def generate_launch_description(): nav2_simulation, layered_map_server, *observation_nodes, + *goal_nodes, clutter_spawner, global_rviz, global_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 index 90cd0c7..66c5d40 100644 --- a/map_server/rmf_layered_map_server_demo/package.xml +++ b/map_server/rmf_layered_map_server_demo/package.xml @@ -9,6 +9,7 @@ ament_python + action_msgs rclpy ament_index_python geometry_msgs @@ -16,6 +17,7 @@ launch launch_ros nav_msgs + nav2_msgs rmf_layered_map_msgs rmf_layered_map_server rmf_prototype_msgs diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/nav2_goal_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/nav2_goal_publisher.py new file mode 100644 index 0000000..649cb98 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/nav2_goal_publisher.py @@ -0,0 +1,147 @@ +# 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 math import cos, sin + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped +from nav2_msgs.action import NavigateToPose +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy + + +def parse_waypoints(values): + """Parse a flat sequence of x, y, yaw waypoint triples.""" + if not values or len(values) % 3 != 0: + raise ValueError('waypoints must contain one or more x, y, yaw triples') + + return [tuple(values[index:index + 3]) for index in range(0, len(values), 3)] + + +def goal_pose(x, y, yaw, stamp): + """Create a map-frame pose for a Nav2 goal.""" + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.header.stamp = stamp + pose.pose.position.x = x + pose.pose.position.y = y + pose.pose.orientation.z = sin(yaw / 2.0) + pose.pose.orientation.w = cos(yaw / 2.0) + return pose + + +def advance_waypoint(current, waypoint_count, status): + """Advance after a successful goal and otherwise retry the same waypoint.""" + if status == GoalStatus.STATUS_SUCCEEDED: + return (current + 1) % waypoint_count + return current + + +class Nav2GoalPublisher(Node): + """Cycle through demo waypoints whenever Nav2 completes a goal.""" + + def __init__(self): + super().__init__('nav2_goal_publisher') + self.waypoints = parse_waypoints( + self.declare_parameter('waypoints', [0.0, 0.0, 0.0]).value + ) + self.next_waypoint = 0 + self.goal_in_progress = False + self.waiting_logged = False + self.localized = False + self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + pose_qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, + ) + self.pose_subscription = self.create_subscription( + PoseWithCovarianceStamped, + 'amcl_pose', + self.receive_pose, + pose_qos, + ) + self.timer = self.create_timer(1.0, self.send_next_goal) + + def receive_pose(self, _): + self.localized = True + + def send_next_goal(self): + if self.goal_in_progress: + return + + if not self.localized or not self.action_client.server_is_ready(): + if not self.waiting_logged: + self.get_logger().info('Waiting for Nav2 localization and navigation') + self.waiting_logged = True + return + + self.waiting_logged = False + x, y, yaw = self.waypoints[self.next_waypoint] + goal = NavigateToPose.Goal() + goal.pose = goal_pose(x, y, yaw, self.get_clock().now().to_msg()) + self.goal_in_progress = True + future = self.action_client.send_goal_async(goal) + future.add_done_callback(self.goal_response) + self.get_logger().info(f'Sending demo goal ({x:.1f}, {y:.1f})') + + def goal_response(self, future): + try: + goal_handle = future.result() + except Exception as error: + self.get_logger().error(f'Failed to send demo goal: {error}') + self.goal_in_progress = False + return + + if not goal_handle.accepted: + self.get_logger().warning('Nav2 rejected the demo goal; retrying') + self.goal_in_progress = False + return + + result = goal_handle.get_result_async() + result.add_done_callback(self.goal_finished) + + def goal_finished(self, future): + try: + status = future.result().status + except Exception as error: + self.get_logger().error(f'Demo goal failed: {error}') + else: + if status == GoalStatus.STATUS_SUCCEEDED: + self.get_logger().info('Demo goal reached') + else: + self.get_logger().warning( + f'Demo goal finished with status {status}; retrying' + ) + self.next_waypoint = advance_waypoint( + self.next_waypoint, + len(self.waypoints), + status, + ) + + self.goal_in_progress = False + + +def main(): + rclpy.init() + node = Nav2GoalPublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py index fba5e2e..608de5e 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py @@ -62,6 +62,7 @@ def __init__(self): self.source_id = f'{self.robot_name}/scan' self.last_publish_stamp_sec = None + self.pending_scan = None self.publish_count = 0 self.transform_failure_count = 0 self.tf_buffer = Buffer() @@ -82,6 +83,7 @@ def __init__(self): self.publish_scan, qos_profile_sensor_data, ) + self.scan_retry_timer = self.create_timer(0.05, self.publish_pending_scan) self.get_logger().info( f'Converting {self.scan_topic} into region updates for ' @@ -90,14 +92,24 @@ def __init__(self): ) def publish_scan(self, scan): + self.pending_scan = scan + self.publish_pending_scan() + + def publish_pending_scan(self): + scan = self.pending_scan + if scan is None: + return + stamp_sec = scan.header.stamp.sec + scan.header.stamp.nanosec / 1e9 if scan.header.stamp.sec == 0 and scan.header.stamp.nanosec == 0: self.get_logger().warning('Ignoring scan with a zero timestamp') + self.pending_scan = None return if self.last_publish_stamp_sec is not None: elapsed = stamp_sec - self.last_publish_stamp_sec if 0.0 <= elapsed < self.publish_period_sec: + self.pending_scan = None return try: @@ -106,27 +118,18 @@ def publish_scan(self, scan): scan.header.frame_id, Time.from_msg(scan.header.stamp), ) - except TransformException: - # Gazebo can deliver a scan before the matching TF sample. - # These demo robots are stationary, so the latest transform is equivalent. - try: - transform = self.tf_buffer.lookup_transform( - self.map_frame, - scan.header.frame_id, - Time(), + except TransformException as error: + self.transform_failure_count += 1 + if self.transform_failure_count == 1 or ( + self.transform_failure_count % 20 == 0 + ): + self.get_logger().warning( + f'Cannot transform {scan.header.frame_id} to ' + f'{self.map_frame} at the scan timestamp: {error}' ) - except TransformException as error: - self.transform_failure_count += 1 - if self.transform_failure_count == 1 or ( - self.transform_failure_count % 20 == 0 - ): - self.get_logger().warning( - f'Cannot transform {scan.header.frame_id} to ' - f'{self.map_frame}: {error}' - ) - return + return - self.transform_failure_count = 0 + self.pending_scan = None points = scan_obstacle_points( scan.ranges, scan.angle_min, diff --git a/map_server/rmf_layered_map_server_demo/setup.py b/map_server/rmf_layered_map_server_demo/setup.py index 8349ab2..7cb3459 100644 --- a/map_server/rmf_layered_map_server_demo/setup.py +++ b/map_server/rmf_layered_map_server_demo/setup.py @@ -42,6 +42,8 @@ 'layered_map_demo_clutter_spawner = ' 'rmf_layered_map_server_demo.demo_clutter_spawner:main', 'layered_map_demo_publisher = rmf_layered_map_server_demo.demo_publisher:main', + 'nav2_goal_publisher = ' + 'rmf_layered_map_server_demo.nav2_goal_publisher:main', 'scan_region_publisher = ' 'rmf_layered_map_server_demo.scan_region_publisher:main', ], diff --git a/map_server/rmf_layered_map_server_demo/test/test_nav2_goal_publisher.py b/map_server/rmf_layered_map_server_demo/test/test_nav2_goal_publisher.py new file mode 100644 index 0000000..e3173e8 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_nav2_goal_publisher.py @@ -0,0 +1,58 @@ +# 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 math import pi + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import PoseStamped +import pytest + +from rmf_layered_map_server_demo.nav2_goal_publisher import advance_waypoint +from rmf_layered_map_server_demo.nav2_goal_publisher import goal_pose +from rmf_layered_map_server_demo.nav2_goal_publisher import parse_waypoints + + +def test_parse_waypoints_groups_xyz_triples(): + assert parse_waypoints([1.0, 2.0, 3.0, 4.0, 5.0, 6.0]) == [ + (1.0, 2.0, 3.0), + (4.0, 5.0, 6.0), + ] + + +@pytest.mark.parametrize('values', [[], [1.0], [1.0, 2.0]]) +def test_parse_waypoints_rejects_incomplete_triples(values): + with pytest.raises(ValueError, match='x, y, yaw'): + parse_waypoints(values) + + +def test_goal_pose_uses_map_frame_and_yaw_orientation(): + stamp = PoseStamped().header.stamp + stamp.sec = 1 + stamp.nanosec = 2 + + pose = goal_pose(3.0, 4.0, pi, stamp) + + assert pose.header.frame_id == 'map' + assert pose.header.stamp == stamp + assert pose.pose.position.x == 3.0 + assert pose.pose.position.y == 4.0 + assert pose.pose.orientation.z == pytest.approx(1.0) + assert pose.pose.orientation.w == pytest.approx(0.0, abs=1e-10) + + +def test_waypoint_advances_only_after_success(): + assert advance_waypoint(0, 2, GoalStatus.STATUS_SUCCEEDED) == 1 + assert advance_waypoint(1, 2, GoalStatus.STATUS_SUCCEEDED) == 0 + assert advance_waypoint(0, 2, GoalStatus.STATUS_ABORTED) == 0 + assert advance_waypoint(1, 2, GoalStatus.STATUS_CANCELED) == 1 diff --git a/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index 1c793c0..0b06c61 100644 --- a/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -133,7 +133,7 @@ def generate_launch_description(): # Define commands for launching the navigation instances bringup_cmd_group = [] - for robot_name in robots_list: + for robot_index, robot_name in enumerate(robots_list): init_pose = robots_list[robot_name] namespace = robot_name + '/inner' group = GroupAction( @@ -174,6 +174,13 @@ def generate_launch_description(): 'headless': 'False', 'use_robot_state_pub': use_robot_state_pub, 'use_navigation': use_navigation, + 'clock_topic': TextSubstitution( + text=( + '/clock' + if robot_index == 0 + else f'/{namespace}/clock' + ) + ), 'x_pose': TextSubstitution(text=str(init_pose['x'])), 'y_pose': TextSubstitution(text=str(init_pose['y'])), 'z_pose': TextSubstitution(text=str(init_pose['z'])), diff --git a/nav2_integration/sp_demo_nav2_bringup/launch/tb3_simulation_launch.py b/nav2_integration/sp_demo_nav2_bringup/launch/tb3_simulation_launch.py index 2983f82..fb32f2f 100644 --- a/nav2_integration/sp_demo_nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_integration/sp_demo_nav2_bringup/launch/tb3_simulation_launch.py @@ -23,6 +23,7 @@ from launch.actions import ( DeclareLaunchArgument, ExecuteProcess, + GroupAction, IncludeLaunchDescription, OpaqueFunction, RegisterEventHandler, @@ -32,7 +33,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import Node +from launch_ros.actions import Node, SetRemap def generate_launch_description(): @@ -53,6 +54,7 @@ def generate_launch_description(): use_composition = LaunchConfiguration('use_composition') use_respawn = LaunchConfiguration('use_respawn') use_navigation = LaunchConfiguration('use_navigation') + clock_topic = LaunchConfiguration('clock_topic') # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration('rviz_config_file') @@ -130,6 +132,12 @@ def generate_launch_description(): description='Whether to enable the Nav2 planning and control stack', ) + declare_clock_topic_cmd = DeclareLaunchArgument( + 'clock_topic', + default_value='/clock', + description='ROS topic for the Gazebo clock bridge', + ) + declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', default_value=os.path.join(bringup_dir, 'rviz', 'nav2_default_view.rviz'), @@ -268,19 +276,22 @@ def generate_launch_description(): launch_arguments={'gz_args': ['-v4 -g ']}.items(), ) - gz_robot = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')), - launch_arguments={'namespace': namespace, - 'use_sim_time': use_sim_time, - 'robot_name': robot_name, - 'robot_sdf': robot_sdf, - 'x_pose': pose['x'], - 'y_pose': pose['y'], - 'z_pose': pose['z'], - 'roll': pose['R'], - 'pitch': pose['P'], - 'yaw': pose['Y']}.items()) + gz_robot = GroupAction([ + SetRemap(src='/clock', dst=clock_topic), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(sim_dir, 'launch', 'spawn_tb3.launch.py')), + launch_arguments={'namespace': namespace, + 'use_sim_time': use_sim_time, + 'robot_name': robot_name, + 'robot_sdf': robot_sdf, + 'x_pose': pose['x'], + 'y_pose': pose['y'], + 'z_pose': pose['z'], + 'roll': pose['R'], + 'pitch': pose['P'], + 'yaw': pose['Y']}.items()) + ]) # Create the launch description and populate ld = LaunchDescription() @@ -305,6 +316,7 @@ def generate_launch_description(): ld.add_action(declare_robot_sdf_cmd) ld.add_action(declare_use_respawn_cmd) ld.add_action(declare_use_navigation_cmd) + ld.add_action(declare_clock_topic_cmd) ld.add_action(world_sdf_xacro) ld.add_action(remove_temp_sdf_file) diff --git a/nav2_integration/sp_demo_nav2_bringup/package.xml b/nav2_integration/sp_demo_nav2_bringup/package.xml index ec55ca5..ec7ac03 100644 --- a/nav2_integration/sp_demo_nav2_bringup/package.xml +++ b/nav2_integration/sp_demo_nav2_bringup/package.xml @@ -29,6 +29,7 @@ ros_gz_sim rviz2 slam_toolbox + spatio_temporal_partition_layer xacro nav2_minimal_tb4_sim nav2_minimal_tb3_sim diff --git a/nav2_integration/sp_demo_nav2_bringup/params/nav2_multirobot_params_all.yaml b/nav2_integration/sp_demo_nav2_bringup/params/nav2_multirobot_params_all.yaml index cbece86..31861b8 100644 --- a/nav2_integration/sp_demo_nav2_bringup/params/nav2_multirobot_params_all.yaml +++ b/nav2_integration/sp_demo_nav2_bringup/params/nav2_multirobot_params_all.yaml @@ -320,7 +320,7 @@ collision_monitor: FootprintApproach: type: "polygon" action_type: "approach" - footprint_topic: "/local_costmap/published_footprint" + footprint_topic: "local_costmap/published_footprint" time_before_collision: 2.0 simulation_time_step: 0.1 min_points: 6 From 24aa7adf119ff3440aa5b336048aaa77bc9c577a Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Wed, 15 Jul 2026 22:47:53 +0200 Subject: [PATCH 20/38] demo: visualize retained region updates Signed-off-by: SamuelFoo --- .../launch/nav2_observations.launch.py | 21 +- .../rmf_layered_map_server_demo/package.xml | 1 + .../region_update_visualizer.py | 273 ++++++++++++++++++ .../scan_region_publisher.py | 9 +- .../rviz/nav2_observations.rviz | 13 + .../rmf_layered_map_server_demo/setup.py | 2 + .../test/test_region_update_visualizer.py | 119 ++++++++ 7 files changed, 433 insertions(+), 5 deletions(-) create mode 100644 map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py create mode 100644 map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py diff --git a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py index efeb3c1..c0b5d69 100644 --- a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py +++ b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py @@ -65,6 +65,7 @@ def generate_launch_description(): beam_stride = LaunchConfiguration('beam_stride') publish_period_sec = LaunchConfiguration('publish_period_sec') ttl_sec = LaunchConfiguration('ttl_sec') + reset_source = LaunchConfiguration('reset_source') max_observation_range = LaunchConfiguration('max_observation_range') declarations = [ @@ -112,9 +113,14 @@ def generate_launch_description(): ), DeclareLaunchArgument( 'ttl_sec', - default_value='1.0', + default_value='5.0', description='Lifetime of each robot observation snapshot.', ), + DeclareLaunchArgument( + 'reset_source', + default_value='False', + description='Whether each snapshot replaces the source history.', + ), DeclareLaunchArgument( 'max_observation_range', default_value='2.5', @@ -146,6 +152,13 @@ def generate_launch_description(): remappings=[('/map/static', '/robot0/inner/map')], ) + region_visualizer = Node( + condition=IfCondition(use_global_rviz), + package='rmf_layered_map_server_demo', + executable='region_update_visualizer', + output='screen', + ) + observation_nodes = [] goal_nodes = [] for robot_name, _, _, _ in ROBOTS: @@ -165,6 +178,9 @@ def generate_launch_description(): publish_period_sec, value_type=float ), 'ttl_sec': ParameterValue(ttl_sec, value_type=float), + 'reset_source': ParameterValue( + reset_source, value_type=bool + ), 'max_observation_range': ParameterValue( max_observation_range, value_type=float ), @@ -211,7 +227,7 @@ def generate_launch_description(): '-d', os.path.join(demo_share, 'rviz', 'nav2_observations.rviz'), ], - parameters=[{'use_sim_time': True}], + parameters=[{'use_sim_time': False}], output='screen', ) global_rviz_exit_handler = RegisterEventHandler( @@ -226,6 +242,7 @@ def generate_launch_description(): *declarations, nav2_simulation, layered_map_server, + region_visualizer, *observation_nodes, *goal_nodes, clutter_spawner, diff --git a/map_server/rmf_layered_map_server_demo/package.xml b/map_server/rmf_layered_map_server_demo/package.xml index 66c5d40..0ec46c5 100644 --- a/map_server/rmf_layered_map_server_demo/package.xml +++ b/map_server/rmf_layered_map_server_demo/package.xml @@ -25,6 +25,7 @@ sensor_msgs sp_demo_nav2_bringup tf2_ros_py + visualization_msgs ament_copyright ament_pep257 diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py new file mode 100644 index 0000000..736636e --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py @@ -0,0 +1,273 @@ +# 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 math import isfinite + +from geometry_msgs.msg import Point +import rclpy +from rclpy.duration import Duration +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy +from rmf_layered_map_msgs.msg import MapRegionPatch, MapRegionUpdate +from rmf_prototype_msgs.msg import Region +from visualization_msgs.msg import Marker, MarkerArray + + +SOURCE_COLORS = ( + (0.96, 0.26, 0.21), + (0.18, 0.80, 0.44), + (0.20, 0.60, 0.98), + (1.00, 0.65, 0.15), + (0.61, 0.35, 0.71), + (0.10, 0.74, 0.80), +) + + +def _point(x, y): + point = Point() + point.x = float(x) + point.y = float(y) + point.z = 0.05 + return point + + +def _marker_lifetime(patch, update, default_ttl_sec): + for ttl_sec in ( + patch.ttl_sec, + update.source.default_ttl_sec, + default_ttl_sec, + ): + if isfinite(ttl_sec) and ttl_sec > 0.0: + return Duration(seconds=ttl_sec).to_msg() + return Duration().to_msg() + + +def _new_marker(update, patch, marker_id, marker_type, color, default_ttl_sec): + marker = Marker() + marker.header.frame_id = update.source.header.frame_id + marker.ns = update.source.source_id + marker.id = marker_id + marker.type = marker_type + marker.action = Marker.ADD + marker.pose = update.source.robot_pose + marker.frame_locked = False + marker.lifetime = _marker_lifetime(patch, update, default_ttl_sec) + marker.color.r = color[0] + marker.color.g = color[1] + marker.color.b = color[2] + marker.color.a = ( + 0.85 if patch.update_type == MapRegionPatch.UPDATE_OBSTACLE else 0.35 + ) + return marker + + +def _markers_from_patch(update, patch, first_id, color, default_ttl_sec): + point_regions = [] + rectangle_lines = [] + + for region in patch.regions: + if region.hint == Region.HINT_POINT and len(region.points) == 2: + point_regions.append(_point(region.points[0], region.points[1])) + elif ( + region.hint == Region.HINT_AXIS_ALIGNED_RECTANGLE + and len(region.points) >= 4 + and len(region.points) % 2 == 0 + ): + xs = region.points[0::2] + ys = region.points[1::2] + min_x, max_x = min(xs), max(xs) + min_y, max_y = min(ys), max(ys) + corners = ( + _point(min_x, min_y), + _point(max_x, min_y), + _point(max_x, max_y), + _point(min_x, max_y), + ) + rectangle_lines.extend(( + corners[0], corners[1], + corners[1], corners[2], + corners[2], corners[3], + corners[3], corners[0], + )) + + markers = [] + marker_id = first_id + if point_regions: + marker = _new_marker( + update, + patch, + marker_id, + Marker.POINTS, + color, + default_ttl_sec, + ) + marker.scale.x = 0.12 + marker.scale.y = 0.12 + marker.points = point_regions + markers.append(marker) + marker_id += 1 + + if rectangle_lines: + marker = _new_marker( + update, + patch, + marker_id, + Marker.LINE_LIST, + color, + default_ttl_sec, + ) + marker.scale.x = 0.08 + marker.points = rectangle_lines + markers.append(marker) + + return markers + + +class RegionMarkerState: + """Convert region updates into persistent per-source RViz marker actions.""" + + def __init__(self, default_ttl_sec=30.0): + self.default_ttl_sec = default_ttl_sec + self.markers_by_source = {} + self.next_ids = {} + self.latest_stamps = {} + self.source_colors = {} + + def apply_update(self, update): + """Return marker actions that apply one region update to the RViz state.""" + stamp_nsec = ( + update.source.header.stamp.sec * 1_000_000_000 + + update.source.header.stamp.nanosec + ) + if stamp_nsec == 0 or not update.source.header.frame_id: + return MarkerArray() + + key = (update.source.source_id, update.source.map_name) + latest_stamp = self.latest_stamps.get(key) + if latest_stamp is not None and stamp_nsec < latest_stamp: + return MarkerArray() + + color = self.source_colors.setdefault( + key, + SOURCE_COLORS[len(self.source_colors) % len(SOURCE_COLORS)], + ) + marker_array = MarkerArray() + self.markers_by_source[key] = [ + marker + for marker in self.markers_by_source.get(key, ()) + if marker[3] is None or marker[3] > stamp_nsec + ] + + if update.reset_source: + for namespace, marker_id, frame_id, _ in self.markers_by_source[key]: + marker = Marker() + marker.header.frame_id = frame_id + marker.ns = namespace + marker.id = marker_id + marker.action = Marker.DELETE + marker_array.markers.append(marker) + self.markers_by_source[key] = [] + self.next_ids[key] = 0 + + next_id = self.next_ids.get(key, 0) + new_markers = [] + for patch in update.patches: + if patch.update_type not in ( + MapRegionPatch.UPDATE_CLEAR, + MapRegionPatch.UPDATE_OBSTACLE, + ): + continue + patch_markers = _markers_from_patch( + update, + patch, + next_id, + color, + self.default_ttl_sec, + ) + new_markers.extend(patch_markers) + next_id += len(patch_markers) + + marker_array.markers.extend(new_markers) + self.next_ids[key] = next_id + self.markers_by_source[key].extend( + ( + marker.ns, + marker.id, + marker.header.frame_id, + stamp_nsec + marker.lifetime.sec * 1_000_000_000 + + marker.lifetime.nanosec + if marker.lifetime.sec > 0 or marker.lifetime.nanosec > 0 + else None, + ) + for marker in new_markers + ) + + if new_markers or update.reset_source: + self.latest_stamps[key] = stamp_nsec + + return marker_array + + +class RegionUpdateVisualizer(Node): + """Visualize active map-region contributions as colored RViz markers.""" + + def __init__(self): + super().__init__('region_update_visualizer') + input_topic = self.declare_parameter( + 'input_topic', '/map/region_updates' + ).value + output_topic = self.declare_parameter( + 'output_topic', '/map/region_markers' + ).value + default_ttl_sec = self.declare_parameter( + 'default_ttl_sec', 30.0 + ).value + + reliable_qos = QoSProfile( + depth=10, + reliability=ReliabilityPolicy.RELIABLE, + ) + self.state = RegionMarkerState(default_ttl_sec) + self.publisher = self.create_publisher( + MarkerArray, + output_topic, + reliable_qos, + ) + self.subscription = self.create_subscription( + MapRegionUpdate, + input_topic, + self.visualize_update, + reliable_qos, + ) + self.get_logger().info( + f'Visualizing {input_topic} as {output_topic}' + ) + + def visualize_update(self, update): + """Publish the marker actions for an incoming region update.""" + marker_array = self.state.apply_update(update) + if marker_array.markers: + self.publisher.publish(marker_array) + + +def main(): + rclpy.init() + node = RegionUpdateVisualizer() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py index 608de5e..3de0ef6 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py @@ -33,7 +33,7 @@ class ScanRegionPublisher(Node): - """Publish one robot's laser endpoints as a replaceable region snapshot.""" + """Publish one robot's laser endpoints as region snapshots.""" def __init__(self): super().__init__('scan_region_publisher') @@ -42,7 +42,10 @@ def __init__(self): self.map_frame = self.declare_parameter('map_frame', 'map').value self.map_name = self.declare_parameter('map_name', 'warehouse').value self.scan_topic = self.declare_parameter('scan_topic', 'scan').value - self.ttl_sec = self.declare_parameter('ttl_sec', 1.0).value + self.ttl_sec = self.declare_parameter('ttl_sec', 5.0).value + self.reset_source = self.declare_parameter( + 'reset_source', False + ).value self.publish_period_sec = self.declare_parameter( 'publish_period_sec', 0.5 ).value @@ -152,7 +155,7 @@ def publish_pending_scan(self): def make_update(self, transform, points): update = MapRegionUpdate() - update.reset_source = True + update.reset_source = self.reset_source update.source = MapObservationSource() # The Rust map server currently uses a system-time clock. # Keep TTL and update ordering in that clock while using the scan stamp for TF. diff --git a/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz index f8c22bc..3babd04 100644 --- a/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz +++ b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz @@ -6,6 +6,7 @@ Panels: Expanded: - /Global Options1 - /Combined Global Map1 + - /Region Contributions1 Splitter Ratio: 0.5 Tree Height: 595 - Class: rviz_common/Views @@ -56,6 +57,18 @@ Visualization Manager: Value: /map_updates Use Timestamp: false Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Region Contributions + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/region_markers + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/map_server/rmf_layered_map_server_demo/setup.py b/map_server/rmf_layered_map_server_demo/setup.py index 7cb3459..39c6bc3 100644 --- a/map_server/rmf_layered_map_server_demo/setup.py +++ b/map_server/rmf_layered_map_server_demo/setup.py @@ -44,6 +44,8 @@ 'layered_map_demo_publisher = rmf_layered_map_server_demo.demo_publisher:main', 'nav2_goal_publisher = ' 'rmf_layered_map_server_demo.nav2_goal_publisher:main', + 'region_update_visualizer = ' + 'rmf_layered_map_server_demo.region_update_visualizer:main', 'scan_region_publisher = ' 'rmf_layered_map_server_demo.scan_region_publisher:main', ], diff --git a/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py b/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py new file mode 100644 index 0000000..703fd0e --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py @@ -0,0 +1,119 @@ +# 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 rmf_layered_map_msgs.msg import MapRegionPatch, MapRegionUpdate +from rmf_layered_map_server_demo.region_update_visualizer import RegionMarkerState +from rmf_prototype_msgs.msg import Region +from visualization_msgs.msg import Marker + + +def _update(stamp_sec=1, reset_source=True): + update = MapRegionUpdate() + update.source.header.stamp.sec = stamp_sec + update.source.header.frame_id = 'map' + update.source.source_id = 'robot0/scan' + update.source.robot_name = 'robot0' + update.source.map_name = 'warehouse' + update.source.robot_pose.position.x = 2.0 + update.source.robot_pose.position.y = 3.0 + update.source.robot_pose.orientation.w = 1.0 + update.source.default_ttl_sec = 4.0 + update.reset_source = reset_source + return update + + +def _region(hint, points): + region = Region() + region.hint = hint + region.points = points + return region + + +def test_visualizes_point_and_rectangle_regions_in_the_source_pose(): + update = _update() + patch = MapRegionPatch() + patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + patch.regions = [ + _region(Region.HINT_POINT, [1.0, 2.0]), + _region(Region.HINT_AXIS_ALIGNED_RECTANGLE, [-1.0, -2.0, 1.0, 2.0]), + ] + update.patches = [patch] + + markers = RegionMarkerState().apply_update(update).markers + + assert len(markers) == 2 + assert markers[0].type == Marker.POINTS + assert markers[0].pose.position.x == 2.0 + assert markers[0].pose.position.y == 3.0 + assert [(point.x, point.y) for point in markers[0].points] == [(1.0, 2.0)] + assert markers[0].lifetime.sec == 4 + assert markers[1].type == Marker.LINE_LIST + assert len(markers[1].points) == 8 + + +def test_reset_deletes_the_previous_source_markers_before_replacing_them(): + state = RegionMarkerState() + first = _update(stamp_sec=1) + first_patch = MapRegionPatch() + first_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + first_patch.regions = [_region(Region.HINT_POINT, [1.0, 2.0])] + first.patches = [first_patch] + state.apply_update(first) + + replacement = _update(stamp_sec=2) + replacement_patch = MapRegionPatch() + replacement_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + replacement_patch.regions = [_region(Region.HINT_POINT, [3.0, 4.0])] + replacement.patches = [replacement_patch] + + markers = state.apply_update(replacement).markers + + assert [marker.action for marker in markers] == [Marker.DELETE, Marker.ADD] + assert markers[0].ns == markers[1].ns == 'robot0/scan' + assert markers[0].id == markers[1].id == 0 + assert [(point.x, point.y) for point in markers[1].points] == [(3.0, 4.0)] + + +def test_older_update_does_not_replace_newer_visualization(): + state = RegionMarkerState() + state.apply_update(_update(stamp_sec=2)) + + markers = state.apply_update(_update(stamp_sec=1)).markers + + assert markers == [] + + +def test_non_reset_updates_retain_only_unexpired_marker_bookkeeping(): + state = RegionMarkerState() + first = _update(stamp_sec=1, reset_source=False) + first_patch = MapRegionPatch() + first_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + first_patch.regions = [_region(Region.HINT_POINT, [1.0, 2.0])] + first.patches = [first_patch] + state.apply_update(first) + + second = _update(stamp_sec=2, reset_source=False) + second_patch = MapRegionPatch() + second_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + second_patch.regions = [_region(Region.HINT_POINT, [3.0, 4.0])] + second.patches = [second_patch] + markers = state.apply_update(second).markers + + assert [marker.action for marker in markers] == [Marker.ADD] + assert markers[0].id == 1 + assert len(state.markers_by_source[('robot0/scan', 'warehouse')]) == 2 + + state.apply_update(_update(stamp_sec=6, reset_source=False)) + + assert state.markers_by_source[('robot0/scan', 'warehouse')] == [] From 055eb9b8061b3e5b917c28118cbc0d4b7828fb46 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Wed, 15 Jul 2026 22:48:32 +0200 Subject: [PATCH 21/38] docs: describe moving Nav2 observation demo Signed-off-by: SamuelFoo --- discourse/3-layered-global-occupancy-map.md | 38 +++++-------------- .../rmf_layered_map_server_demo/README.md | 14 ++++++- 2 files changed, 22 insertions(+), 30 deletions(-) diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md index 60c0d46..9a6a90f 100644 --- a/discourse/3-layered-global-occupancy-map.md +++ b/discourse/3-layered-global-occupancy-map.md @@ -23,8 +23,7 @@ observations, can be added later. * Robot-mounted sources include the observation-frame pose at observation time * The Rust map server composes a static occupancy grid and active observations into `/map` -* The Nav2 demo converts scans from three robots into point regions and displays - the combined map +* The Nav2 demo converts scans from three robots into point regions and displays the source contributions and combined map * The observation messages live in `rmf_layered_map_msgs`, leaving `rmf_prototype_msgs` unchanged @@ -126,32 +125,13 @@ occupied space is not accidentally erased by another active source. # Three-Robot Nav2 Demo -The launch file starts three stationary robots in different free corners of the -warehouse and spawns one deterministic Gazebo box near each robot. Each -observation node subscribes to its robot's local `sensor_msgs/LaserScan`, -filters invalid or out-of-range returns, and publishes the remaining scan -endpoints as point regions. - -Each scan is a replacement snapshot: `reset_source` removes the source's -preceding points before the new points are added, and a short TTL removes the -source if it stops publishing. The observation-frame pose is recorded in the -shared `map` frame so the map server can transform the scan-local regions before -rasterizing them. - -The demo launches Nav2 localization but does not launch Nav2 planning or control -components, RMF planning, or navigation goals. Three robot-local RViz windows -are enabled by default, and another RViz window displays the combined global -`/map`. - -The scan-to-region conversion is deliberately simple so its information loss -and publication cost are visible. `beam_stride` reduces the number of point -regions, `publish_period_sec` throttles replacement snapshots, and -`max_observation_range` limits represented returns. `ttl_sec` controls how -quickly stale snapshots expire. Each publisher logs its number of input beams -and output regions so message density, update rate, and visual fidelity can be -compared. The current demo publishes occupied endpoints only; it does not -convert raycast clearing into free-space regions or compress adjacent points -into larger regions. +The launch file starts three robots in different free corners of the warehouse, cycles them through fixed Nav2 goals, and spawns one deterministic Gazebo box near each robot. Each observation node subscribes to its robot's local `sensor_msgs/LaserScan`, filters invalid or out-of-range returns, and publishes the remaining scan endpoints as point regions. + +Each scan is added to a rolling observation history until its TTL expires. An update may set `reset_source` so the new scan replaces all active observations from that source instead of being added to its history. The observation-frame pose is recorded in the shared `map` frame so the map server can transform the scan-local regions before rasterizing them. + +The demo launches Nav2 localization, planning, and control for the fixed goal loops, but does not launch RMF planning. Robot-local RViz windows show Nav2 state, while another RViz window displays the combined global `/map`. The combined view overlays incoming region updates as colored markers grouped by source, with the same retention behavior as the map contributions. + +The scan-to-region conversion is deliberately simple so its information loss and publication cost are visible. Scan sampling reduces the number of point regions, publication throttling limits the snapshot rate, and the observation range limits represented returns. The TTL controls how quickly stale snapshots expire. Each publisher logs its number of input beams and output regions so message density, update rate, and visual fidelity can be compared. The current demo publishes occupied endpoints only; it does not convert raycast clearing into free-space regions or compress adjacent points into larger regions. # Example Flow @@ -187,3 +167,5 @@ The committed server and demo tests cover: * multiple robot sources being stitched into one composed grid * filtering invalid and out-of-range laser returns * preserving original beam angles when scan points are sampled +* converting point and rectangle updates into source-colored markers +* retaining markers until TTL expiry and replacing them on source reset diff --git a/map_server/rmf_layered_map_server_demo/README.md b/map_server/rmf_layered_map_server_demo/README.md index c95c7fb..ff8b977 100644 --- a/map_server/rmf_layered_map_server_demo/README.md +++ b/map_server/rmf_layered_map_server_demo/README.md @@ -10,12 +10,22 @@ This demo publishes a synthetic static grid and a temporary rectangle with a fiv ros2 launch rmf_layered_map_server_demo demo.launch.py ``` +Use `use_rviz:=False` for a headless run. + ## Three-Robot Nav2 Observation Demo -This demo combines laser observations from three stationary Nav2 robots in the global `/map`: +This demo combines laser observations from three moving Nav2 robots in the global `/map`: ```bash ros2 launch rmf_layered_map_server_demo nav2_observations.launch.py ``` -Use `use_nav2_rviz:=False` to open only the combined-map view. Use `spawn_clutter:=False` to run against the unmodified warehouse world. +By default, the demo opens three robot-local RViz windows and one combined-map view, moves the robots between fixed goals, spawns demo obstacles, and retains scans for five seconds. + +* Set `use_nav2_rviz:=False` or `use_global_rviz:=False` to disable either RViz view. +* Set `move_robots:=False` to disable robot movement. +* Set `spawn_clutter:=False` to use the unmodified warehouse world. +* Set `reset_source:=True` to show only the latest scan from each robot. +* Use `map` and `params_file` to override the warehouse map and shared Nav2 parameters. +* `beam_stride:=1` and `publish_period_sec:=0.5` control scan sampling. +* `max_observation_range:=2.5` and `ttl_sec:=5.0` control range and retention. From 7508d21d615162fc6c0c81ef74f2011bb9c70091 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 16 Jul 2026 01:13:04 +0200 Subject: [PATCH 22/38] feat: support convex polygon map regions Signed-off-by: SamuelFoo --- map_server/rmf_layered_map_server/src/lib.rs | 34 ++++++++++++++++++-- rmf_layered_map_msgs/msg/MapRegionPatch.msg | 4 +-- 2 files changed, 34 insertions(+), 4 deletions(-) diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs index f5529ce..50214d6 100644 --- a/map_server/rmf_layered_map_server/src/lib.rs +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -494,9 +494,14 @@ fn region_validation_error(region: &Region) -> Option { 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, + Region::HINT_CONVEX_POLYGON if region.points.len() < 6 => { + Some("convex polygon must contain at least three x/y pairs".to_string()) + } + Region::HINT_POINT + | Region::HINT_AXIS_ALIGNED_RECTANGLE + | Region::HINT_CONVEX_POLYGON => None, hint => Some(format!( - "unsupported region hint {}; expected a point or axis-aligned rectangle", + "unsupported region hint {}; expected a point, axis-aligned rectangle, or convex polygon", hint )), } @@ -833,6 +838,13 @@ mod tests { } } + fn convex_polygon(points: Vec) -> Region { + Region { + hint: Region::HINT_CONVEX_POLYGON, + points, + } + } + fn patch(update_type: u8, regions: Vec) -> MapRegionPatch { MapRegionPatch { update_type, @@ -896,6 +908,24 @@ mod tests { assert_eq!(composed.data[0], 0); } + #[test] + fn convex_clear_regions_are_composed_over_static_map() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(5, 5, 100)); + + assert!(map.ingest_region_update( + update( + MapRegionPatch::UPDATE_CLEAR, + vec![convex_polygon(vec![0.0, 0.0, 4.0, 0.0, 4.0, 4.0])] + ), + 0, + )); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 5 + 3], 0); + assert_eq!(composed.data[3 * 5 + 1], 100); + } + #[test] fn robot_local_regions_are_transformed_into_the_map_frame() { let mut map = LayeredMap::default(); diff --git a/rmf_layered_map_msgs/msg/MapRegionPatch.msg b/rmf_layered_map_msgs/msg/MapRegionPatch.msg index 14dc00b..d735041 100644 --- a/rmf_layered_map_msgs/msg/MapRegionPatch.msg +++ b/rmf_layered_map_msgs/msg/MapRegionPatch.msg @@ -24,6 +24,6 @@ float64 ttl_sec # 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. +# source.robot_pose. The first prototype supports point, axis-aligned +# rectangle, and convex polygon regions. rmf_prototype_msgs/Region[] regions From daee5fe2d346ff101ba7d55da9eccac568a687e5 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 16 Jul 2026 01:13:14 +0200 Subject: [PATCH 23/38] demo: add laser ray clearing Signed-off-by: SamuelFoo --- .../launch/nav2_observations.launch.py | 2 +- .../region_update_visualizer.py | 25 +++++--- .../scan_conversion.py | 40 +++++++++--- .../scan_region_publisher.py | 61 +++++++++++++------ .../test/test_region_update_visualizer.py | 19 ++++++ .../test/test_scan_conversion.py | 60 ++++++++++++++---- .../test/test_scan_region_publisher.py | 34 +++++++++++ 7 files changed, 194 insertions(+), 47 deletions(-) create mode 100644 map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py diff --git a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py index c0b5d69..e901358 100644 --- a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py +++ b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py @@ -104,7 +104,7 @@ def generate_launch_description(): DeclareLaunchArgument( 'beam_stride', default_value='1', - description='Publish every Nth valid laser return as a point region.', + description='Convert every Nth laser beam into map regions.', ), DeclareLaunchArgument( 'publish_period_sec', diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py index 736636e..95459f2 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py @@ -74,7 +74,7 @@ def _new_marker(update, patch, marker_id, marker_type, color, default_ttl_sec): def _markers_from_patch(update, patch, first_id, color, default_ttl_sec): point_regions = [] - rectangle_lines = [] + region_lines = [] for region in patch.regions: if region.hint == Region.HINT_POINT and len(region.points) == 2: @@ -94,12 +94,19 @@ def _markers_from_patch(update, patch, first_id, color, default_ttl_sec): _point(max_x, max_y), _point(min_x, max_y), ) - rectangle_lines.extend(( - corners[0], corners[1], - corners[1], corners[2], - corners[2], corners[3], - corners[3], corners[0], - )) + for index, corner in enumerate(corners): + region_lines.extend((corner, corners[(index + 1) % len(corners)])) + elif ( + region.hint == Region.HINT_CONVEX_POLYGON + and len(region.points) >= 6 + and len(region.points) % 2 == 0 + ): + corners = tuple( + _point(x, y) + for x, y in zip(region.points[0::2], region.points[1::2]) + ) + for index, corner in enumerate(corners): + region_lines.extend((corner, corners[(index + 1) % len(corners)])) markers = [] marker_id = first_id @@ -118,7 +125,7 @@ def _markers_from_patch(update, patch, first_id, color, default_ttl_sec): markers.append(marker) marker_id += 1 - if rectangle_lines: + if region_lines: marker = _new_marker( update, patch, @@ -128,7 +135,7 @@ def _markers_from_patch(update, patch, first_id, color, default_ttl_sec): default_ttl_sec, ) marker.scale.x = 0.08 - marker.points = rectangle_lines + marker.points = region_lines markers.append(marker) return markers diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_conversion.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_conversion.py index 5cae001..c069235 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_conversion.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_conversion.py @@ -15,7 +15,7 @@ from math import cos, isfinite, sin -def scan_obstacle_points( +def scan_regions( ranges, angle_min, angle_increment, @@ -24,23 +24,49 @@ def scan_obstacle_points( max_observation_range, beam_stride, ): - """Convert valid laser returns into points in the scan frame.""" + """Convert laser beams into clear sectors and occupied endpoints.""" if beam_stride < 1: raise ValueError('beam_stride must be at least one') + if not isfinite(angle_min) or not isfinite(angle_increment): + raise ValueError('scan angles must be finite') + if angle_increment == 0.0: + raise ValueError('angle_increment must not be zero') effective_max = range_max if max_observation_range > 0.0: effective_max = min(effective_max, max_observation_range) - points = [] + clear_polygons = [] + obstacle_points = [] + half_width = abs(angle_increment) * beam_stride / 2.0 for index in range(0, len(ranges), beam_stride): distance = ranges[index] - if not isfinite(distance): + has_obstacle = False + if isfinite(distance): + if distance < range_min or distance > range_max: + continue + clear_distance = min(distance, effective_max) + has_obstacle = distance <= effective_max + elif distance > 0.0 and isfinite(effective_max): + clear_distance = effective_max + else: continue - if distance < range_min or distance > effective_max: + if not isfinite(clear_distance) or clear_distance <= 0.0: continue angle = angle_min + index * angle_increment - points.append((distance * cos(angle), distance * sin(angle))) + clear_polygons.append(( + 0.0, + 0.0, + clear_distance * cos(angle - half_width), + clear_distance * sin(angle - half_width), + clear_distance * cos(angle + half_width), + clear_distance * sin(angle + half_width), + )) + if has_obstacle: + obstacle_points.append(( + distance * cos(angle), + distance * sin(angle), + )) - return points + return clear_polygons, obstacle_points diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py index 3de0ef6..f364605 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py @@ -29,11 +29,41 @@ from sensor_msgs.msg import LaserScan from tf2_ros import Buffer, TransformException, TransformListener -from .scan_conversion import scan_obstacle_points +from .scan_conversion import scan_regions + + +def make_scan_patches(clear_polygons, obstacle_points, ttl_sec): + """Build clear and obstacle patches for one converted laser scan.""" + patches = [] + if clear_polygons: + clear_patch = MapRegionPatch() + clear_patch.update_type = MapRegionPatch.UPDATE_CLEAR + clear_patch.occupancy_value = 0 + clear_patch.ttl_sec = ttl_sec + for polygon in clear_polygons: + region = Region() + region.hint = Region.HINT_CONVEX_POLYGON + region.points = list(polygon) + clear_patch.regions.append(region) + patches.append(clear_patch) + + if obstacle_points: + obstacle_patch = MapRegionPatch() + obstacle_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + obstacle_patch.occupancy_value = 100 + obstacle_patch.ttl_sec = ttl_sec + for x, y in obstacle_points: + region = Region() + region.hint = Region.HINT_POINT + region.points = [x, y] + obstacle_patch.regions.append(region) + patches.append(obstacle_patch) + + return patches class ScanRegionPublisher(Node): - """Publish one robot's laser endpoints as region snapshots.""" + """Publish one robot's laser scan as clear and obstacle regions.""" def __init__(self): super().__init__('scan_region_publisher') @@ -133,7 +163,7 @@ def publish_pending_scan(self): return self.pending_scan = None - points = scan_obstacle_points( + clear_polygons, obstacle_points = scan_regions( scan.ranges, scan.angle_min, scan.angle_increment, @@ -142,18 +172,19 @@ def publish_pending_scan(self): self.max_observation_range, self.beam_stride, ) - update = self.make_update(transform, points) + update = self.make_update(transform, clear_polygons, obstacle_points) self.region_update_publisher.publish(update) self.last_publish_stamp_sec = stamp_sec self.publish_count += 1 if self.publish_count == 1 or self.publish_count % 20 == 0: self.get_logger().info( - f'Published {len(points)} obstacle regions from ' + f'Published {len(clear_polygons)} clear regions and ' + f'{len(obstacle_points)} obstacle regions from ' f'{len(scan.ranges)} laser beams' ) - def make_update(self, transform, points): + def make_update(self, transform, clear_polygons, obstacle_points): update = MapRegionUpdate() update.reset_source = self.reset_source update.source = MapObservationSource() @@ -173,19 +204,11 @@ def make_update(self, transform, points): update.source.robot_pose.position.z = translation.z update.source.robot_pose.orientation = rotation - if not points: - return update - - patch = MapRegionPatch() - patch.update_type = MapRegionPatch.UPDATE_OBSTACLE - patch.occupancy_value = 100 - patch.ttl_sec = self.ttl_sec - for x, y in points: - region = Region() - region.hint = Region.HINT_POINT - region.points = [x, y] - patch.regions.append(region) - update.patches.append(patch) + update.patches = make_scan_patches( + clear_polygons, + obstacle_points, + self.ttl_sec, + ) return update diff --git a/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py b/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py index 703fd0e..a611d6e 100644 --- a/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py +++ b/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py @@ -12,6 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +import pytest + from rmf_layered_map_msgs.msg import MapRegionPatch, MapRegionUpdate from rmf_layered_map_server_demo.region_update_visualizer import RegionMarkerState from rmf_prototype_msgs.msg import Region @@ -62,6 +64,23 @@ def test_visualizes_point_and_rectangle_regions_in_the_source_pose(): assert len(markers[1].points) == 8 +def test_visualizes_clear_ray_sectors_as_polygon_outlines(): + update = _update() + patch = MapRegionPatch() + patch.update_type = MapRegionPatch.UPDATE_CLEAR + patch.regions = [ + _region(Region.HINT_CONVEX_POLYGON, [0.0, 0.0, 1.0, -0.1, 1.0, 0.1]), + ] + update.patches = [patch] + + markers = RegionMarkerState().apply_update(update).markers + + assert len(markers) == 1 + assert markers[0].type == Marker.LINE_LIST + assert markers[0].color.a == pytest.approx(0.35) + assert len(markers[0].points) == 6 + + def test_reset_deletes_the_previous_source_markers_before_replacing_them(): state = RegionMarkerState() first = _update(stamp_sec=1) diff --git a/map_server/rmf_layered_map_server_demo/test/test_scan_conversion.py b/map_server/rmf_layered_map_server_demo/test/test_scan_conversion.py index d848bcd..f2c631a 100644 --- a/map_server/rmf_layered_map_server_demo/test/test_scan_conversion.py +++ b/map_server/rmf_layered_map_server_demo/test/test_scan_conversion.py @@ -12,15 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -from math import inf, nan, pi +from math import cos, inf, nan, pi, sin import pytest -from rmf_layered_map_server_demo.scan_conversion import scan_obstacle_points +from rmf_layered_map_server_demo.scan_conversion import scan_regions def test_scan_filters_invalid_and_out_of_range_returns(): - points = scan_obstacle_points( + clear_polygons, obstacle_points = scan_regions( [nan, inf, 0.05, 1.0, 3.0], angle_min=0.0, angle_increment=pi / 2.0, @@ -30,13 +30,51 @@ def test_scan_filters_invalid_and_out_of_range_returns(): beam_stride=1, ) - assert len(points) == 1 - assert points[0][0] == pytest.approx(0.0, abs=1e-6) - assert points[0][1] == pytest.approx(-1.0) + assert len(clear_polygons) == 3 + assert len(obstacle_points) == 1 + assert obstacle_points[0][0] == pytest.approx(0.0, abs=1e-6) + assert obstacle_points[0][1] == pytest.approx(-1.0) + + +def test_scan_creates_a_clear_sector_around_each_sampled_beam(): + clear_polygons, obstacle_points = scan_regions( + [1.0], + angle_min=0.0, + angle_increment=0.2, + range_min=0.1, + range_max=5.0, + max_observation_range=2.5, + beam_stride=1, + ) + + assert obstacle_points == [(1.0, 0.0)] + assert clear_polygons[0] == pytest.approx(( + 0.0, + 0.0, + cos(0.1), + -sin(0.1), + cos(0.1), + sin(0.1), + )) + + +def test_scan_without_a_return_clears_to_the_observation_limit(): + clear_polygons, obstacle_points = scan_regions( + [inf], + angle_min=0.0, + angle_increment=0.2, + range_min=0.1, + range_max=10.0, + max_observation_range=2.5, + beam_stride=1, + ) + + assert obstacle_points == [] + assert max(clear_polygons[0]) == pytest.approx(2.5 * cos(0.1)) def test_scan_stride_preserves_original_beam_angles(): - points = scan_obstacle_points( + _, obstacle_points = scan_regions( [1.0, 1.0, 1.0, 1.0], angle_min=0.0, angle_increment=pi / 2.0, @@ -46,14 +84,14 @@ def test_scan_stride_preserves_original_beam_angles(): beam_stride=2, ) - assert len(points) == 2 - assert points[0] == pytest.approx((1.0, 0.0), abs=1e-6) - assert points[1] == pytest.approx((-1.0, 0.0), abs=1e-6) + assert len(obstacle_points) == 2 + assert obstacle_points[0] == pytest.approx((1.0, 0.0), abs=1e-6) + assert obstacle_points[1] == pytest.approx((-1.0, 0.0), abs=1e-6) def test_scan_rejects_invalid_stride(): with pytest.raises(ValueError, match='beam_stride'): - scan_obstacle_points( + scan_regions( [], angle_min=0.0, angle_increment=0.1, diff --git a/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py new file mode 100644 index 0000000..b6394d0 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py @@ -0,0 +1,34 @@ +# 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 rmf_layered_map_msgs.msg import MapRegionPatch +from rmf_layered_map_server_demo.scan_region_publisher import make_scan_patches +from rmf_prototype_msgs.msg import Region + + +def test_scan_patches_clear_rays_before_marking_obstacle_endpoints(): + patches = make_scan_patches( + [(0.0, 0.0, 1.0, -0.1, 1.0, 0.1)], + [(1.0, 0.0)], + ttl_sec=5.0, + ) + + assert [patch.update_type for patch in patches] == [ + MapRegionPatch.UPDATE_CLEAR, + MapRegionPatch.UPDATE_OBSTACLE, + ] + assert patches[0].occupancy_value == 0 + assert patches[0].regions[0].hint == Region.HINT_CONVEX_POLYGON + assert patches[1].occupancy_value == 100 + assert patches[1].regions[0].hint == Region.HINT_POINT From b6d70d63bdfa7ea1cde52b0be333772bf15e75fc Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 16 Jul 2026 01:13:21 +0200 Subject: [PATCH 24/38] docs: describe laser ray clearing Signed-off-by: SamuelFoo --- discourse/3-layered-global-occupancy-map.md | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md index 9a6a90f..3dbec68 100644 --- a/discourse/3-layered-global-occupancy-map.md +++ b/discourse/3-layered-global-occupancy-map.md @@ -23,7 +23,7 @@ observations, can be added later. * Robot-mounted sources include the observation-frame pose at observation time * The Rust map server composes a static occupancy grid and active observations into `/map` -* The Nav2 demo converts scans from three robots into point regions and displays the source contributions and combined map +* The Nav2 demo converts scans from three robots into clear ray sectors and occupied endpoint regions, then displays the source contributions and map * The observation messages live in `rmf_layered_map_msgs`, leaving `rmf_prototype_msgs` unchanged @@ -125,13 +125,13 @@ occupied space is not accidentally erased by another active source. # Three-Robot Nav2 Demo -The launch file starts three robots in different free corners of the warehouse, cycles them through fixed Nav2 goals, and spawns one deterministic Gazebo box near each robot. Each observation node subscribes to its robot's local `sensor_msgs/LaserScan`, filters invalid or out-of-range returns, and publishes the remaining scan endpoints as point regions. +The launch file starts three robots in different free corners of the warehouse, cycles them through fixed Nav2 goals, and spawns one deterministic Gazebo box near each robot. Each observation node subscribes to its robot's local `sensor_msgs/LaserScan`, filters invalid or out-of-range returns, and publishes free-space ray sectors as convex polygons and occupied endpoints as point regions. -Each scan is added to a rolling observation history until its TTL expires. An update may set `reset_source` so the new scan replaces all active observations from that source instead of being added to its history. The observation-frame pose is recorded in the shared `map` frame so the map server can transform the scan-local regions before rasterizing them. +Each scan adds temporary clear and obstacle patches to a rolling observation history. The map server rasterizes clear sectors before obstacle endpoints so a measured hit remains occupied. Active obstacle evidence still wins over clear evidence until its TTL expires. An update may set `reset_source` so the new scan replaces all active observations from that source instead of being added to its history. The observation-frame pose is recorded in the shared `map` frame so the map server can transform the scan-local regions before rasterizing them. The demo launches Nav2 localization, planning, and control for the fixed goal loops, but does not launch RMF planning. Robot-local RViz windows show Nav2 state, while another RViz window displays the combined global `/map`. The combined view overlays incoming region updates as colored markers grouped by source, with the same retention behavior as the map contributions. -The scan-to-region conversion is deliberately simple so its information loss and publication cost are visible. Scan sampling reduces the number of point regions, publication throttling limits the snapshot rate, and the observation range limits represented returns. The TTL controls how quickly stale snapshots expire. Each publisher logs its number of input beams and output regions so message density, update rate, and visual fidelity can be compared. The current demo publishes occupied endpoints only; it does not convert raycast clearing into free-space regions or compress adjacent points into larger regions. +The scan-to-region conversion uses one convex sector per sampled beam instead of expanding a Bresenham line into many point regions. Scan sampling reduces the number of sectors, publication throttling limits the snapshot rate, and the observation range limits represented returns. The TTL controls how quickly stale observations expire. Each publisher logs its input beams and clear and obstacle regions so message density, update rate, and visual fidelity can be compared. The current demo remains a 2D occupancy approximation; it does not fuse probabilistic confidence or compress adjacent sectors into larger regions. # Example Flow @@ -165,6 +165,7 @@ The committed server and demo tests cover: * late older snapshots being ignored * reset updates removing observations from the same source and map * multiple robot sources being stitched into one composed grid +* converting laser beams into clear sectors and occupied endpoints * filtering invalid and out-of-range laser returns * preserving original beam angles when scan points are sampled * converting point and rectangle updates into source-colored markers From 41fdf7f6f269c9a581b52eb2c81d9971b476e43c Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 16 Jul 2026 02:13:06 +0200 Subject: [PATCH 25/38] demo: distinguish clear and occupied markers Signed-off-by: SamuelFoo --- .../region_update_visualizer.py | 34 +++++++++++++----- .../test/test_region_update_visualizer.py | 36 ++++++++++++++++++- 2 files changed, 61 insertions(+), 9 deletions(-) diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py index 95459f2..bae3500 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py @@ -32,13 +32,16 @@ (0.61, 0.35, 0.71), (0.10, 0.74, 0.80), ) +CLEAR_COLOR_BLEND = 0.5 +CLEAR_MARKER_Z = 0.04 +OBSTACLE_MARKER_Z = 0.08 -def _point(x, y): +def _point(x, y, z): point = Point() point.x = float(x) point.y = float(y) - point.z = 0.05 + point.z = z return point @@ -53,7 +56,17 @@ def _marker_lifetime(patch, update, default_ttl_sec): return Duration().to_msg() +def _patch_color(color, update_type): + if update_type == MapRegionPatch.UPDATE_OBSTACLE: + return color + return tuple( + channel + (1.0 - channel) * CLEAR_COLOR_BLEND + for channel in color + ) + + def _new_marker(update, patch, marker_id, marker_type, color, default_ttl_sec): + color = _patch_color(color, patch.update_type) marker = Marker() marker.header.frame_id = update.source.header.frame_id marker.ns = update.source.source_id @@ -75,10 +88,15 @@ def _new_marker(update, patch, marker_id, marker_type, color, default_ttl_sec): def _markers_from_patch(update, patch, first_id, color, default_ttl_sec): point_regions = [] region_lines = [] + z = ( + OBSTACLE_MARKER_Z + if patch.update_type == MapRegionPatch.UPDATE_OBSTACLE + else CLEAR_MARKER_Z + ) for region in patch.regions: if region.hint == Region.HINT_POINT and len(region.points) == 2: - point_regions.append(_point(region.points[0], region.points[1])) + point_regions.append(_point(region.points[0], region.points[1], z)) elif ( region.hint == Region.HINT_AXIS_ALIGNED_RECTANGLE and len(region.points) >= 4 @@ -89,10 +107,10 @@ def _markers_from_patch(update, patch, first_id, color, default_ttl_sec): min_x, max_x = min(xs), max(xs) min_y, max_y = min(ys), max(ys) corners = ( - _point(min_x, min_y), - _point(max_x, min_y), - _point(max_x, max_y), - _point(min_x, max_y), + _point(min_x, min_y, z), + _point(max_x, min_y, z), + _point(max_x, max_y, z), + _point(min_x, max_y, z), ) for index, corner in enumerate(corners): region_lines.extend((corner, corners[(index + 1) % len(corners)])) @@ -102,7 +120,7 @@ def _markers_from_patch(update, patch, first_id, color, default_ttl_sec): and len(region.points) % 2 == 0 ): corners = tuple( - _point(x, y) + _point(x, y, z) for x, y in zip(region.points[0::2], region.points[1::2]) ) for index, corner in enumerate(corners): diff --git a/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py b/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py index a611d6e..fe8df1f 100644 --- a/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py +++ b/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py @@ -15,7 +15,10 @@ import pytest from rmf_layered_map_msgs.msg import MapRegionPatch, MapRegionUpdate -from rmf_layered_map_server_demo.region_update_visualizer import RegionMarkerState +from rmf_layered_map_server_demo.region_update_visualizer import ( + RegionMarkerState, + SOURCE_COLORS, +) from rmf_prototype_msgs.msg import Region from visualization_msgs.msg import Marker @@ -81,6 +84,37 @@ def test_visualizes_clear_ray_sectors_as_polygon_outlines(): assert len(markers[0].points) == 6 +def test_visualizes_clear_regions_with_a_lighter_source_color(): + update = _update() + clear_patch = MapRegionPatch() + clear_patch.update_type = MapRegionPatch.UPDATE_CLEAR + clear_patch.regions = [_region(Region.HINT_POINT, [1.0, 2.0])] + obstacle_patch = MapRegionPatch() + obstacle_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + obstacle_patch.regions = [_region(Region.HINT_POINT, [1.0, 2.0])] + update.patches = [clear_patch, obstacle_patch] + + clear_marker, obstacle_marker = RegionMarkerState().apply_update( + update + ).markers + clear_color = ( + clear_marker.color.r, + clear_marker.color.g, + clear_marker.color.b, + ) + obstacle_color = ( + obstacle_marker.color.r, + obstacle_marker.color.g, + obstacle_marker.color.b, + ) + + assert obstacle_color == pytest.approx(SOURCE_COLORS[0]) + assert clear_color == pytest.approx( + tuple((channel + 1.0) / 2.0 for channel in SOURCE_COLORS[0]) + ) + assert clear_marker.points[0].z < obstacle_marker.points[0].z + + def test_reset_deletes_the_previous_source_markers_before_replacing_them(): state = RegionMarkerState() first = _update(stamp_sec=1) From cc51fce80d2d60f842931109ede9fdf2a3fe7a3a Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 16 Jul 2026 02:13:15 +0200 Subject: [PATCH 26/38] demo: default observation TTL to ten seconds Signed-off-by: SamuelFoo --- map_server/rmf_layered_map_server_demo/README.md | 4 ++-- .../launch/nav2_observations.launch.py | 2 +- .../rmf_layered_map_server_demo/scan_region_publisher.py | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/map_server/rmf_layered_map_server_demo/README.md b/map_server/rmf_layered_map_server_demo/README.md index ff8b977..f9d53de 100644 --- a/map_server/rmf_layered_map_server_demo/README.md +++ b/map_server/rmf_layered_map_server_demo/README.md @@ -20,7 +20,7 @@ This demo combines laser observations from three moving Nav2 robots in the globa ros2 launch rmf_layered_map_server_demo nav2_observations.launch.py ``` -By default, the demo opens three robot-local RViz windows and one combined-map view, moves the robots between fixed goals, spawns demo obstacles, and retains scans for five seconds. +By default, the demo opens three robot-local RViz windows and one combined-map view, moves the robots between fixed goals, spawns demo obstacles, and retains scans for ten seconds. * Set `use_nav2_rviz:=False` or `use_global_rviz:=False` to disable either RViz view. * Set `move_robots:=False` to disable robot movement. @@ -28,4 +28,4 @@ By default, the demo opens three robot-local RViz windows and one combined-map v * Set `reset_source:=True` to show only the latest scan from each robot. * Use `map` and `params_file` to override the warehouse map and shared Nav2 parameters. * `beam_stride:=1` and `publish_period_sec:=0.5` control scan sampling. -* `max_observation_range:=2.5` and `ttl_sec:=5.0` control range and retention. +* `max_observation_range:=2.5` and `ttl_sec:=10.0` control range and retention. diff --git a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py index e901358..5f18d63 100644 --- a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py +++ b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py @@ -113,7 +113,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( 'ttl_sec', - default_value='5.0', + default_value='10.0', description='Lifetime of each robot observation snapshot.', ), DeclareLaunchArgument( diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py index f364605..96a0571 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py @@ -72,7 +72,7 @@ def __init__(self): self.map_frame = self.declare_parameter('map_frame', 'map').value self.map_name = self.declare_parameter('map_name', 'warehouse').value self.scan_topic = self.declare_parameter('scan_topic', 'scan').value - self.ttl_sec = self.declare_parameter('ttl_sec', 5.0).value + self.ttl_sec = self.declare_parameter('ttl_sec', 10.0).value self.reset_source = self.declare_parameter( 'reset_source', False ).value From 716d553b2f6e19bcf08923df737b8798554147e1 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Thu, 16 Jul 2026 02:17:56 +0200 Subject: [PATCH 27/38] docs: add Nav2 observation screenshot Signed-off-by: SamuelFoo --- .../rmf_layered_map_server_demo/README.md | 2 ++ .../docs/images/nav2_observations.png | Bin 0 -> 87696 bytes 2 files changed, 2 insertions(+) create mode 100644 map_server/rmf_layered_map_server_demo/docs/images/nav2_observations.png diff --git a/map_server/rmf_layered_map_server_demo/README.md b/map_server/rmf_layered_map_server_demo/README.md index f9d53de..136dac7 100644 --- a/map_server/rmf_layered_map_server_demo/README.md +++ b/map_server/rmf_layered_map_server_demo/README.md @@ -20,6 +20,8 @@ This demo combines laser observations from three moving Nav2 robots in the globa ros2 launch rmf_layered_map_server_demo nav2_observations.launch.py ``` +![Three robot laser observations in the combined global map](docs/images/nav2_observations.png) + By default, the demo opens three robot-local RViz windows and one combined-map view, moves the robots between fixed goals, spawns demo obstacles, and retains scans for ten seconds. * Set `use_nav2_rviz:=False` or `use_global_rviz:=False` to disable either RViz view. diff --git a/map_server/rmf_layered_map_server_demo/docs/images/nav2_observations.png b/map_server/rmf_layered_map_server_demo/docs/images/nav2_observations.png new file mode 100644 index 0000000000000000000000000000000000000000..0d5aad7abe341d7da1f40d78b3411993e4765d83 GIT binary patch literal 87696 zcmbrm1yq#Z+ci9N3(^A87_$s9gM>6lN|!LC0uoXpJ+uf6T_W95Qqo9B#}Fbg z2nc*<(BD74_j%Xzt@W|=b{J>Qoco+}U1#sJ_cdW^D)J)m^XHXFaTi_oIR}Fb-NXY=h2KWWrN=jJ@0x6Fs`fZ8>ekX8J&~t@A$lcI?G3G3p zUO*rhxsPO|v|bo(o%#FFule3Q@k?*zdkK+>%~sH7+)SiuYmvc_sjc>E; z^){O``_s#Fu+irZDk?CeQsnd9YKmr$UE6wC+GL)XbB$H~>}YDv?AFJlQI*a6&1s($ zDQ-Ide)!S+>Dy=O%@7UjM;Pov%mEmd>8D*Ll1?%j?05Fb82si+Clr=Nv3wLTU-17L zxHlNu{+d(?BIL5n2p2!YB@D*={r>LFxJPVpkLpuJb2-MefY5b#rbhYZK`OzVy@Hr&~{+oUj43mJXWV~0Rot9r=vALZU&gv{c;sb`5uEbRzy%KjsieJrT6W z+F)h16}jxh-*(i${l~}O55DZA48~{l=M>+H-}ET`=Z!zF|9|V4I^?}ZfvgfYb-hCx zlM?s1^Q^E$f^N~nM+tRsU5M*%|AB(uN4a%WYi@kQt1H=@T^q25o~uW*o*CEyBD+2h zJ5ScBILGy{~1(M zQ!^}pr*3nv-+$8~bCn=?_r#)~b7!R{iw5*mv8;XeNHCp?0WDhzbCx-%2WIVB2 z{qVH|kwH{ARv>7L)2P1SOIW8F?RBwThoA3ekKSX(=a~HnF^2B=25KSb+1O6^db#-c z^mhfvpa|T293Bvjr*lX7z3T2Jnq!he+E5hiVb_O2XOweDNXQ)lfj4Uz7s)URRrC_j z02ATy44S9fynsB_M0Nhuz5dbBKJ@%quYUr+D8%j~-^J;cH!i40=w_Xsr;F zkg&xo;&9#!Mc;a<@Bo;Yz%EB@F7+{VU`~Nc;7c%GDnln4;3iMemy=JKf1`5?L?^Qe z2zmx8^kocv(e^bMa?$#CfTNpG9|ji!eROOy9_8w1sY!xAxf(Wav{0^lcVGWIU|)vO zJZJwWNLc?PMAad=3AMEMK)74BE(*rV&*+rg{O5Jqhvxr3f<7!X)LJ%Jy0tO zja!s|8bmj0o%X-J0sXSjP|SP~^B~tuYyFRJDAwnUcezYO(|0ur-dTb4@(`U`yh~}S zq2d zsujG$ipPTFy{<2xycxgwxx4$EqzB|I8v7-XAcM2AOy5MM z%jar1KY(q5JqI%pmzk+ta%F3>91AS<4Uo$; zptqR_o6B|O8>9D+8V7oMEWU!=wqrS%-z(+o&u$G3ZYz4(B(FR$G{nytAO4mB(sU$- zaDkJC)5k9zHo?zYt553~N((?bGMg;VG1fz8kkZo#go~O#2@VfPN3vOJP7l0n!DoYf zF1Z5van>lMt-={E%{_iKAI^*dTNy|cS}QF}Ou^W1WKMc75RptSC;`OcQ1}g5k6=i%ktG;&I^EEh!|CKcH8#yNB>s{*y&O zx(~5Bs)e)3@9c3~J(M#lTt|0%3tTeiq+om4PY1Fd)?fteir9lRtB|YlXO4gGOSqyp zfT)oy9GAAd3jAvyzbqBD>bI`90N1*tKx%eUakElL1X)C#oda^`*;aK@tufqM)8zkOM!URckcPI=eWU07kM3R_rtbRBEQH2AAO-%{ z8~z1${=qs-l!TQ#+Gs390Ad>bs7k71q5U-ngaPV8s*>S{g-)Q6c~K0tc*&3Hk{?gc zF5lLMzP>UWBArE+4|XHp6J-$4AQ!|c=8nU2kae)JaMU)v6lZ)}VQJGM2;?STHd;Em z&(Z$B*)Bu>y~b?@ai7ErxQ?;dirst;Cmv*{e%Cj!uhz-5YNg4_dfv!_*TAjR!D)@V zy0NmP)*d_q`LqMGGTc9clsOI>8N4{PXQnf%nzK7aP1e%z@?D6OT0HglUqY_=|Kg7N z{rNQUcavz^Ljk&135o*Kv){N6&lD4;KbGdv46D&Djl@Na!|Z>yaWI_68|&S*D#);F zJn{A(o*0R!q!|Da2*7VyWvPS{l!n*%rRn%ge>#A`5Iod2fFW+;uQz-KW|s?mVW@1i zBty@EkAc^Bw3Vi78<|3E%)D1q>U-{~o^{>)!PWe;5E4LR^a}^_eYq#@*U?4u#kjSj z$Hm3LhU|Q4=fJkdytv=?iTHq0j{=@XfmWsq=!LV5%M55epZfDa(|OCv8&%b|vfoIM zg&-~cXCm<{>vFldO?h6%4$xO&4(oE)ysN<&YdOkehUgK2&40O9_HvNf+3v_ZH06Ny zSmWIYKfh9<7EeJEJp3(oatN-IAr_Y0bNgu1{V%$hM^+$QzB{Ku%Q}%qjTcx7J9JuK zoe`TLFEE>+(E}A4IK>NF^Fg7tF2fU~2Ox*e6Oiafg{Rq~Gawquqazo)90dH5CwfZ5 zFT)xAKcf4~nx9VDF36%K4$A7M?j6mY8bMlI2hTddZDQeF$u2D=9TYpFP`J$!q&s@v zk`w)X?=_>*=&H~5+^ANPv8M z>W%sUKoxoige9~u9um&*MdiA6E%Nge=EBF+lCdC_@oxb}-gCkpRufW#_9%3{y`GY& zwNf-H1?%ZaF8IJ{OLHD222fj#a;TIJVs15fTNjrx1+ zRvetJ#3M2VtBy>GZ$6vo-&KP`GU=&&79TQsJSbhCEH@W&UhH@~&m`heWTRA{PYkgd zOxFX^;p;0hG%-Pw51*|+`aW8aIHTMr3|^Vte1Rw72cpF)-N$zLC@SUy%eC@TMv3Ft zN-YT*a=OJ>e8n-&#~a4zi9DP@AQ;+Z;G1;q@# zTL!CbJ)~KXLmR3xUl|LfpyA6hIIz4Bp^iXr?vIs~vo&hP`?$c$$#4e1V{dBkyde!ZjC==o?c1cXed%QyFyb|qeIY!pyMyxjT5b65@gW{G!&q2u5{;6q*` zl*1E>yv_N5rYP~P0(bfV0i-jjs5ac{ zR^U1E(TgDPcg9GVPKI7PO&IQ?e6jtktZweqA!mhPDB=%!d9&!;^6F%Y2uxY)sW?hq z^pFasm~kpmd0ya?kYlSBjDTBIVST6)?bow~MEXLiS9{JE6}3n{X~xIB=^*LF+3RaM z%?Fd&_6?w0p61@(ggPY#9uIgnG&B62Iy1~`QG;iN{=v-`t2Gk>izYnq8T9Ws*hOGw zmbCF2zv8JhtF$`1s$6KsFCA_8gL8pjZgLPT!fYFbIX_Wt@kA2=n#z{RDCYX5#*gs|2cAG7Ibr z;wQt=IJ|3HQ}^sR+e*3`kD6#kMn+OOtqRWD%M+SLH=e$KxATFX?s_8x!b2nPXko@0 z0eD(I{J96Y=ggi8m>svq(BWYsKyI&1y|(yy^6Ivco_!STB;^oa-`G2c;YdKdPtUe~ z)i+Dx*OG+8)zpY@x^q_6^pvP`A&(XfX(bR)!)#si1bM*SBEy8EPHrr=3@ zgnguXlIf7RIUEX4)+5bEzo*%fvDX;t(3L60=P=#O=_ zUq4?N>;Bxc=8bvK%*U<$+ixRv9tSdW7VH1X`b^`S@VKU@%kBlQyBKCou3z+OhFw8v zA}444Zab0E$}iL0?25!Ey{3EK&x}@|hb_8ui6lR`E9{LCCSx|{_#Bywj>@RsG(#}+ z9tiCfr#$bohRFgCkATF*DxSoVhW(iaiizh1rw(%;yl&c5i;VRr_svcyA`^PSpg*bg zL?rg!hrbwjAJrB@^ro^PPynJj1C@|D~cDdhwjU1q4D_3Nq`#l z5W9PC5ezRztx7|B;nkD|<4P;%r8LlNqc6JPTnL$S_q8u-x?mG*KFk5=G*No;m0mp= z3&(du16p|OQ}$$MvD#RRWbVhkuH)wiV8=1g(K6b|l;)a&_5*yLoCr;U;anY-+cUQ^ zxoy{@*Z0v#Zv$&j(caqDaZW|C)><#Te)jm}*WQwl*Ux45qXi@Qw<0%VuHvU)Wp3*m zs=$THZ&}s8SsAZST-{E&z`dygxBl|F?yJYnMQY8{O%J1=OX8Q<>?OiWVzzJvWC?T% zcSklFsicdBEp3GnLYN`J6k6%{li?Tq)W(rxFx#axm>7I)024Nb?Z zsevaER%~)A{fCJIj!PQIXXLv4?}N4B zRf=E#i8t-V|C?SD`0SmyM*9x`L%#vdeH4bxcT07Y7t{DZkL&ualqgGc-|c9RhFF+- zY~&i~|7tdHnSb*SbPdI{1}`WO6Y8|)ijyt8Y?@b;}F z$`do-2D<5I;o*S8J9Hqy@S=Q;z?lwGi); ze()E;d)a>EzVkF8X^4Y&m18c&NY^N>jIM9v_LT0x8vAELBBhe5vQaV*d9v8Pgj+^L zEHj1ORs+^H$lp+1tGR&Vh>TXBW7#CW-wfjgG-R+-p67iAF9lET7Y)0{B=SmHmP)Ir zO!if8$P)d8!jjXmOO(HTd#u_jd_-L|Vf^a&E8o`QQM6yc^igy0^n>QW>6!~%u{^)P zrU%Bd+j$Zp$G=rg8_%e})Tp$3pYn9~Y%LDYa(jR?(yPNaJ`ak?$js%lywOW;*|=SQ z>nyI1I=#_kVeML2MlOdDL)@=tPZr||U{FC8=N-Bq`qVGnF<*EYEC$~ROYGuEADrwP z{7RlzqS$bsW$)yDN9VQgy*>;Sa+qtbL^#_TV`n2|_V!P+Untvlw{aJ7C7PspVU63B zPIhiS@F&>sH*WJr3Rs+wFTcQbxj{-xW&MGGmg;D>DE%2PcQu__;q(PPt{NwX>~%A5 z^6U4Y-9&4qX<)j7Gg`fmagzAXFsqsTNUL{$^VrUxeK6cQADGWRneY$ZoELo-y>q_c zsV;JiHR|U=z8WF%`grqpl4Aqze&oc~{Ql-Av0jmkhYA@F^H(oK9^XxNLmzx(D@#te zA8~?58kt(v@{f(V9$65QI~rFK4e~`kePQ~gEt>D?ulfk;a}*EkxNm|-RAe~Pc;Il6 z#vF_RrsG-w(LKpJh{b9^7%M-N^vg9=Hhs%;9RA0dwir|t`FfI5*ouP8#<+wbzNuU| z-_00>#BSdars=fyqMw6T5KWg_aSS}>doyl%CpEOw&Nh}i(ZsM8dw*1k-`I;_eEpk8 z`N_<+?gI`J58R{-&-uqB_nS@MSq8)Bv#^G4bFc84n{jGY9s4KFIG& zezfNErsC%gL{R4I1>D|##$(}&0a3r0MwI5?r~1V8K~mc`={82%Qr)C9N1$dhW0@QInQ{RHkr=9`tuBY_PN09<)MfUn#shhl17E{;Qn|xIkMb
sj~opDZ3wI_=Oo zd5!bRsU~iz4_8vb4)%TQO)aW!(}ef>*)+kFc+VLj+>^3D5d>PcD6^!NNx0#VpE%o| z#{gsetU>pUY0i%_dnvBXUhUVO*{AtYtZq!0HXSQ<_t9(*_ippod9x`H=u!vSNGlNK z)5U1&b#tL8Z?E$CSw?>q-^cwj;#d}QsW23N9=sz%DUizfkFSn0lRZW&t z&3!&&=Y|3p2B|d>Q+Xx4lohFKJvRs|c+?v{jCZ^AH2qk#D{b-Kch}AHC-7Q%D>fEV z`S5yG?09huIlmpsja?fnhI+Y^D28GQy@*!nn}ghUwxn57H6zF_hg}^Kk7&_t<$aQo z{`Q6QjDAubo1N+`7#3a?Rmwm+4%ihNm?BAi%+o?0k5@+SF^8-li~QNj)H+H=x9n{b z47;12U>ea@=27*;Z((6_n`x>6sin{Mi`Cg8pGng>XHm_dw8}FY!m$AXx+>Azfr_2H zmWiu@iq~8?iI87GVh|@q&P>Vv?G9AvjHSBX;_iZx?C33B&?O-;p1sfS#b^$R%*h?s zwpiU;A%s*g3+H`&3w?u*kDId3S+-bFK4XkwVmjqD*HfPwbgaH(rIg_|8gZ8%Lx|-a zZagq;JQiMFAwrEZ_siPpi}Y3SQR|IvN66b^j;}=U8~ZT2+^x6Pzxrnf%4>Q+nyNztSivWoALBVSWHx z`E4iNh7YofCk*7~iQ5tU>3+VZ@2knU^;w)(C0DfuB)Zu8F93w9t#K083wJkj+08`x4iQE1nbuoi(SJfA!GzRh8^M}_R&tB za%LjPu>1F@O9`Al;6wedgq8QL!+Xag#>b_HQ-bXr^$tC0!N_>z5(f!n3OEi%_W$`b za6ZLir5{rc4U4$fLLWzN9*^C7cRDGj&X(s5g9l;BG6}(H3nT?JzyA_TQUQ|uQjGj` zRGVncmE5)ZM;q>FMW&$|G+xoG3Fu4UAZeo5c#!o9lQ=iV_wHDj)5!t-ElR?j`Bu$8 zdPU?PwaUPBQtmmp9bl4&VdLWA%_yf|qI%kEQ)>>k$?z|2z3)w* z@wTTVV=!kOi+bG30LeNQF1CYiIT$=tn{C9bzmBia0=H7`EgE=-k1PdT;#W z?=Ek9IVm+}VK>ib1+YL;YT7k@BD1w3=q&kT zE@@}scS@Tx#_$bFp8JoYa0!SoG0ExJr4J?E1;`{O;q|)W)0CPWMU7|RaSgj)uQNJ0 z6YJ(DUUu<%hQ7~C}Auk6$V} zcY%@xWR)YBK3d~ALYJ!h1Fyvq@>Lv1C9@9q`hGSxsBz zzDSt0z*h~G0^Bd!?fI-h1&WlKEBrx2ThJnw&6{=;c+JORtnpB#ubWGx)hDy~7(vWe zNv7uM1u6EXiLdNz%5iEubZn?@Sh1+oLys%gg4W6>yr}J2*u5RE`BVOT&|2%$L&e;> zJ9jIXa&p$xfqI=P=DwD;JLd*nY-c(T#SQ>^Upvt6y!LAcL6Xh^Y?t9H#Ee3a^5O~ir@PM&=mRHGTz{W4 zA5Ghpf@3t}KBg*1nP&qnlFhsR58|!L!RfuKRiA)n{JlG|vgNv3Z{zw^7NbPwfU5Hj z!C=T|=5`?c{g1Trho1p1OauKxS3ztvUPmouaLvCs(BiUks{mB_|BxrErW2=W&Rstc zluMkZBp9;uOl+|h_+FJOJP-SHsM^W3G=z+|Hp_~!f5c3O*+$3AL-AFBe}=xRx(EqFjM3B43MhqWH+W zK&zLZhbTm^{qjsG51E74XqY{0?Mpd-Xm7;pp>j}nh}0t=BNV<_8T!Uh>ctH_3<4Hx znGuyw`w2^HZSY)b0+we>u>QL&z56H|i3PH^`@(ffvg`<4a?|a)uJL+~jj4V6mCl}W zdF|sG?q&2wTx}*6Y{#12|2cW$m)fSZ|z&(JD-hJ6yfP_lX$+pDn#5c z8QqF~l3*Z&l{_6$L{pwyMKq~Rl&?$2SysAtmAqw=!O2E$(MKg)%Y7b89R_&dG_*Q; zGZW|$$*R5|XJh>Wxf{zq7H=&dy!SPg8@(wpz388+-09Wj{DHhcE8y6xrGj}M3ZsiXct&0JN06~r zh7XTBuCd?6sW(u&6R7WNOV)E+`aX{p_Q!o%+crM~CGAYzY9^!$iBLP{{p#`-no^0Q zNt)ThkHmVsQkGv)Z_~k8<&+l0xK5@i?+MNUv_&A$iFUlUl3%qM1G)L>l=`oZ6rlMjJ?@hLTP)ACW{?)r z^{Eof+b%#B*m8Cx5tSw3GBpv6u?M-eYywSE$WT8jv^fhPY*6*ST zSuut!C1U|6_|KuwjJsoTp8uJ97uVGqMlJ<|^lJHqac-C}L4SdM#35zLgddScpH~o^ z2s&PLLU||QOcbBsG!s|eL{r3cPdK2ca7o$KU}Ei&<$~~w_f-+&(vxiEg@*)%tXI1W z&ACk+G3rYGaTM`z=~NKKfmyh*$#fI(L5xsmRKimKF!*K9*-%P?*oAvPrf^$}!86k< zTN-24cZP9I_JmC=#PN{TnlW({0YebwrQmC?ijK>RwYM=nYKeo|83O+X4T;La$(O2* z$5V`_TGd-PDht!TY=rE37hTr^?Z?(yN|GYhS{PH~xJ^bv%=d&P(T#pdMd6%s6O}!N z@mdhxw{mMFlguTil{3Hc+{>NVg|jb0i2M?m+ne^YmGm7d-HNv(qS?kGns^#DCQwtG zKNx6;m=4~K!CepWl~Pql1tuFdvLosZ!T%xPou@rAdpcfi6s*^^(dRaBeD+nlg}RL~)^TjD ztwax`lNg?#m{C>E;FBQ8O`+v6=k6H+vZ%>M5_!Alh65r|>KF)IwL+tc_MO3YB+CWY>1+=&*A(SSyI&Qx=ZFU#|=oi zKVd*AQ2REBcj*>ulr=s-cZ+o$H+6JeY`Yhy&_ybFQt9-C9cF0omW|ct)nD&f4s{#m zmbW&fOD1=P;cF>FKA$L2Qg3XjZuMhHPtrIp<1qY55tuwWZ-p}Ax}TVpZSFtSQLeWz zc@$Om0m+jh{@$piL=SbqLXN^=s1#mnzJYGD11x>eUc$KO$z0LYV5C~N12`$t)#(!Q z#lKuQ>W~#h@C)_#Q5??jom>=8or`S+D1VPwiJF8jchWz)4aWPn z9XAOSNHustr%htxjO;?jXmuujlj{8sIe6sS-+7YQG=YJ9t8XRDa zpD*>W*yzOhLlCom*u4{dA!BpXlb9IXcXp_CJLZOObcTIX4fTc7^`Pt zS=)11_mVLo>vd6X!mnQ8k$&3k)KnSx@gt^G-dY=NDRFt@Vp&di|tzhczY12(q`qMVPVg|yM(-ceKk@J z$p3(fl6`{D%`qMv9DrWua^6fEU`~GgJmpg3J0J9!Zmi1^87I)xABwD~w~5Vb`iXrh z`tN+u?pfRCf5Qp1nptH#4Q}e$Lh9i|RyV_=v;D9V8t|Fru%j3te<56m+gmz1-dDg| zTm1lI{HrO)b0xQ|u*VSq0y%y(6PgGGC$lttv#1BsnVf=2U&WQ`)p7Su-`i_Lx%G=w zBcS?c7$2F*WT{M($~Ha6Z(5c=OqIeG1X`s5@9rrqW)s{*x~%Wu-cpzIem8#agBPks z#lT?%454V(*QNV&bZF?uhrX*BXIs09FMRU2(1y({ek3fmp~h3>B*e3oUs?2`-74@T zuFtO4fN|Km<49T?P}PztyGgOc*D-U{MK*<3W29TY3M6ImLo(?iO+VI<2B(W>lFZ>Z zd;qc@;HtfVx(4m~MgvzSTA_iY`_Srzp{bi~ri)wSIt)n9^6qHgur^aoGWxZ!F;<-t zi=l7dT-&$xb2Z*CH_>Zsse4Qb6FPgnaJ^qd*Tnn%uP?O(1#! z?>I5q08hjntBJ79}%vpsv;Ya)&}LRHv0`w{eIIjijhC z6SPY?PsLX9H94#Xrk}rMSbLIEwp++%Aw@whZM`mu1B;_N=&}Rz`*ovg_Z4TufZopj zf(zCHYNzqqQNylpQZn42>dZS=V-_4_L=O+|-=eexl5ZVgY>ssddkdugGMFvEbj1Q3 zp+;Kbz)1y^a??LV?!jyo#e-&~$2){vUk>||xBV)tbdg1;Bv!RFq2HEkYs%~oc-Z?n z)e5JsN0YMkq`?e~JwJMX(UX``jLOgW#+EBaE)pB`MutA8v^cGv`QxF>vOAGhR0AV8 z_GnC6mmRPTzrs0a2COYDm1zYlj~`p8b5mo*|Bqx1IA4o5-Dkrmj9_9;P%NyoORpV} zjh5+SGE3M?$Ap=27$maEusx= zZ81=q7o16*mA?jCN7x%9%sKik%{&VD{p0Y6_bvd(CY zSYAfy?V-j?YyF$pWChVp=?}JsCvxdBUMx2Pj?(=5JIzeteJa3FsNI9sHYwmDVz|TJ zf&)y27siECIb|Zk%!knK0`8XKzvg}WKCw;j5)Gyt_N`!Tte>ym9U`5=PP*+V_#dC1 z_En;$ATmZoK6~CHT=C3#dE2GIpRW~m)2l4#1UlYY6V4^bxmq1) z@aVprq5MOX=)^$2of7Zk>(a{{Yc?weN_`buwWUjk5f2g&lG-8avXkyg+2hzvFV29S zXx*r~0Bo#aSb&*_V)#Tbx+&qS-$Xe#^BjR5CyQ2LoukwruxH3X}KUA*Ieg=@;SJT zYAQ7dT<+SbnvDrb zPPoQcI%`MO=DtCtx3aivx>^4P5e6lvWGD7xo;I6@-L9bJJ9@m|_Q`Y#50!8qP%(rx zFmD^FpeW?qX3rHsJ;NA*>IdV4GY%u2pp3$KC53g((WYcq7;NXbU^x|t#~Rb@sr z)itTG73`m65fk>0jZ$C2(`@MM`)gQ!#=IkPEwH0gyN#9WB{{+@vJ|St8S~7DGwE`T z_SQUn%Htb#D6Fyq%LLygE z*O%%V{nvaAE2(+u=tz}xeeS>Q(rs`scPTOPXMytucH<4mN(z$jbE2dulqGHHT4KRk zO${jl>>G6UmL!n>o>t{qhlXrLyJUH5cU>b;dM~~7GV<_kJV#(CuGZr~&bC`)Td#Pk zszKJ@(MGnRd%JR1tza_8RMyvmJ9YvdA!wV@xMr>o@Z6n_UXJdsqIp+XF0;k7N$uMX zU8(z)R6JvlLh;S`hqDTTdD1IiVD|bgKK*(()$;`$l}G8QOTTHGQdjUly$BfziHQy> zBbtz&e7#MAl~2e2kOd&Qqd)CslTejJL&Jq~ic@*V72dzyhsWisnrrfzC_w~+V!0l@ zv-Poj`vBa(W`*tK>r!|dYpgL`{Y-i?*bWjZ!@^DEa<|XxuZ{G+ZV5IZAi@O^}vQ;YrgV*8VQx~s0+6>xd=(Fye)SU<4fUvMjmywZt!?9=O zD|;~;rN~hqz{6pZjB29@>{cD9k9>y`MM7*B_?I82)7by1E0~tdO%a=SXUAR-F{CWI z-|xO{AR4V8I3+MzVToayG-Q(1c1@w@Az1D#dfd_~s$+cPx)N|iXdSyP{Xnhwz`9-E zOM<`{8%)ruLEyhyHCZ^pLzR8xZ|08}uZm3}&9ufWBg5Pb-jaT{r-vnj>|AuG2a<&h zdtKQ}3flplmB}C2@N9{y-H=K9ky?6~S5! zx1q76CL@_*D^1g5YNeV6nYDigwMhqpb@XC@)I{=V!NB4TF!7kEmX^98jE~^ zKj>Y+P{7v)m{PWK@j3$o7;@W(()jpSj96@y29TPEc2#Op=5)C2f)(-BInYvE&1tqW z3|Kh5z;z!MTVP<4v9J@?WsM9MTdP*bcx-e79r+4Ww>jAH%E;6W3vS%eDL=)|W40Dl zz9wBYOs6@ia7r9V_Bco|)8tbPyFQciuOFraf6HCa@)2Me2p80DGY;Mk6thvkb%|7r zc+?ZS@&2Vb2@{<9?|g8toD;^{A4?3hJ&SkHbGlM;;#<|4%ohQ*tK4u6Dl08w#x_YQ zV^yamTUW?s&Q`Z(tC9RBve=~E_s28_AlqvoIy#_6?MfS)SfF(~gSY1ISq)^@)Zk%Z z%^ivYB zPI+KcuuP}D-iTJ6fL;SkUs|hxv1I1ubzNGlfqvCvA^j-oY;I`RK|c=dCH!xm5uf)6 zw#lUmfdI$QhJ${*k2q<2{>R^^GoiizwjxqR$O2p!zxK=hLxL%7Qc_aKcN2;1;d~%W zAzUEe|7u9s-mq=)9UwSD0w`{(pq0mnbH~mO8R}u{s@O`n*HoZ0TNn^IQeyHSk%Sod z2sTi0U02DOf7O{>DczLvZMBS$4ZXy0_w$QBb)G9j#g56WicK(>Y6|v$@ zp3a_)?QOl$BL)dSh6mAaGPj0dvfR||QPMc~Kt52vWC?KUPT%+TcA_B^^DWtaS~Ei*$OMQ;VBez~6zM4RTrSBQ z*S#~~C51fr9W%tK4XrC_=SKwg4j*Rkiew>N?tAA=+X18Fzp5H4)qPRkq>{jP``@x2 zmwza1wI?sY8^cZvtOnxtAC0PjV<2U>=ERnP2BH}Rtkd(dy4p}R1Fa|VBj3J_th=5s zew(*FK5V9}A5^*TLLQcT`l(i*HpiQxR*^O(hM&h|j_@IbET+ss%g`Nn?^tY%z;)az zZcS+LAtZ(@)b@r&@k?ugkg}r1>jigDmgX}0?P;~&kJ!wUJ{N|9XoxTZD~Pdk_UJdA z<&8XQ`(~qZU1Cs~7Tfb)>^7=o85HQeTLIidaI+a*vJj_D!F2vZo;~4{W^;aT?Z$_+ zPsfHeIa-4Hj2z`}fx8m0zRK8}3;I;CTiPAKQT3!yT3S+pKyR10VcW7qXLlf}QV!^I zgFiXsM>dbRS*pp1vsmbMA#XUpc+O$i{jxmb(f>GlOT77y7>k94)kH&~y#jj7PX34k zs>aW%4|KNFU{l&i_#w52!yjko`su&|Se8>;>^q;9(k_Kj=K)?Yb4ODg2$!-BJ#Ihs zyRbXz+EGkajJ(f)q712;LGy+G8%rTDLsN=fa87WA@(6egQ%=9G9v4rXo*4Jq zo_;NV*-=11kYhOrE2ft$SoLwKw0p~Sp*6tH*5niZJbTl}SsPl>#BarsZA40QxMp_O zw)O7n@Kmyl0C7X1v58La>Drv!(UdfP!|p4lA4McBoJwIr{=EIZjBcg8p-5-T z>hL(jrX0P^@V+WU-d$h8w@{rRBKL&p)-o)>!ej z9xOxG+<8(z*r?sPhCduBr1=_HcF6dzzI>-_MMzd1s+#NZECuzAT;6Cz&IuLp&s+0( zI;0~aCI_Ue)Md_~)`DVJAM#%%!ARrgh>-_)Nq+d&(Acx+y8FtKOYJg*LNw2dq7M>N zLTsyh(ST7#hFN^y=XnB1|nO>3Xz)y=0Y2 zQTWH4C?OMlk70fM`cUQNn``Z{uFh&a*rs~;1d(i)`0IYavh24oe94A$XKv~W8Uf$u z<_js)!S~!1`YYs(TnFral7n5F^ZnGW0)x9UCzP!;H|W|GEIAxj#v0Q6Kdzowmw|j_ zxYt<~iwu2#vM_c(TPf0*!YfcU_dW;tBS~xc{h(1B1Jl-*p)zE5`-2~zpl@V!T&N#k z-ARxJBX>E}&X%d)Q7j8kw9$;py--QfFIHUDQQE`z*#S8fUJNK|-h|rYANSujm}pM; zfO8w0EKK@2GI)L6Y5rxX^@!q)xXWrI8T&%ZDa{bK(LA!)0RfU7k}yiwUVIJA*cd|# z;?|&dlANERr8(@gcK>3S@y%0k_F^EE<@8$^F8 zD1GO(S~?Pp5Q3x%zXH|)n6iC~%ZKs;3Mm@o`2zsq`tp+2)2pdjmOsO z+uz@@EMG^tZ*z_rUcBs5e-!(n;NfEyvb)L(%CAB_c^NJt+H~9R^puDAtwem}!zAUY znI0^gvf#)yr$l*DUKdXEfZQt(plc2=-X9M~`8bNoLzU03wuv{(hkiCv>lR>nUnOsd zhS;&>8j7lf%XLP;7@4iiO;x!@asP3BD8)Fn@yF2W=hF&;cBZ@@5d2nYoyW|KtM}p> zS{JMF!eyB}t}Ho+T9AjzVAxD`%Ca^qSoyh6(hyN$CQ!emE@YCq#F0_qH&|ET1vG6b zV$DwSFZSF>wgB)G8kBl%I<5IlcPJ*@d7A9!6|QW1MYj%?I*kCEpV^Vid=tTR>V+67 zxGc!EW2dG|R4dt9#e!@tkPCgu$M}&bu$)Y-fCm5BnY33C^^lx%x~DWGHs;K~6Z>s; zM}*+pJP><)!UXcrshkp7R=`BRj;+jH2LE)Zj%Fm8I!sA6 z(50Wz%dj#aW?W!1J{MEtOY+Qx+0Fz>D)O-)hywctUAD^;hGE>zNFujRY=Qpt6e;{uRe4!wBsp<{+B~9}p(ozOglmISec#p~5LX(xJD#SajZ@>RZ|bNI zm9V>nAtP-DNB55ss&F{s2rm7rz`@F0G(}^N9guJ7?-qBs$ntSit)_TC_2WIO(du^| zt-?i9R=6^Pr`LL7|D?jC_0WgU=P^1&pmAe#&-@R#Ezh^2?_BhkKK%Wo{+8c9Qs~0* z9LL4~&{+K#W!(O2&{%3mLNw~9XZq*(i{IFH2YtGHX`a4zm6TDGB*LiXlo258uMnF5 zQOK534L_jef42V^>iTW%9nH)(WAzI^m4b6;u_kumzfLr zGGEh|rV(0dTbrxzSkSpgM#i3*(TH{yL2$Yl`PB13&|W|pk^pqw*ch=iWp?cMW;;A# z`dxrwQ;3O4$}?zKqpDb{IL*9W_Kpr;`U})K?S$K;L=w$}o(Fy?uEA&7iU>3_<&#fX z4OGLg4n6Zk1bdZ`{+U=gWF!$WJ2;48rb9$GmFN`9o-JL^YcfjRr*QpMsCneXr)Q1hsZ*%Hfk#Ekwfjy+Fo1Ok3`>ew0xi;^sT@&j1>u@1SMVfJXiVq#&D z0m=yPW;{Gzt{e(?Mdh3J9c+ga52!zFj<)@kb`RCQoAa}6@ak8YC1+K3^86=-`D3=U zv}F-v()e#8BTXkHe&c6*&TZEvj_scwIlo9@>u596lho*acscMtf~{<`I!sR+<@0^d-* zjdB#DSoo>LzRl5Q2q>3(Y$NsHiFp5UL8)k9+)dRwol<@AjMt`GPtScF*fR#%68lb1 zjCw(T z<)>bQ{KStRKj!@F+p{l{@Ytet5PLzkqJ3Z(h2}E0v7i9%vOI3D=2z+6f6CH}lYe#G z6u*sOVzEpO%Hxt!^hM3a!0kEI+1$iE+J0DI`91k&1||e9}sI;f~U(h@inllqS@kzqhh*{x|Cd;k9B z%y1leRIl@(18J2KZ>#M&!iuhL0rYUD(R=K^rg2@$s~_wgbw zRh2LIJ@qiC_aMn9b%^L%N-9;AssqnJTDG>hA>VWG+CRDHb>VD%{&7t0pGCjNFaHWV zC;&ICBs4gML%mvnVUw5j_8Pyh;L>TJB(VIYsc?ZQWsqbo|DiADeQ;~1X>9`7_NuK< zpD+l!(Sy4Do1Js<`CpP#QhvTa=Mw+D5UqgL($wQ!N0ddJ_v;zKc?3QobvcEUiZErVtEtUt+f8>C_hS# zZ%zdk1`|@HpY_GNT`1>I(x9%?Xzt|?QOO%oV1FA3z7*u@Fam`2^E0tqLcsWHG^Mce z#O2Ap)CjbG`5zNsrLGnztwKBfY*x4vszI47>3|cZ^!G~}fW!KBS%y{Z&)%FoM*sDN zgn?yvQg^m4H~MU%`;xuAzZCv1zf`386ny=ONk*dsazjZ;`#W&WJe_#&yC6rDR66BK znq!pF(3DuQnZYb}Oj(O*m7+-+f-330JzSHp-=BDwmWSs}kUQacT3TA9g^hRK64S}28z9O3Y?1-!22OjzUz)Ghs zjs7EGFTm9Go<-^0bHw(TjzL>2o9cH7JE~Y9PKRSUWc6+shWoAoHoT7yzn72KSB|O2 z-aFns*XhUxwPM1DH;p?#!u+oX8~>)vdMz) z)tcyUZTXK%=O3AT?u4X$RAqV%|1wDW9XTUhTEt>Rt1c}{l$UlAoY3x|e9oF)=r#_O5UGjdLq!ksKZ! z5h53B1+D+M_wCHjP*PmA5bZCLI$K}5;+%%zKI6F?DFQ$<^zG6Z#wLu7wUhOk7GOJzi11e)*`!tfTQ+~EOX*Dh%?*kt1287QpMa&mJAfXpLg?RkzJUNmgif#b`N zPfP6OZFXu7=~)Oj6DcFbSfty4lbUZPIpV%Tw*oHgyzHGX-aEoLp$S)CE0U8o{uaD) zYq^Vxc~xmZ89fNiG*)=I;#F!Mz!))o{PH1$lolh5N-zj?(Rr{*yxBm7NHyBwcz+Y> zn0~$Us&C5!8-JL86Kf+?mVx?22~B@r4^08V9fqS>naL72e>**@a5<_Xq=W$Eej%Z|i^oBR>WdgNYgbM+*2 zjtP(1M#~n(3RJ}Q#t^?u+o~6swzctYefPjG@!Fn|E5+vT*^a)A5m6M*(7;W5cnDvH zvU~IdtxLiOPZfG;$JCU(ksiJYDwK9@95+)E}3_u z03`o7WWaFGoZpiO3Xa}i@Fwq6*esg}LYZLC!$8!)) zW@#CzPWG;xkkMo!FYXqH{>y_uIKD>g@r6@8>%R|4PIEwFvGQu-m67Hmb@}Cy(^qGA z#a>xs!aj1GQvM$5)m~pG#U4Tweh}q1R3Ib5MiUcX+;w?qiS%1s(%ntL(0dBV>16Me zqI&bl-f7}T!4(HhjuKM;OzUUcyKd(Hw`cxj^trxczSfIkaBiyiiNv`{)ZV>+>uiw1 zj|3@L47{_6GlqqKZ)ay}++)3=SHY(N6qEbmKwshc69t9JIN_b1aPRh*`1vl7C{Eaq zLImq6K(7XD^tY($^;-2_wdW9=Y@3SMG;}5*=fV8)X-f(XE;Sn{G43i@epB; z>zF>%@lvW zpSDdqg;Wh^)kMxy0_@*cE*nvKd079l7{_$odg^43x-7#eQ4gKC~1;O_erX+j?U=^yNaWDx*3D2l zdf%E~Bm4XLH0C6^U@jlx)29}19z}y?evoxt+H(g5h$4k2FE-mPi1%G$@X=x0VMxyY%Yfpc^9L50Q}T=0$7DcW_nn2jo>jK@}4XQUpQC zf_x6F`OswH6Y@=Oo>jf7`3gyt>fF~-*J@IrV4`my--5HlMG+t0a;lH7x3}+R#@T~A z#w4Dv1}H#(?wh3f0^EZdaIF(#m3bm z8(&4N*Q}Jvq}3y0?|YYMS{^|0AW?b7drg$y$z^rh^(EpXXI-mhUd=y z6nGHey}ETP`Re3uoQJIyL%8k9ca7bt%UVBk1)r8VnP8WWeyIOs7OZ2oslOVXyt?0~ zOL&~B-SFUg-bh?+yHm7B6@nA)`266$ z?Qhjud5SE_5lhMMdNQvAd?UWK46HAeaQs@Nc9NSV1M%iwPwyqGb*uy3`=8C6vJdS)po|HoWJQrmF-i_WfopxJQb^Axam$zOl9YB z7%?ThOWAC&>E18&k;e0>tq4v1RABv%TqQp$AOLpx`(f~T-d2>CrXKgTUJkfzC*3BI z^+jBUG;hGX20Ome7_91laLRmtAcrKK{Xj$qV&To5B{3)=7%^>DlD8`30U8TGdD8LA z&D2j`dcukh>D47N5lDVAv$qnf=p_W*&TB6hm+a2D5nrBcHJ!zunQdt-y$tvgWY(UV zqVUYynNo}C7rB=%&pg>9eH_vPb@AUggNSyS_zo!ptDbkCemlCge7n-PN`&t0WLH7Z zaw&dBpLs67QLJtvhtEbbB?EIG^C})?l4K~wQb|cBiSZ8y$0`LFsZm_EVAf>OU~4E^KqT^rY1_9F z^ayo1c~uZJU|b;rxR?i#wU=C4nK|^{?@35e8g_Mb?@kzo*OYv13X~mok8jay4J^-_ zvAabw?FiLjitb>kne*b(N<61c*~ucw;1smG8Izt<#;@CCTJpM#tJ1Vr?CyKm8`aMh zf$Z<+2zK1^c+e!ocO7Y;;V!Ng;S3yk99&BYty?x0eI3RRa+YpuVOXO;NoRbgqp(RV zfn4EP_#^NrUAshgbME33Z5=sV;AiJUAU=#MBO_%2huDoM@8G0O3hUJ(u_5|t+ z;^YZ=)1TUuj;DSKQJ>V2oG!k=^V!ZV+&@I#@!ZMKJkChcX-Ya{3gz%QVy!t)zT$J5 z%6+zFAMM83e zPXk`NPYHZQDT(_TgvEQVKDT4eoSg*hqmzRMG5mulWU_QNWHk78XR75T|M0sBC zA9U`U7W`~6wn#Y`eh&`2>qg8{@Zbr>y|*1`A_$e{14HbYKMbD8_{Q*N!A(nsFq%-8 zX^|S0h8SE2P52bUn5+zeAnY|Hk_TFZ0#$E zP&epTeYD6Dt?3VTzft#2^sBdBvpw!0HD-`9s}4sX=31YjDwl6joU_=7&)vpdDoVfV zD6D59#VM2JrYPN{h3{T+%VgUMtAqp+v=fH%AK_;cd$QEHhN}KXqhvnw?4y?6W7~&=V&J$F+{50Kjp^vnQqns@z|)7ZJvM>JH*#H{&2$lX~N)Fr$t=|Wr_oZK3+udy&-r@yJN z8TltE^-l&O=qVzmF~sMvE8?^n(b$x6dYO)_CTy|E<`#UH?e)??w}qZxyf;ACr<=z)FX;t>#^F9`fU-Pns%?HH5SnEK%6=^U)l`6-WL-R>2db6Fk*xuKOlihyw@fxk#J-r+LMw)UV0Y*cZ_v42ZJ2-*6-LnH#&%;xEgG_C0$A@G6tR8#gE2oj@!MF4KiC*E;M8j-ne+d70@eBO7 zW+mjb*lh*q*E06IV_sz2=4o~kjp3;WptcKx0RRBVva}S!fM5ey-Z;$>0b1O*CzbEB z5kst!zsVLdn@0nc99Jpqtoj99!t_fd!aP*Gn67-qXHwK$5>i4HKr zw4a`wv!bC&vXc=66eg#nL}6e$QLz7D(K&-quO;`))=#LK5X;Y>#*B6?RO={-;jrF4 z47Qf_2lA?TgC_ZLC`D)Ohu50B<0Dylxt+CGkm1oz(lRZd9D$)Cp z0)PS7Vs5ADY}z1nj_i>~hljT7)qA=zGeC6rKbAkG@$)T6*|Q^rRGv-%koSvoGyhl4 zsSE%ak-pDcwiiR2{#Iu#WaD5`)-BET&6$HEo?EbmJ% zIW_eomMHMY*z{!OEbd1hU0q$zk9Jr2jw?z_$;7Zg1zMU}i6!cr*U@hCIsj)YvB%H` zh}zh2|DaD_enE_LFJ$NV_b)_KrQc`i=xTe}#(bQW>PZ;j;@x5AG zvj#_EC8bTfOv~Ng(UGi{Gex!Tih1h}yIw`Wi~GT#6eP_b;1?kLMi+fihGBF0AO|{R z-iILiH$O=I>b}$-1jFB6bAA|;ApM38W?(39=rMt(6VINSi5%>i2v4n)DcebBht-;@ zwfv~9|FItdz7qS?w8~ZEe$jI7S5wxwfWoL-b(o5%8zMWyv#tJ3&VRVW#DG7lZA*wtk-rbO#{G~+wQL1Ks8H7Jpdp@ zSU8s+6D4m|s!~-GIgWQEET~=*lGPE5Gaei9?Mp2fV9`3R9U+O@cFqEC;|#enRpcZN zCxSn~!+v>Q)s4a_BdNqpv%jI`B@k&|{6>TIHNn?_E50&mFReyalAe2eQ>Q6s zDRnPGoCq3k*x3f3Udx zJ)|YtC|QS{4|_>I`E+=jyJT;t{|p=F%Lg!dd}GDwF(nC>KnFWSC>vT^;|U83bIKJ) zih$ZrXQ#a7NC~d8j7pBGMBCE{f{(=l&wgjae6=x&$q@p6V;p6AVO3Ojk(uI+;T5DJ9ux-@ zWqc?@t5y`f|49M_SnKgZ%t%}EKQZ~DM@KYdA>N}*od1aF2kItI}%iwR(EGl6^v>eul6W#Sda(xP0%bd za5laIG`C~elAb4Q_ZlxF&{rktWTL;bSq@Y5W!xry!Yq{5?ab{C;>$OE} zkgHHwXJ=>g0JN`;U_P5B#}_HO$VXE*cg&QbnWxW1N=ho#K)8D%+`OM%QLxzwEpj** z%ozdogqG_ger;T&hJw4ag85S&TsA}bW*V+WmflKJH|0dSw5$T*`2$*g8NY33okFyz z2)}FT&1$H#170NFeL=LoY-hp|X5#_)ZI41Aw3VHq*9grvz06TE-A-Vpqx$$mUaZ$Q zPlfLgkGg(vkUaa*_24{qY35|40MQ|3`}iu>TplfHNQ!Z6#H;bEqN2tvkx!hHuGNM|cU{DRvQQ=hO4gOd{o`2u?YQ)?TH-6P=oN^QiPXiX8U2JJ0?WUG%v`*_N06 zg!J<--?=Wo@fbqAakdRS}$vo(Yy>~F=0MF(n5aKiPaC|^JF)3O>O9O$rV=KDU{BMFK{Xi}TGajiX0 z+4kLr`V}_;pvr!>I8R?BJ6#pDr~W~C4mf@31zat_Fw=EOrq2hRm48G=fQ7<2P!tl< zZfIzLq(z_x>Y)={PrrdKsS5xN$;lU8_}o~|o13TOpII_ASPc#7D9Ykd2++bEHfy5Nz0sZa8{bc%r>wPZV*7jPF zMumml+Dy%E3<91%nafYVy`@`wo6wxtNtfvMhZ*z`(P10CTMn*Wr|id517w}f`-ma(9-I9BexX$n|#b91*o2fNKgjh9lSfC`Pq;#aO~ zmmC}%25Y=Lo^uy(X^tO^ADj&rTZB9c*9YCYO;<{w63)fN#S|q|22|^w{WkAj4%`n8 zzIyH2HBp&hi-EjkP@j*!7hX9{X1jH)Kq0~fgA`}?CtKF_i`E{A#Rk2g4`Mt+4%`e6 zq(M>a=&xWN0n9E0!)0FpSl6AM{8E$|fc%n!FB=?p<%$9M6=P%Lhz?`BmZ=7UousytUMB8~f-vFh=ldBGgcwuYIGwbH+YZFk0K@ z@4m6mZN*6cLknqD`TL(Xawn&SE>u=YfD~$s7iBd73xiwZdE`aO;sE#spN*9`i7Fft#_4@YaKAyrY#Ar9JhDUVhQc>=rPZePh*Ss^9(XKo=OmT zxwedtWp*qm);Obm*Q)1Ico6s_|BPeseYait$Hx2`q^|2>*D0=b#X{1g=j_;E)Hfs* zRpG)n4)`l|*-}H%Co@`>WKWnC?~-kW65iE+G!WH>RWRXQwv`ma1>I?;%3Mt9$Fa@>gH-Zs&j$6;i!DbT38I!ur%s zZr+dmE*L`~n?&&vaF)5&_OT`J3$>brIlAk*dhV^mA!U_*%o)DUQ>IBp`tVVC>^x zjR3Y6Jf(oj3)&HS^Np=rSPJsDVjqjQBb{lB9=sgLy0@@R#~wuITCUvm#cX=-^iNk$ ze}5ooWGXTRNYlSuT3V8nI%%Do3m*pU9!D1!KJ1?z@weZlDgV6T8@gKU{IJ|j+vJql zgks^<^&K1*SZ&{jF*`s=c64+U&Cze~E>C)-Hgu{_tTiKD`jo__#6f=PVYTUaSv5>q zH|x<9)lO2~P;-yxm;Q1M@nS8YZ=Kfb#DhdsN9XX+c`~nZeANQAM~W8KSb@CIVg4yd{i|lj8@aTNW{YWP)>IRk zQdp$sG+BY4&o`*QU)B}9=?|URZ-zO6V_NySDYb)jw*!%DM?8tz^!} z6TXVqwj`-AUG8l9dEI=H`nrG*QEeI<(n90DU*YbrorPWH)FIZ7@yPwM`^*4-eFKr# z?m`lE1O9!xDvL}UpTlIVX~N0L{3;p+p`A2`d;dbKjaAsE>-8XtC2`gU^sgxO$HH-dO~XsyEux6k7C zT3v8wMcQyIkG_Zj8=^rAsKUmy9e^%cV@_A%c9CnYJv(T*>ve3@nTczCG0zxAQUdvh zo3i4@zaj?GSF#vcafr~>_+OqKuM{qp&EQpOm4x3P-|{&n^EA*VGtS5Wbsd7xN3_VT z1fxO_{5G3a{i)Ip`~7c6{`j7t?NBht93Xf*dF-+Yw^Mq$S&DL&D3)JhV&Wr4mb3u9 zzMiNeLF>Itt5;kv-A78SLVF!!arz)QgVNmhtjtjN5{I6^`J@6jiasexb(ZVoK|M@) z3-;xbx62dWx(bamsif*vIloz!-xL4{rP9(;r7GtV!q_XszSdg%9SeHR6!Z06{l`~^ zEP2$ioZ0Dt14r@7%m54Hf`$GO2f$%jw3he88FMk;@kW+DsY- z*oN|w8G2pQH#A_k?%`gSKfQx-H?S)+tcO-RzL;jG=e8Tasa8~F`lISr{Irm!BIAWjL_j`wYz zr<8E!bHPTpep3uaQAsHc*kx6c39GYD4R>2h%QaY~`%SN~Vqo0zr5bVPYFV?~Z>#d#Mymy?}E`gO_pdsN;38L7dTjI1TvT28lXPPQch+~Zud<^?D zrZ`z~l;j$ASG_XTZ~@=nlpDM4JLkQ7&&UR4??ARHQ;Jw%2^aKCv@e4%tKa;G@oFz3 zK52+qCZ_d{V@3r9nLno-F%JyTLckJc%fSH@ZUWNGnA$t1JHjyXa92w(V{!b(w~cft zhC}0QZr1O5l4411h?T!NX7}K(>Ej0P7j767*D{v@4+8S}`X)w*>%U`|pG^t1^W1$Z zbUFwXhf0H36q99^zqy3|M4ba2i^nR_*?`GvxgFsCk19 z6wz~)u{Qmo@({{Sr1Q~sT3mr^0<%>^Q{1WM%+a_aA;L?F=#kkKu21TbvK;Y#WjdP` zntAkgMrwI(vpefPh#>Vmt!{2VpOeJ<<6Pg%T)j+Ne(gEj&W5Z=w9~rge?aZP&UVm! zR#{a!`qWDvD9jVQ+XLd=3!@L$_En-`mOfSX|3@4-ftu}ZHGv2akB7WVtLi9SEbLc@!E)_ z__I+@CrZ#p9_fv_S8c)(VD!wx-HsGy*`-srofwzj=J;n68JERgwyz6R3@(8-tDF%q zmBI4_a@$WKRf9v&YXyZaPXqb5fN5}%D6gX^AUJ>mGTD1-!{y>v&%xrLLIR#g%P6te zG^j!b%Fc;5g=6e(6OwGpLvg&4bN$cJtN8+8DDlY6smWq*UC3-z!gGZE#eB-3wVnim zx5m_?W$!dRe~S2Q%Y*{a6>JkewYx`I*NIm-Sz7p<6tTC|bvs8^$l;F-#qHVw!9RzM zdx&DwlThnYwUxpFlXB1^2JL8pLSZR1E3=dgvf)$hWU4aUCcijEdHM}w(v8?@KmXxMA^PMB7zFZX)v^8OY+>#W?^Nks&&AE!>#E1WIetJ+D_Z`n?){y1Lf z()4moIld~5`%I{FPuA{ig|9;Q**e?DajUZj13|moHO{09F5tKb?=dSo|D;dnZBf&^ zH#l5oWR9gz&s!}OT5#o&$X-K{Dz(wJp0dc@yc=#H( zoPWAm<|oolh4zQ_>{zc-ERxs_O{CDW%&+JcUn^NS9UrS=h$ZO8h~TOmlf#w0{yB!l zl*6caglEgXB64Sz_`vTUD=l+DkK)pM`DBW0WJR5K)K1DZ*SrSJg1z4?mB>n zkY-gxyv>fE$N7mxdo$yWt6aZiIfyk?`!TJQ2oBp!7R)MxnTo8c2;iEwZ%ccFFN#cA z<=l@5_!kQAjc)BD5vLU(Nov!TZpLTfXia00YWnLK*_2GtwiV4Kc@5lYk)h^)-8ZeAs!vGps{YcFou9=3-x$!MBau2EjPI zCCZ+{ATc{UJlq(?-cs_Is`c}SGBs4?I$hPmTWGAwvPBur=S~gjyAN2+j3PX$Z{Nh8 zAx3P*`K=d!N%ZOc`_%5~tv#}95iTPg3;Eg2js-&(#oDDDkSKEc zmo3z)q>MCcdjhM37xu~_(BHU4vTErmCsEhMJf@THE zg$_(x1~<2uI>MgYZ?}|LVSdZU930RL(2N&LbHz=o$9VF5+>472oBUs2dR663FW6z~ zXT~Xpg3pY?Mm3Em)t0(X@Tc~>06{PsN2~<sZ<}CI#Ir*j5o1UOK%gF#p{$~HGD0P6XyB2 zh^G@~{UKTXE{Ytwv?^W8X#r{b>I_Xpe)}nX4Q1j3bZDT$z%Pd`;qZK~Rm4{}k^!wp z7D$ww6fkuGWmlKNjLvvCD82#FCs1m-5PgydKzEj(D5c>4&Vo?M0jyylCzXY&-64+_ zZZVp;x{?F7?AXc;0AKeBx~3&>V}lQnV4(3PgW^K?wH8cJ9R6LtEAl9ec4`rLCWBL< z>e|F8;xAQ|@K!-FADp&*rMl)GrE~l&aPiH=ak2tRFJGf0Q=8ZXs<15w0OuzTQ~C zx1N5Fvdsi`UP@UiA7)nu3o+AT{UDPZnC*1q)jeajxQ{>3VOKCuP9i=75*WX!tI!u5R)GHRj0AkX+V8$ZWPf;3}l_yavXAi>o`OlVEi_ z;Dl0h29$!0@`rL^y)9X-|FXALJN(s8EnVX9E@#}kT4nyTo-FjWVEcdIB0yWpcl}5m zRm9PWnQHuVm2Q)5Jtw!EJ zK=Qu64~#n#l1)f(tx2#?!^CbvvRaCY?|Lge$b&Sus)#MpvopW4WR&~s$!@ymrfSYa zGEGlm+22{$TLk2EAYlt^sr=e{LR({LMv9k18PQ{&W~(Y?-$ae{$BzUJWbP`$X_KFo zd@s5Cp5piLn`JQGY2~&~MBldp+om^Jzv58^f%6i;9XOeu`(>YPDW9@6{NbFs$N{9w ze7hzX>-&07RG@Noq%{3NRAKaVa5fjZ>f96V1h+-TDEPm**FebZkrW3LS0#ID=0a0(8L)&e=428IH=Ra`@K!x_Ne}E+M0Max^iqN3b zE4pCTLA#*#rF6THEd_6&_U<@mFrOdue;ppgfugHmx$^fHs6WSj@Y2!-cw9eV-DtAt z{9i|e|7}J1QcecwzDS@E{D*lA_*xFb1b=;Zpm_|wy@fo5*!fw^(&!GL1bBo9|9|^Q z{ZFFZ6%QT=sPLOV5t?D3Hfxe^vk_0S5FX9)P(V5U7|)Lt25P2o0Ji z>nY=Zi8INpjOy(5oE9hxQZ(N>371KG$JA@pT6UG7SS%IR> zLgLXqKz;y{g6Gc;Ao|g~dwc|6i~0Mx0qbtxyrVq<8I&$PIXSs4IK0tc(uF{JFs3b4 zqYu()Gq&59L<8pzy8Z$QS9O5?A;G(!UoNCQ+CLSB*dNCZ17BD4|J5-9k0fvkr2%+< zU{6_!M$FHyTi2fZ47L-hJA=^Cg9Pku(w5II(3 z=?+BAd7f_JqruL@4Mc*o&(|ufsi7LI_Q011_s4&*2&}cAk^eviE3*}qBO_FM4FTitDlftc3?lT-Sc~>BLgx3LeS`&u+|n6zU-WB^Oq3 z<4^(0aBmHAs#BTcmmK^&$4wS>Vm~LtVEb1v!kgW^FXcH+ob=&u+5TjZp4qN&3^=89~i?v1%$B2k)uD&*!4V=zB-S9YYytmtSov*z&Y2%1RZ$hsYE*f+=EWJ z{m@ud%6uxa6D?s5&}gPX4LY>Yq|3nFeHi2c9_wB;gA5w=E1xB*ae?j`)%UP6Ou$psizE_di5Q;3i%WjH&Q}%AA3Jr{U)N?08@vAWcu?*ZT0p(vB8mviHVs z3a_9UAX>DtG_;E94ODJ|5k8j*(?CuSs z-0#uy>osD zac9_kS`uQ`B5Y>dV+^jStF0KYJ*0NLOWr_ijQ`{f_%C4c**r+$mYkXJs_BxQUBU%UU#S?-^&_NpF=V{ekZ?WH;z5|$~#}zwz9P*t_ zAu)Lt+Gcj12X!1@mu(8h<9Apew!ed4g4Qx%vfR4e|_@wI@Rx zP9_jV4jhyJDHyF9&6g6H-(++e^Y0Sv;(o4~zIX>y~1JTf%@G9BF?q6tQ( zAMuxB_`X92{vinc`Btvefz<&+&Fu}R1X50GH-C%dCdhELM^Eu}~t zJ*il`K&*ruY*3cROtos-wi+G05)vdjYlTj(qOVHs@A=8qkOCP?Q16u>kp>hx+ZhWJ z$1zY3bHMyi!oBy3vxd|(#~yDPq6*Id`KyY}6Lb7sl$c56O^`A0k$rn@iZo+a^jzM; z|1SCvBnz|C<0AGP652lj`Qq#yChvjPExKWHC~3kC0vTE0)$X$0ieL72bIf_x*aFK+Jrp$4rm^P zfJh}V<6(kQ#nAeBI_3ea;TZkc^?U$&@X7@_A=(!3o#R1KM7gyYWHhtQ{&7GdhOrxz z$WrEeoFJbLN?j?`*{_g!fnlmsYU>kR5(^ey%N2$8Ad#E_y322)*$!afPOO?q7Ec|;&4iZ!LTdR1gS&Qur- zhZMHZrmMp;qcrvV&)$~2>wmDWUP;xXR)2D57+cL@(w=98AZwN+)~09$*pZ;1Qio~L z#p5np-=pKzNEuDm$K&!XwnCEjyii3dEn+UeO5a{vj7K&etMdTk@5A0#iiOE&K-dUtX$;H)g8)vNOl$uqpU6_ zP~dXO*ZLK@Mv*fb#2W@?+HV-X0JR7$tr^}s+stHvb)Pu2#M{F3*}w#9atpu>8_GF1 zgdshG|J$f^0okMn843g0IqI@_KR@q(#Il1W=oI0K-REnZ?AB*V)y}B!fn@+VS5ih0 z@ycRF)~R|?&q;i#=)~=rtLq?8v7 zWVsfS50U`fMZec$%>65$Np83iH_4;>HyhMX_ruA?=0SQ{Fzdxydip3nen{qOx>0!? z8Vs?gAQJ3z7IW57?Jl{N1ahmHR-}#Lz$#`;$EJ*pP|v=X_j;1|_svqodd(5%g9mHl zo?{T+0P@MnQUPI1;4c+r^Iv0*;=s10+FbMY?b6S{tXn-SZhzyI2wl%KK7tg0pFyAg z!|ZYA`Y|3#pF7dq`}W3Dl+eVbsKQ~q3Jsm3S5&Xgh4U_)m^%McUTg13M^E$WXq+N$YBf z12)@Oi#fo8%+Q?8_*Bi~8RV*57Xw9`YYkXX-Wzq@!#{e)g-{JLZi*Us_g?y|jmi{w ze!wW*fAEqnE$dI%LuTNpKZ0Ei6|g#n}aI%vhr*NdfA;qDxh-0 zg&#-|ib&fkJ~~SSgl@hJzQbR6c`s&oQFJ0444<_E4qod$5J>nE-*?9MuTk#p$?gnw z!1W%qU=ol0hzbEkv2JeR)(nvov#RZ1^>w~#RB(HI`6Tb>s!l^{nXzsSZItp9zfyMM zdCRF;6mZp}dabD(?*ixRYGN=Gym2}AcX;h7v{UgJlbu6TnR@Y1-iNV_Rb?c zJ|7@*G?T<$k(5>@rO3z8cKz#^_L}$Cya(z7#7+HjH;%zK?Pj`}ANao4)33oer&@ncl5DJ0o4utvfh-<} z_rc-Ona$@Vn=YV$HgAL{rFWiLLh+~qrxqkh-#nu1b8g&_kNZ1Cl#qCHJ>)b@IX%}X zHKZkn>hfA*_H$v8Jz(+u(WBpKPqwb^pMFFSLrLuPra5*~&W-A0F!u77Yt#xmjK9Ys zaNWWk^h#eloaT19FF0x4?RS`kF3OsKDtuDM6Pa!wCIl`I6nm~}{q?Q?gROy%8e^yu z+(piQnGJP`fM2_rL#NilW4(~M`IOyZWw^YbV@`)#_2@cMs1u|`1AyHD`G0S0_fFVa zt+FvLlrMO{1xt(zBLpdt!-7ZhTN57~prJ?kuQr(6&v3(IV}>+Kl9Ggc4!x=XU@ym4 zqi<$6(WOPoxG>!$J5c<1psTO-y9(ZAvcJ-5Q0rp8Y-RqYf1^+XX>xBc|0qyOax;xI zLt38VqPc;}0aJ{mX>isbssIZ%^<0Ycz2iPkIDESB<01K{$MX7h=IVd#lAsPmXAb4i zsKf&Lr8M$vGd2&1f?lizQg9o(m_P=oP5jRpf@Jg}$v;0rN=d^%_YrEY{OhwxWKr6B zv-QvxRyTqrF3NHsGaLSJo|~MfBciC>1mIgiPU7Qx?pPlhrU!#fIGh*3WPtk1<59a7 z!7;f+iYND!mwA!Ay~w0{yCh zer1Q_Xq^Wz6q^UodOl^>6u}`yDdVnx{5#@==4JtM`anQT=|2q`|He-LBeL582aKm{ zFer6%wCb;L<$r0)^ma(*Cn(Ce$gAK?p>%bCHTM?_ zTT||7P>KQAyFHIs2cSHK4J*4yCWhTl{?BA$HY+mn1dt&TXI}%x%57AqL}mV8=>zEJ z?C|mWIQ%&fx|W(vFC~eY3+4v!t7RSJ60Fm$QP=*)9xH!(|AYb9S)Ajzfl>nSl7j-j zo-N8A1YhMp-5<|T$={-(|E%whCPp<70HbX~sPZ?O{V!nt6bCzr@gzrh>B>mE?}H|(`pLx<3=Ps6~Ak^UElmc)(2(1 zxb%hW`46h0_K808nvb`#LSih5_e(ZhQVfch!b z6%-U!7D5g@^_Co)ABRzXC1oWI!EWGGf~P6NccWVK>^0oRgktWd=+15L?X`I0#W&^{ z%}%mJA5wO0(La8rK*<+2T=hgM<5;CqhHF!IGZ{G>FW~;<9f_pUSY+H&TgT$J$h+ve zbJvK{^BT|#qoh!>s%d^)RP*#WM znd|(r*XaYV9_K*s{RXet&9idizx0Dw_BwxU{7$FPOfO$w*#i8j%px3|SY{e`{y>ZI zSq#1<2JHN&h%$EI;-zo2_}0yf*WLD}dY4eI={4eybW73Wj&v`XvT^65L2ru6h8&3x zy<6!{7N1v~?u*@my?Zs!<7;7Shz0%mCorHjW3J1|voJNd^i$&Eptq*vsf3O3?$l2a zx5U_>_JB5q)#BdD!nK6`l>Cnm_oYxKP-&-yi4qHSRnC{_j(Kir6K(Fno+y!g5{GF$ zaoJ;cz`8i0jtV{hQeJD`Vm|lEgZWa^7cXXG)4#83vticmIxH{V6ZW1Zg#LHH7DabG z+FK8&H=C##;eo@mz`byLtIF;GQ4UN^7fHb|2DQU?Bp&j~iYh=Q``uI4FeH%SGMCq! zV`0Q%0nS7bW$|F7fB5;O!z`yW(d(Wa{N$vPhqK!T+Qh_h`{l1C#}xYy-@sGTA_*S0 z_eJ29R2aAolw`$ys`9_Uy`qc(ufjs02MdekFi?MA(rYtBUxPYirr{SDX5R&Wv2>`A zbas8}{jI~n7-syj4cgQU+KIC7*CD0Nl3W@;bafw= zbfWCw1h?dW4+$c=_dh?17nHefUN>73pS0A^o4_{rSVV{U=1Ok4zyG!5y7wCz8op|( zBS*wA*e$$^J43;H|Gps}T5a@XDEF=Mi$N7jBuOZD8>Y=3JVJ)1aWO!;Np6CXcuOAI zF(+GvWu5>U9hO%``mOQMq&d&e$v2&TQ~BhnR5%y{FoTfpsw>b&+)Z0mjXdA*llZ}= zFkiOm)d_^iyDwfx)&sM%Uz(f$%->ThZ$AJ6f63s^wKL~n2d1z@$)wo7X|fx$Ytimt z&e55}cqj=txRkYPY!O?KUB!Uz@CBdDFsstY7-Ec6$KKU-`!^31+!s`(Z6O= zr>NU4f8Ekov16$FD^FyxMCDSYnz+#Pdt>M6YU`SG4);?fei0LS?dS7g3bE`+_axvh zEgmj+Scq!FWdYatDLB!e#vXkWG&Z3il>YG&QG&nw&^8B7dfCCc=78lu7#zT#-Q%-x2?v1q*#{ z>p5E-F@_(!C|f2iMJzc}hn;a^oiiiXE-E!`EAbFwK0W?5<}#71uJ6^)U*UGNJC@fL z!-4!WXXnt$q7D}q7y0`0=i|HZaj+59<6qs~xv>ce!PQw+4H!p8VtH@`m{7w}12Vp7C9z={}&NYw>82}&xA9U)s zAIGqV7VF3t%2BcP2Rvb*@@QyM1-nGAyYT-6vCzRkC;k=|33M755AW>64#cs#q>;u!f_>$&!x6@B1JYe8b8%u&|fOn)YPpjevyTdzZ zBL49Zgw3oI`Ib%Bou|xcRlIUts$g^+tH^j~>ptYK6e4Yz;rYoUC|~^dtXn)$>{2yD z1>&W@KU_{;uLq1uxKUJ2d`-<5xPC*nowKvo9i+!E;R}(Q5xZ0#*z19GI zPouG7#surNy{3MOuxA>o50YG>flm9xHsYsyS!>TP3X{ieKl5fnYdcs-TTn^E)dt1# z(L-~wLY=t}Z6Q3?)$k79*f&knza|*R`@@?ImC}^m8gj;YvxoYfC{)`HvXb2MNqS}E zAO9Iq9_}xHs&1S?!rmU&`}-r@?(6JVyK=f=Re2vDpOt*ot%IsxOHWeli|V2VI^vH8 z^x02pCg6zLDm z*U%&VzEP{f=WOK5$Jl4)-x8#BmL23z4-IdRJ9B+)hW;+zG0v}ET=d}8{LbN>Vq++1 zpU<>1Tkc+#yFLfCkDy5j{D`(CyWyU{gqgxGW<(95mS}7k>@)Q5ke9c{PpY9U8j+i~ z!#|3Ts1aa56W<7ShXrcKXd%ZxN)8~bS`z*>14<~pz_aqm2&3kD1)?YE#YdRFS#X2) z^yRtu9J$BSt>75l0??%Et7>%r1~`mwqk!jh8NBa1h^cE}AizjC?2fZhmP_17jH9vD?O#OE3e$)bmSG`);)ayZp3EHudKJBkfJVq1?m2@j0i{ z>6|*+rBLeRq=*(k7Qq`#ZnlujImGICVQ44>-_KU zGo#b*ob!9%_qzVBt1E_i=9%rjKlf+32lqfizpik2cvuSV2$XY`UldFEf6fjF7_2R7 z=N*Upti-_tHilkD@@uoVF-`a^FE7D;T+V4EFE7eSU*WUmQ&%X1`}(pga>O$pf8#Od zyw6AIB{z3iNJ>4p7{w5!<7|8upMqEI+CphehHWM<-^u)3pOG%Bd*$ty?~fN>%|+M6 z>OUYPU)zhRAyuth6ydh_{W=%yyQO4hQyxBiXkBKVlQlf+K0RRSO?S6T{nQ&lhb9+s zoPD~!wpLkYv*kX++tbqyE!ES)9=2~u(xL`M=e@n?<-@eA*QX?8d%xc4vX&=VahZa&y=+`zVt3-x^Oe1+s6yJ zony`hSZJ#Br%JAB`nzdw)VI&tLHqjk(sS?QA3@fNIJRKP~pgCM}`YV z0tGECzGzdbt1WDH+siBHN=jT#7PP!PAp(=%~UWTi4(fS3~nm z))clzn6U*~Bi!|yhilKl6d63?`nbL^x8h+eAjvzGsGHZ5@e9DVC zH6j(C;`ng9?Cyg@u(YE$6CfxY8iag__>|@E&fI)^1x++Al~Z0$ExbDTpv!%Ak28p; zcBI-!bpZB=KeMx9YQChKkT0=ADV_01+4O^=>gr#Xze@@xZcR9l&}@eqWum4_=(q=Y z>IRH!|8I@XB+iVsa4*Y;HPzu3Ry@|G^Ow>lvc~Mxkj9SSb6bLA8PRSo)M(mq)-d$w zC#UqDoGJtKp($KBC9@VoKyzN{AJjypPQ+tVq_=QZlPs5cSUK-}wRcZmnxU(w95l)< zdgXx8++80#)zrk$+MPZZFJ3Iw^`h8awi#j#*a!4lt>iB>$LDy@L0Wj`Ezd$kb+kf` zY~vSi9SxR8eL|ott11I9Q~czhTZa6%H>`*d-W=YDtjm&SszdaC*u+R(v8V7ne|zDZ zH$<35EgT)|h_5QNRE_9%@Eq%Y7~OK^)0>l_BbOz;yzU0vsSGQJ1`fUoYtB?k=*>L< zu)1reTWbX-<2ec#fVGF`f|`FZWp0DS6B11u+SgL6^wCJ z_@n3E{qY?lN;6;Hod8_&^&hRZfic%Hdg8BL*{in5xDFRO<|&F$Y9s8uEB$?DYvf%V zrw58j5fd3x9m%f6<3lvL7Z1^;M|`H77}d#Svce=S37dj<0H!oHc)CNW)Yu`>jBy#T z|3hQ(1FS$L%#XhSt_(nUO+$Ru9=z3{KCyrZ@p0I+4+^uo=uMzspT`2ba}SWG;Ow zmy`;`VDD8vVSPCk)MQCiXi5F=Vf*85rAyCS^380eW8?od0?W0Koy~ zDg^#G16{eZXgir@TgR)fW8)WXrQ>o!FYbx|+i5(!U&R>>wI-*fuQVDbo6&f8GFg*M z%B!mrVv!5&bZVZ{o>Ysck%0c$Z={C7UYld0wi7ZYDlcL*07JrI@TOYu{U)DV%R@ve zP59;`B_*x)N>?PNQ_pB$4>zE1?Wm%ExESMBv;|mP_ueN8IqF?-W1*zds%LIR@=~)^ zgNrp?!}?I_IRUKsj{+ionR3Mo(I2Ac~hK?Ub~!`jg-zA)ZL@6vsGgJ%6@@p$?SzwdKC$h zxo}gYo&M%7i>alevEY7`~%1 zW$PaZW|dj3KX#f9#{3u#>Y;S_47rSUiikM&vsOykXAxVx6ui41_C&bZ3~5IwI8QW< z(a_yV56g8^2nuz==`SL+*hb&xz<$2^k)m-Dw-avgPO@qPZ@GhIZ}9+`;m$J9pbIXu z;b7TAKTBBcph{=T)mUHo1BXlX@0CO^E1b%fgVPE4hPtXED{GeA{{7ECfBwu}{^j6o zYoia>*1T_oEjB0E5HCglV5HP7-=ocTTx~7N{nTu}+>R^T`S2TXzPTKTdY&)IoroAcU1_2c>KUW%5gX{95WPN`5$VGQpd9?o_d7#u7#+Z`BD zK-^5bC+RUhTs%8`GRV)*jygDa4=53{)K^x3d6eqDd~pR_gMMK@SC%FcR{2CjJ4D%3 zOIY!_;}$pI&E_X`s&*L^e)GpSWN!WKQO$kJk&afl3W3q>c)jA{{@w}arHbcQrb4Yv zII~kYtn$N@OjX|i@0Kl6w=VZqZE~`>2|$Ri+OiC^WYY|}{=U9_0DeM(F+5Zrq``9X zESKp>ge4>M3nS5n4)<)Q=N@~~b$nQM80*mEc1sE=^@^6^;@%Nvd!H_oW((}|Eb7>;2qO(DM9KI`Ng&XsP~#1fqIk+vTC(uQ93l zEi34`Ecpu?0qe+AP8ZQV4hLQS^hvIFn|5rWl(X|?i+nw*XXbfT{g6=^NrTk>w1vSM zL8a~6x5HnoUw#bkb~^7y{e;NsbPje&u*!8$rWZEp+50lR|;o- z;NiW6|5Bimwh|4tcmOAns$dwbm(fL%BRO{GhBh)9ylLPkG%Wi1b_H}vKK?38$1(pM zLP8i@HNhNegy%Yhf8^@2E%z=?tLoI#a+>z$db+j2g9O+YIEG01H6PEY>!q{1KC>h+Dc30lJW?#g=|f!J2TAjgFq;fn){G&Z*cP9h!z| z@EYXZ_=SF^D5?29z84rbDxEGcYkg7&zo-nvHt_`JRqWbuSEYsvkLYGWB>}H*x80gv znEo%wXtKFnRnNdCKF%jcJ#o-x8!?#}AeP&@lMg>2$CB}wJjTV<{%W`@!nF5Fl#r5p z=N+Mm?qD@ujwH~eD?fRxl;nuuOK@;7eCk6=VHB}&t%pWlh>oH3_zI()>$MU!jTP{h zpNy-JC!Q96E8PPOAq-Rf)iNWhglean`8dcPRasgSdQR3YlmDt{j%UOU; z&5dCY{`m1p%|JoOU+Q2li#C^5G>#qokT;ZFG(eIAeS-$fan)z|u%GyIBm{a6ShoR5 z|CiE|K7*Oahd%K@0m1`p8l_p@GuX^SrITeQrF{uKY&sM`lb{geE~y&e(fYex`CWlI z;lN@dI1FKx1C`BylGF{_Gbn0VA_d!1>rVYO*~>iS9;PhL9XZD=Ei+ty4BjJWG9oaZ zJia)zG2G5U)=+$NX8|G{6}3A&g8w4bpbkm;65$3aS+oq@T52;E5lYB z;j}Wdz#Jde0tV}Ww-i%y?~AjCS~k?`T7cO7173gL4z45`JG3<`S({(v5^KA^4T+T4JNuj}|pA`A}}XL2hX+JAFk=D*RRyl^XWAUR^x;m1Nd zRds8ZwybiX@oDPeG(K~2V3BtG#f)vCDt*zGINbTEA1OW-hSC(%arh>-N$s#fkIna+ z2f2rO0ItUXs%m=)97XzYXZQZ{9ygaZJc(}`$xu2ssaYaTx|$<1gwrTWR+KHp`z2;pbkrR}fge>x18;x5HJbqny5ttaWTZn<`` zOJeLA92q3K(b!#rizo?+gYgN7&cahv?9V&%(aYp<|xy^lv)IZrGU;?mUNH z)fSdHr_Zp(ugv}IOW0R94S^vdH+EEE(d(Q)3a;+Dflc)Z+)w2a)yB=P}hL&OC{cm8G$#UuZqXIx?S(Voj$t_czbSi|m$Z-MT%F3ic)MQ?n? z3Lq_-Ntp{H3e5Vk`ISx;{>rAK^!~cIeO!MGhK|g_o7x~>;sv5Cvf*yP`2Fn6qmfzY zcM!z!uZBD#K&!^DuLV|iC|E$x4 z0XByZRfdTN7T;r}NM}#s*J}=!$A4Ys6izIB4r&NI2OJ(>8;?==AI2g{FQC5)_F=jS z`YnJVmzkrh<68=39e6TlhybT`qZ$Nv^<*U`$C#92)hJgsQB_^_fI|Ew`HmfV$wboR zO3H~9z%q1q3pP+c*b(uc=ZKjThg;R+E`#bLTLP?8Vz!j&!3_~b4vvxQG|f%y4M+M1 z>l6xRNlj`=22yDm%~4GX#7eDdV-uk+!4oUPE5sb@`Z6neUR`?%n-DvC6Dk4Yq_9OX zLeGs0TGO5#=c?bW5}=lp+!&s1nnwVqr3JNWkT&LI`NG(3mBpx8aN=6EY5;IE{0Ve| zkMOZ#AOl+0EP}i{Q(0E&2gK+jCy%P`2tF%M&V9OPc&QblLY^ZQB&;47&tpNx=VC+q zzGx<;icXH_Vx+@7_lgGj*(N2WI#971Hs=A?wliAbuGde`28aZ^y^W~#oUDTeo+B7P zdBFYK=UdFUJzgI`uCn);l|eK?kWoAK$5tF^K5VdOwqmHyV5B1X}?+)493 z62;i%|0lfLgS0aYkoxa<26}G;t*&MADRj=9G1*z*9~uSHux5_6N!tsesd9n;W>j+O z1`PrzFUG8(Er2(}%F$6BiV$d0T0;Z5c6af2MGCtL4T@#O=u=^0!Tuwms`2Y0#P{t> z&s3*YgS3$;Uo+mwZ05=8YINF-vk_{5K}~O&K+|o%PJ$&AMLnINX>u{XNPD~a_HtC+ zU%R7PJK71^YCal(44pRKb2a-=`TV$>-HWu5T*1{f_mjo$;Z={|Iw${xU)<-DYP&2dO#v01Yl&bpi&A?N!)6 z;`DE_ZwE6~=>Hk46am^aIyZ^5+B55qrF?HarWS@>`MNN%zYZ9HC#W)~{JyNQs)}=Y z!vfqobYtL9{45AdaF2b0c;pe+CM32=(|NW(1&6dig3HJnb+4kg?V_dlD*@SLq%B~ zg-fy{S}FzvS$1j89Y3F+MW_6Hr|~gvXuZcXcyYc%dp;mH2j~N|jhi+pwPiyeKJP`E z>dnI2dBZCPUJ-6GcSIhkZI(N(v4wB#FCor!C;#pSUNyPtk-ZlwqKIRU;3^{h;wzKi5qDY1Yv_lGptJRHYr|(ogiCV#}X^tW2L8!5FZ?6UO zE9LK(0g;<}Dv%z!|5jrI^|MG%)-J4l5_+-5+3wkHv!sO zXAQ^}%$_|y&Cun67lj7zNq$Fcc>uIV4@=&u_n!Rp37?m9Oe7_N@#LvZ_6b-6485cg z)80r6vo%C~#+(=BJ)<1_C{#>lpq^5Vdo^0F2;EkY>yL>DgvHN!j&1-R6y%DUC`EXn zKmp%*Pe_cBUcQCepm}U$`lA=S&_9Awd?qrW$PD_3`k6>-^unl5i1FNm0yHbDNfj)# z)Za66lA6iS!Z`m&A1cENAk+uPkoJm`#xJ6lzuZTu#j&I6pq zgpkjeX{3UWYLKyqu?i;k-=4pd8Y*s(Y9;SKa*?K0JzgW5d|iR5km6#9DUE(Eg)z6-VryX_h21A?OM{n{>yqg8zL`uJvk&leH(hdM#8Fy<>tO_ujC4w z$s0(dq&XA&Uu8A?Ha*5u{8Y`WKUgF%eTWUjIwn4N3@wSg9QlG0r(yyW1H4ZDQu6uM zqi}A)b^5CsyVWIf54TcJC=oAO?t%9@62#s`i6Z;+RS? zphw8qltsKHAfI&S&Yd3V3>%fKEK#YDcjs=7JFy|*wM*JQ-%3W5Hgw!Bh?7Y6>OsmUC&hKb2^ z(`+2_COVE)WLJ!0+!vC2;3DOq<0r_V#z`J{TyMY7YY%)Clmza(rEx;UZ=k_B4lYgA z-Q7=LX!VJYk6!Q>GxRi_RTk2z9M2d->mRqM~8e-*XEH1_~5Ue{3&P9 zQSy3Oe48`E^{VNtdAg->=PSHzKr+9QdyPj&ftl*OzuUN=Pf?hL*QX@{4U5zJu4MVI zS6B7VO*>VdP6yXZ-gtF^#J+uN^i_xUlX(_qqm_fAT3_=mrR2gd1ef7++Ou__R^F*V z6Z}jk5Ba>RLCmDM!H7wQAxs*>h2z7{JvM$AcD+k%sd3&~q(T-S8@W6^# z5*Msn{Wi-(Ompg$OX4WFos!5tQ;y=J&w}qO2W?RpyA5>f7s+yIYqqCt@U%)8=tlNC z@++F(hun(zy#H@uMVRf3dkl^idAaeb=m9o#`7=B9x9@f~5IOU4GZBfB z@#{@sIQZ5(K8`sp5i$_5dZ5;(`gop|a6v_)WLiz6M`EH^m$i@q0$I=frW`;=DEdlg zX~wHX2Nq4*lqxyHh@YaBG2oLO5PoCe^9+;^)i$AL4vYr!-Zm0t;P@UJ0fn*ml0qX{ zz0Rk_$>jmLHb4mcZ7|8urewcy77dVzPfN2eowkq3zp#Wd(3|(eV{v3WAAFvP1N{1o zyUFQphWWf<;1`juHd_hU%3317N?%3DHro!~BF`~q-j;5KhaVg%L`p<5ai@QH^**)N zw0y~kjM~=s?{5Qx??lNRf?w6gtL-r|wyI8LXtTtV$oJi|qD|*}=9|3xeMs57-WHp) zR;_eLOrud*|J&mogA$`2e{B7|)9Neu>H+}wSQFCR0)|r0USHpOtGQd=Yi8ob)T7cy zaUar2znSP=HHoi#f8m?^bRaipGyN$*v({x4YeJX1$aByqtaxp$fB96F(P;kB<^&Rv zG<2QQ?tu_v;Z`8zHJZWlo+UXgUTWn2^6n!z^-IwhMKc*zzOw9j)R zZNXQY4f{l1`d~j&z93ghIHo*p{ps4)y|;{e+@jbdzcwZyy!JZ=J{)vxUoAHHyue0! ztEDkbsA(?(vN+srKF(RQot7m5y^`ARfiwqB;k6`KX~41Q>kNY(I=0=`MP7uhEWp*# z*V7)1e0aS)qIvC&qLe9MLWUNeuK6;wLtJnc@^^RrVYf->#tLqV}&hBKj0EWwrbZy4nt z47?e^wQW=zzyl`YzRjlWi%`cVGYv6mptUoMt9Kxu0#=+a0?R)#A|0Osvi6S_M(~y) zW#K9uEl|*yPxuYx?5oWjlY|cdw+d&?GthHs?S`N7#2#M^V06xZ$6rLAqR8XIp(G8 zO(_e~NGx;^=Ib$^Kb?a3OMUc_gl_?4r9d*7i^H5 zpN>zF@YukT-vpO}SQCc#PhX=B{uE?H!~}smMZbfE5hMTM1)8N7Hl!@_n>q8A+FX?2 zIX7|Anh5&?q%zoYf>8yUsvG^jTl~vSVa3W=Sfu03-6Num?lggi#%;fT zNQrZB2HDFFoEeBf&rP?Od+HlzKF+(Dq!L1l{UeqUG3XPf2oSdUgT$uzSEgwACT z{6S@9eOK{xF*$Sju3`{kRW*SCQnIM-OiKD@M{=|xFZ)n&j*#d%M5O2w@K}k%-3HPC zT#mk6$*Bhw2W}5In!x4fli7vOuen&32mRft|EQ0L-a1g%`wrdaNVXPkAEnZ+GY{67u$DIdmVkQL-lq#vEyVT+Z0oZf1z zvZwUHEH6@Q#wEAFxz+C z5ghKpWoU^daOUtR=wdC#H}lP^3xyr&G`?l%LbgxOmrGpXHFi^&_>aRWse^z5-Q-Z^ zZ!ocL!~M?goFX=6T9RCs(2ZuJGbg}n5`30zI4p&M^(`lM-+GbyZEIQTdvsSn9{guR zfN%uK5nlcWo%^mqaS=x?i{<&32N2Udk00JM|SI>ktI72(ZF| zev1vDBFNX-@&HUoA`wGpeOUeYh>#ivTW*CtJnV@a)0@AW)qyQu{YwR-d~H}T+ZSK2 z4TF<+J8*%Ij|{-tLd86SYBBu+;CT4H+2%eh_^V3f;@h$%RIq(2eOBk50<*8?i+J?3 zS|vVuE=ECjpOH*2A}hc8feRkJnNJ(!PsoW(xypgq3?L%JCx_ZSZUWL@0Sx)TkiLBx zjs#%-+3CV$B4(HWvfI2UD_-4lm>`D9&F+KgCnP1cYCYsHN4wpYEI3%OUejEX{PH~b z-Yci;6&BKhVgYY~bmq`kXFT-5tSc3STgV=G(p`hLTT*C3;9Q3M$MS2h-Kqt=r`3?< zOJk_tYUCr^g9W-jxS~ZIqZZ~gtmO@*XYwWgI1&O@dLQ{LB9H+{;|8N)ZJo(ZOZ<~l z?$gfi0PG3&{osIOGt(Tyc_lCwNd0Zu`&|--Qe9A>KjJkqYwYtg*R?~6XYLd|1tiMW zfuUVktXOfGfc0(6zb1%Jbhw~@A!(3)fV3C#U!}7f;ToU`Z=vA>Iy|$VCBM_r z@wla2&u6{X9?0a=@MLIrH@S}&8gA5v7n_OCyL8L|-_T`T1b52v?6Eq>SI&2jTCI@t zX6pE8vByUqFz4^P)hGe%F%5^3){v)YHr0dGih+Yr`EWA>5c&|=psjPZN5@(x1Bx0i8$Is`ZTD3 zoG5c5u*QeYIRB3s48=u25D5qnEpEJjx03%{i`#YEBT^rzA4tjkw51RTXj@9r3e0{9 z`46qY%3v!Uyz*BiX958Q*AVpJ*O3&n#AjRWqFB#@rkaA{ZJeN+L|TM-hFMyf)=J3& zyqNVa%0R#mr}D9|<3Pl`k^D=nGNMZ6xVF8}n@QDyiU>4yF1QXvM}$y;|5O(8gl3aeEOB0lM~^W zcWC3+oBW_KvxL(+*eyl`qCHjo#%lU-Yq)Cmv=H7@zX5*owMI4fbw1{FN59@G^&c$_ zjOTy-_OsJ(zq4J|d@eA9vTq@UP032urW{viJNjW#xQ5*wZHtBkd4&0Y!QE*{_!UL7 zJ{P*lK?W1GtEV?VCZMV5<8q=jihO+xm#q=M)i?y6wUZHrFkOK||C@jhlelP~e=P|N zz5tjbg)h zEpwQr8vf60A3BJj=ITqPJAH@l{?Hu%W_a10?)b)J=sn}~ove?n)#y*yI7|khGUA34Ul8m= zj><=e3HCVRVW5F>1fTq;35)hP1TjWrLR-iHz3B|HI3rLl2n|Ty0S~=qX0|KU!J+#X zFg@*x7650owwl^~K=$$qJLps3j%dZy&OCzlM(uUT=aVWIZ>R~xLfNP6*$oyCDnl0aFHBuKy-#6PPhDIoZy2RazYA+0Y6!H~ zFbq&=O>s4-!66mkeL52~am#379g=}H3m_hbR6@vofzkfd35Gv0UV-&UHCnz1acW3s z{WeC{Cs@IzwSu16V!Z9?Has&;9~9JA%~86J{oG($+TYhylR*Koa*a}*t-VLGp#a$i zT-Ea56T|EKLv!fYi~>y7nR!;A0zM7;$}aBq$b)_w)5W?(rRSy>jrZVD#+D zs7r0#Fo3Bi5_9==Zi-vt^+y*clS?RP7woEo^uIrn??9>tR4#T_r|X9_B$N&@%(P;a zn1_4qeEluBjJW)Pfit-;mz0?3-vOj4G}dNnbKKK|d>-TZ@{p(0<*=coX@gAL(Y$TJ z3H*2qV1VF(gk*3i@$yVS^v&txpM+RnR`myF#xJ&)03Lc%5N0$YapR@vHc9OC@u2;I zMifBSnQOCgf-K!q0}^DAqndbR9;3C5L9sHdW<_qXVq>-^-OSuPRl-B3YkxeamvGw4 zIXlDF{CX-Fx3pbdbApLeWY+vly1?vNn5@R{Y!!oQ$YY^{&tA^3+Vk9iL~;tLM;@jX z8h?A3_)~7fbhmp0x8H^5z~VX%iiCi9D}5}#c7>XbPCBxIpz%X+2=$s*SF);@xfQ$C z`-83RPQ~YncL7dXD%SXAtBUp4ty_l>FyI<+0dc~p=yi#b&yc{qrK?D>3rvRR!93Zj z4^yeq{{;H>9{I@43T`-G5IqJ1C;W*AwxlVgHajqOUS*KY0vT>Wv8(3hPE1rt4-E;Y z1Md({BxYteO}P_#-`A}aKpC#yb&3Euu=o}MO|3SiaKV8@J9fkPN7DJ<*)1%q4*Z!L z7^`!EHFV5Q3au+Nj7E+jr$?{HjgaMLrfE=o{AvKIL-6&_KiI2a zS<@99+bCT2PXh@0JNRPDh3C)h2;K+6+R2oi{ut9F!lAdqd)Wli`K8#{fUfa9A7;VZ z>IbKRg^%EJi2z#|2o4e0*KB&QHQ_P*{1o1h(4UaTw**}qzK}VF{oUWkf}j8GP%T|{ zg8g?-IWaQ$>7V!d|9H@?yCQp+^!)TR_87H{}lNdz;^0H=l3cwHTsDtbTWhg6y(6m6Z&0rns}Ehb?Td*&+A=B!Vj zKHXP*zukU|@#$=l)J5SgrNSjnQcj0@S+W6HwSiSN*46gT;1F>VYTM^Ula$^_z1ngegW;YQ)%Yi9@ZTsjwKm@pu|=YA$pt6G5H>-rKV$T7P}taWwXd5URW1nF|^F8HKLk;v+jg=-zu z{)0MrNYP%{xC4=6GWv+XZ{Sn$-otva6PqO#MHna{Qiwn7UX7xs9<%}g+7h#8rgXjMdH!wf)gvhFR0*9 zjQpT@oCVWo3H%o6NDK4!!LdM10nYDAGOHY&h6Nz7E}=^4@VMT}8M2fUj6Ak~4u@R8 z>eyDdZG`ZPigM}@K)&9?SrrS$U({`%$)7RuaTxpRv-M%uf&=`hFpt z|Mzxs+C)-FIK&@+V?zn|(!XyU_j@_t=^uWC&eHXL14Ec#(L`NNl}QROe-$!7Dpfz+ ze&RCCz!ly=fHAqhJBFA3bN>R;O`%B~Gyl*18Yr;|+d9Al2jc)xeoB8c66>QuUf~5B!pl}A0ska}r7>Z*!VtWO|L+u3>SReIU@5Jg>rH%AQ#Fx7QNa->(3X&y z5Tj{YczAdnxG{jZ{~rL}w4g3LD%mQZx4%naYK>D%IQ%`R?>{C%Wj>rsX+~rdO5B@b z+j_tX2@p^eC9J71>n|~U-SPSPEF#@R8}7( zxBlp1$TY8cx^nZu*eTC1@-Q7iu5mhXJ!OE)F(l@(#|9=C!RLRCoc8Pb`t_w?U$u|R znVWahU~f;2>3IGkKu>w~q^r=a9U3Y02k4Nq9aCj>g{txND`ROsJmRXD9HB<2etn4L z@1q8)l0_n-(bCo5{Zralva^~v!=nz&&n}@3El+H-Li2|tX&yXDdo-{5jz`=pVBrnj z+5y^oh}UYVVse5KGEu5_Ywd;^fKb{AjRAeznXmVv1BCnjADn{mshduYs9U~x4jy*! z6@m7zcp&EK!DP&ArXO4)!r${0J-{f=*kF~&a7&ld2D3f3dw~ED23+^Bs3h$@swm7= zF_*RA@7|A6JJB+Q(8kF?*6SLdqiLS?9i+8vzUKpYEl4gMFP{W|u|2ZI^J|woebyTS zxBkKXy}81$gyGR^Xcy%ONL2FUQLr|HTZro52&ayw%7c7PJw%Nb}Qt`aFr4c2|I0kfZ{e_dk%mWE7)smc9pO{*v&RI9`Y{pe;#V`LIa?-u2jv zfrMlQr-WUQ%qbp$B?bwP;s zW|J-Zz2lYZU8?b<__NiV$shx07MQAkXc^(mUQOW$mTS>v%TpZE#WbXHv0Uq*0NR3M zH5fSsl@w;yDLi{ydY2RGa&gfP+jAeBd|WHVqJ#niM5C6Kmzo8-jua`ZLrZv8S-ZGQ zhRX~M4;zj$&783cJ|&6IhVSa2P-*1PPZlA@8v zAPJeR7Jj`8F?h;;Fh{F{T_#b@OF8;m39FL$s8-1WS!qL<+!mVd~eN!yGb&pZ>OWtOwT=K$F}&%j6yIk zg+IMOF&o{hx(~|HuIkBsNQ7^L(bJ$A7m5h~J%qfC6`Q z%`Rdz&10mn5{Nsrv1R)*q?Ec__nFagdoYG)vITfK?q9fJ8cW~=(rRQ&{MmvF$JMzL zHQTJ-fz|*=YQ`lfH*$Avvd!!7Kg`OlI44_Ey_?bCzT4h|RxmM>TXcN<(|J|UKOqxf zta>i%;fpb}<0sP&6npTog1fT-Y0Oxcwf(w^ya{ZK)aK?sK}P#XvCw3%^E?Rk1nCgg z5PANzld|54cTPo(!XNuGi#o+Y-~t{~dn98`7_g5uemZAhb7R^!_f*#x5tQZxFlH3f zA0|M54XJGQN?FM^#mh+hW zlE)~{2OosXa9biQDT<^~o;=Pi>>PkCQ%9q{2D|;!neb6h^)el!s`K-7R=ebsWiZFk z7LiLtsmi*zk-pAS9SEUSN6MrvEG$SGS=ei1UCSY8`!cjwZ#~%S7=GQ|um(y?N4WLM zwkHkArk%z8@4)LonmKUgYdfUWK7t(_9JjyL6=;Sh?j%NoIfNHF+3M<@3Dx~*I}T`F zg@1n33N$VrJG0V>VCeP*rZ2WFGn!a|3^0KDhnTW~cHx?)tUd znX;NPSa<9pe4ycFMkK63_uy?eib+3iRZax~sHe%eu`XcYoDN?ytEDPEbyA!pdDwea z$A{A5WU*b}d29=h4%c$vN%__Td?3PdI0vt&qQ>bFP@~pR243W$S+V;&DnMe24r;?F zhL*zK8CVT-uqh-sSAy=6Y;R~C@@u6THR=)}^2QF)co+4}LOS^7&6Xf7AuCMXsd4>> z*nYEeLo6WSrGX##ykDqmy)w9=Q{+6Zqfi48^e#l1-|PX zeBSfJH+ll#PNTU`9cPEVU|w7_*1z5orSp^ECdrksnh6OTxI-A-v+}1y&l}_mVSm$W zDGUhg<3%xN>_!GaD|*cZ*=@G}|K(roSpFaXCGdPV0^t4^U;%b|OiVf-QOORVf$DMx zg^dy{7iU+KR#5#HHxT%~NVf%e!iE998zb>f5UWF{l>&h{?DW_`B*2feECAeRgh98V zoH#^MURr{oi`6l(01i;j+M9Bq5iRnM=-O_l)9 z@L5P8w*882-#2owjiaA-o5M8Mep%x{c056VKL@)5jB(!E9D}wiDQ+CO3vd7^ zP}mY=>jaY3s57k4Yj+Q1r{cClUJ)h!zFEOS7Ew9BpwMw9i%FiSB+pes6^2o{n7+m% zrouEZwuk3;V5MNOA?DeDMY0P!j&Jk>Ac1XP>PG4okO`tiPg#g$JQ2uWs}nVw1W>PR zM3bGj6G`ZI>hNF~^kzyTF^M0WHXyS*-~>DqC+MGhfW^gNgnIlAtM021Tf-xY;S%%HrtCq-w0eR-ov~6JUs%Mc0R@SLN5*Tf1rs5^ z2sssuDu=Y@v@N{;`>aN8d&M1R8z9y~Y%PN(>t_pdZd}W{6M=cAry$D$@Wz>vYwKko z-FJVX-h4uV#b5z13|39BC8U!-x;z2hjk|;s)PoQJtdYuA#{-L-dYe>~Kq3e2ALf}r zF7oTk=&1nTnn+EL-iiD?;DkKm@K9GGg=y!DWZiyvg=5;1DjHf1qPyG-ZSHH)bhb2d zWTaG}F$lD}SR@00xjMIEpEV4m{~;0GD>BMQCe`kXC;rfvu}4&7p8MMitq*$h3MJaK z0tbSp4y4J`1C6Uaf^qh|tAWw))u+lfm@O9~Q70cIL{0zipjs*K1D7dJ!H% zat}NOy%6zMs&uH;bO|wOR1c>Om}Iajmq}rj^m`e^ZQa(duyZZ^+%){}i4891=sK%o z#LcTgxp}ZNTVW?epPu@$>ADPZn&`N+3 zYU2Od_vnQhDCR%ZduuSh9GdYzMI+A1p(C=Jjqk>EbY7riE%KkZY%ca1*PWEOoH z_6Q1H=L&RS%rA{~iz}L7eZu-<5An7XY;v{aq^PRc?g7Nt!Mj^am|F;6+Ia;wOOqV6 zWmsH4d(`+~e&~93%8dHGTd3>h6FBor|tX=-2QzU{@3z$ZBnF~>dO(e$YxmK`= z|8cB@D>p@W@RIO(5at26M*$0DIg75TY(;nL}_z#MiL$*{oXT2 zkT!BE*102*OU%i8(%eUTb_%)iv_vc%$QvCZgDZO`f)%EtnxdQfqc3#^4JoI+zhlka zhZb~Do10kjNZF~bv33_jmFHVzxY2FE&Htk(^a#d-k}rM zQk-|Coi^CKIh>>|DNw541p{~n5v8=|P(Kr8>|vGH^KRO-FFut`N;dHUY!}=W5inJH zo4pNI3vDa8Gf!En11m;gP!|#tiWqtbvGq*wm(-rCX2?&?B5oJfN1?%k)C`CIu|xco zg2=10><(ekKgiUJa>Mlc5C?W*FCm zU3%5 z0(KWR7SO%!+mXG};I2ZOTVIw1{3f{E2e90{>%zn=AQ5fr+@{`FuoKqYoy0qvR8kr| zHGLHJmuXD9lnjy1E(VMKsH`B49$GC_59wH|A8GDG-; ziv5B7iYFMCJ;G<6MP!YAP&;R|Ml+4QrKNbR8xF{~9g&x4cgpg2AC;hE7&>!DL+qZA z26lult7D)P>4>CV@4v_1F3-gGR#-<+)`THP!r8Y-@apPhFI+b!=evuLWMvWiDALxS zrkpeV&Axll(IbN`UQUUNVkia4e;pU76`^P5B$q(OTSa|PMk}`>J#YM1DEM_&Vd5>_ z2a)-0fZ@}n19K&>+~<6|K~D66{+d@a<^^HUvX>tM`#4pjRVOGkHFl zY&L&POKv>&pqQA`(d`%CWj|lDR)~i9N`mA{LQg}~^-inXA3jgpdsTOx*a9z8jQqSE zUUY$kwIM;YSN4ClAr;uOO^6=MugEhT&u`RiPKlK|Oz4qly<`d7#*3o+;Jo6X8NYCp zGR`6%4=8ZArqBwU`s!-nWYLm?yF?Iwu>4mKqoz=U_?|QB4{k(=K+dttCV|ZdzF&2ILn*TEg`Z?h*~L>}%*fgOnCYc?2`R7PZu&acFVo;a0n)J&{f@v}$xFLK&UjfxHGV-J>A+4Rt4r-Cceq6ZH4?lta3lf4mJ(xuXVz5OyBgRmE7mvZ?9CS z1Wx-o{Bxu@WVQCy$468&f3eLXW_gnv!_&yXXX$~Cjy8RY0_9%b3(Q>A7_HYq${x1i z)xLlKUb1VTh|1bDK&?rWqP@3rYCXOFn21%%3Z3r!tX3NyCW8JRjYSIT-d}N$}oVFPDZ}Hm17pUyAuW>MN+PR)4kk7rS2u=H-lP_8kfWLTCVd z0EOB(-wkMN^t}Tq7p3|vU^Tn1He7zlI5mDvPRENa57wKRT>EvYon{mx zJ%1(cCECgFj26>^m6<*^aASrK_gW5G_1cdBm^$T3GxaovVa?Nhsxx{dae}xNHg^0!jKV^-lmQmA zQySYI2`lv|yD}e$IBDeXmyV+)=`i(}NoC18q x^`wLGSi&bN0}zn)FwVh7wez| zSt;UbdOzcE%fRkQ0?A^dKy`Mh6{XK!vpAk1WYkEn4(yLm*!zr?Jmx%qe~dOE6vs0W z0{^3b<~7)!Y(l>ChRWG=h-tlKdj^D<{s zyNY{D!4p~rDsm{_dNrSg#GW*3=}*ViXN#oyocSypNAX~kOTB?5i zif4w&hE)18o{6>)r%*rQHl?~}n7K+QvjOr&2=XTc2=`s7qqCMQV5geBI#^xG28E@xWzEug{O);P) ziugb0mTr5D@dB@xw=z8~;XN0{_$y!fZG^=kM_X9J{K3i8PI-?SIf+3c*4M|S4b)Va zGvQVk@hrvZONY~HbS^u4>%l$keeOm%Hr{Js({$}minZbj|4{pJe~F^iR)y}z!pwvw zDh6}?WnN5VHS0`B2Q!#7o$7&P9mBdxjkNx^m(QkYrS(^|JrbiBHmZaa8a~-XL;pp& zYwb}hO0xfjUQfGrcSi5hSspWg-Uu&Sw-xJnTU(FVO4i=;ndz_=W3`fmqH(@gM(1#a zSId{!t`v?UPIcGPNKD$a z0@qsM;`>?TRGPrV)41W%;y){ zg(>}6nlWzNNv7OMX#*io94NbQ_=jrS=XmVOa8Hu{_EdflJ#HiZic3u^RwN`}?KSZf&$-KE1oigz^V5yXYsN{I7Bsrm>9ONd z5nN9t+aX1x{d1GUh5Q7CJ>&#)BbHyaT2cDu_b!&YMm08?)Ki*N>hz|YwZv;TH#xg+ zR^baesw^sZN>)_<4J}*fN&U5?a2lL)Zy_|?4H-$*4GBm+Yu%XY zy!MF3DkZbxm$^~QEz5BF0SA}j_%y7ABXV*dUNz`s$1ph_ttilR(# zR9pNK8&_=Umai=ko1{_q@{|SR8KU8D#mSY_4}32yZIh0Vt0a@?FI{G5xGqScEW%cO8?RBgmTyZz+UXRv!m zb1{RY4O1k1b_ZHk2Rr#-_5)ML;^LihJ&*3aAVN|%bpOHC{`}OA4pD3N2fHA%Df^>Z z5}NsXk)CiaIn+JMm7;7lP9%HZbSawY!omywiJ`wLnwHX}>2n*37;+<2(GmB8w>Piw zkgkuL2*BAjpnE0e}vvNYcD^6a*IA}SyuT>}D24I$m3ECvWD2uKM?Ne(qb4528Xq{Pg?P}0)UIm~}O;J)vE z-|-#)95@osJaOk0=XIXkIg1-YfD)b9LmdaQdDI|SuCr5nIYJ$C^AJ%{fDr@Nbxv_{ zr_x>u@gKe_NG_N;M67{>F+kZ8m0$zf0oZF13eTkil^n*BGLz?@5+lPKD^5g-Otx6L zrH~7LL2J*7D)dQYqaz$1mFkQbZ3cGzZuQeaxTlxPF7@-vdhSMGSXqtpMJCvYUT! zt;ISAyeVZPKttdRyqoL8qHWYTBE!b&w5xW@SphWuWva&TJd_0>b-2foMDn3F`k!>@ z)%GB(_7q_kuj>ZJ54@S^5BM2Ee9KW2)n5H0Hc0#~7oq2a)AmU~sPzi6 z^40pv{>23%+_3Fi6!eeO3vZW(A>c?*Kn3{uPD}JiLRumf_&MJ&uTmrT@z^fXw|@D; z+eDSQT(2uG%R;>apbQE9RZWd-exl%MZ>GpN5Su%0 zFgPZ903sUaWRr6du_ne^t*Zva@^WTFLjB5y6m3GJH89eF-rbbKI_JydE67by7? z!15phGP+I@-r195XWnD3lAs_XF=eCRk^mJ5M~#dD-jd0fZ&q068aA7w zd^_LiL?_`=Xj!x3V$>6CPQ?)l+0>WPQ*#lV<~_$R#21|AzM>}3xlG@sh6az+4yVU?&0_qgI1}P~t z>}c;}y&=yx|4tjyQxokFSk51ff&iOgd&cK%oc0g6oY-F&>vwC-E>jBepSo+DqS|;ZQkeOC%8Mh zXV5%<-;Wyql?BjA!f)<{zF~y{TLBB-ny6yk{2t)Zys&Zg*@PQwJr>$+^EVd>hnB%$C zj-e~)$V?o+9ZJx^_U_FH>7YB>U=p8G$S%>QF8F|-&QSfFOr(Y`}p~9ldGEkBae35TmMIis?FK`!<{zGA>;>c$9?RztFs z1uF2d*D;yKgr-i6&7=C2$LXV4AJL?1%Gi8n{`@LqBSvF8L-ltOU?s9J=p&75Z?wYJ zy^io^AV^tTC>lOKik;G~lBQ%M{gl5!p0Wv@R><4HF!Rp_&ML-{9+fOpj}vmBtC4>G z_E?kpFq9$Drz^p}>=;6H2Am+%`#0jhIG~;Ul#1^V5^8xQD_^4vTrzhx#H3adk$CWRN45!Dc;jJElKO#L)zXP?q?X8BARr?aS2Ic znFX|J8aN6^x~5cfRx}g0M!YmwMnQDWa9{Nwc{f{^b6}pdNJIMfecWvh^CNb#z%YZ< zaF74U7W0yZg6gA4(Rhh;9O>{6SV)jh@U-0dTWl&rE5^R3U6A0*WtNj)nXlV>4O|3p zcy7|!tZDH4jmbG#-AW6M{=y2vQ?9*_q=6azF>h`vR|KGvRpvJ9U1w!!jyph2HX_R7 z;LMy6f~-^l?`&*Lb>2uLt^LnvKla5| z5`)9I&UPaKo#3})NSopg#gTgZlr=q7TGe7d>OHabz9HAKBHv^pD3)K^5E3iCT4UVz zicj2A-60ZM{Ot`4_ftkTgElU@PnWCi97^EcwSi)@edm))~tIq$JP)ria}O zOiED+xjI-P(ncrG#HNgClRAbwre{}=SvYRJsH49QZJ;9fH!?X5UTgowSBwy;vXh}E zh=3qEh8T%)haf62{noQerAm5*r#DFCiy>KH6x0f;@-Jkuv^d05@>v(6cV@5@%tY#e z(3WbP-D(Xmf*@t>$jknL2&Ocvu3i*Anwpa*@6gs~c0|zbLm`DPzdCI6OJ{Az>R(Ik zm6E-ZU@E_vmEZeV0Y6{|A9o9+dA><2?ee{4URBp3j?3JX+b|Nrz(j#{Uy3ZbtZ|`+ zU&jO)OI_gDyZl=&aFYc)^2^?2+dEO!8NACD_{|BwVWURgNNdO77#6xR?)rK4+Ok2C zsNg6}G=$a=v-L1^{K+L^7(Ds=)hkP0@mYBl^_!L3Ldo6f+ZD}8K1}vJNi8voA~}cE1T`9CtyKDK)oe;?XS`=2u}*gDJnvd<>j;9q0R{0 zk73u=wo^3(VNrxJU{6Rfv1<=|avG`o-7V~6CwAPBz-E+`2b23GKY^ne_Hu#z{%3w~ zc@O#ecBrP99}3lIYfl{@=tE8COjrrffKyg6I|h81Gw6KX;=sF+n47=v!3gWT$6({# ztqFPn3Xe=!_j$YBJSrHCvT}f-{$F*U1Nw+s9pv!an01?wER2o8zfpq1AZMx<;qqhO zJPG2_B>6Keo2HOxx3RS?%%}$~I$&x65SRhv*AWtaWp^@EMj}<^;e`kdJ9tH#WYP|R zb&;P|solSz>Nedd*?e)uZ9Tqb+mncw-~{@va02eZy;;jB}X*;?L;>{!MT*72!9`>4}u8oeEd z#07TT<9)C>DsSF$bqwIxwES&iB$w%i`$KCltIOoQ?q;BEMl7WF^sRnaU0IMK$!egsJP4!C%B=EpAq z`9UGo2Y~EZS_2&jDJ~<`Zh%Fo0c}D;>L>Nkd#Qh;F`qf-8cbFeH$p1--@Luw0JIq- zi69%kv*<7F+cF=b@((TmQaX?l_=l1;fnbxRlJ7j64^-TvWfqoD*@~Py24vhaSGNnO zNvENDiWlIP)6<4NcK*8vd8itLl2uo?xq^rhnIG>1cco^2n&(OrzW*}MR}Z(zE!pyM z{wYAWT%=`oM|4q2JAp3aFWCzh)MDtdGaTl&W*pWca&V&pi zHFyCj@z(L-S46lnEmO3JkRXEz$_xLk|FsfHqqg zN*n+f3<)C*fKjOJtk#eeLg)``?qdHC;vXdOgQPB(`~L8Uq)?de0nqOu=v?ph$txuj zE{T;_1OpMkX%GY)Zj{%RAmM}S#Dp{5R!Au(Qn?tpEoSJEUJ&$A4EDYGJGe~l>vwx! zs5ogSx1n05Hx^4pdymfB1hU()UNUH$&Z;#*+vjKJyBO$VjM~|%C{K4v*}S%bM-Y2g zx6!1N(Bk)-MYDg>ee`Ih&=|Ky+ADGwVVrj?9Zb{eLZy&e&0x# zkH9X<#Kg3$Tjvn>>b2>eu$k&%#E}WO8q~O0psUbP|j!?Tk4^k{cxBLC!tYrW8k->Fy z-!1IA09_yL%d`Uu!O?!E8O+yO1pZd?3P~N zBy9}#*@MP!{0OIOB`eb>o^&l`i07T$qM|(YYqqMW`P3hYRLog6ii6iouYWFe?R(5} z(v6z8{%bWmkvEhV=IZ7Q>{eDDF=>U46Xw#{6~ULutp;(eU(pUt?{c;E4uFx8?g&Qii@RT@h!?s>w_i#8?Rv6tyu^G$ zHOjk3M zpmZjcQu^70L*%A(KzuZg{tyB34ZoJIG1(i4`J0Pn-2>#i(`yC*SqDc)JXQTl`NA- zhy*1~F48sQF#H#bYMUmu2lZ^xhQ z@|9awTlpGJt(>FWbe|sZcPX`RnG>oJ6=EWx1jULbu*qT=m04pYy7oux*0yerCSW87 z%x1(=^5Zwo!X<3!<4F87W0Sf)m-g#)$>IuC-dHea$LaqTQ;I-bx?6S7H;NI^$@|qC zOBD)*%tE$H(Xu6u0&%)Zc2o9)Ssi`a5Wn2`b^}fZFQhDlKl_hp?Ke5EKfZrVAhIe1 zgB|(k9K@M9CNf67L)a`NMCI_q-f(clcLWgZZ7fb^KuqntpmEobw~q9^l@+t{OmA=b z8RFlj%V6{o%`9BVW`?t`a7FteA+I;3(mUWb0adl(GY}9!1f&0=G5i}oTwzr8wz2lP z#P1>(B1#HM4)#W7+K6OMrW1&!BD?HOwyZ1kNDfeN-m~9oewr&dx`7S2I2ZH8z@7{s z!XqAlxt|@DK6{!bUfNTcw}5%pYFfQaoiN&W0`dC_>R|0Y<3t;h4av8-QXkG#AS%VFf9T|lKmfa%G8>&RdVNy#z+M(Axqby zUiUO)w!Svns7!78=A*q0FzM|B$jGx&5y2RKMLPq}%KKnOHgef=%>z?a=*jf*_lS?B zcWl4U$x@XSZ(~x|YLT&WY8ArA_%#m3Yck~dMDZ6bkL-)bC0kh!qSfqdG*2jy+leV1 z9jqKP-q4=#LNb>X3N1H63?*qCV?)NrN?p00kB@o-bAWjgBo|-s%^Yo#Ts^qK!RDnx3ixdo z=F@HnpFTilujcYS3yiICaFJK>H*A?$NfDXi#-AZ8rsSE$*Z}9Exzcgq>R6oNNgc+= zL!u)%2P9T`%La5!Hx=-zm2J)Qtd?#!oivId#j4Fm%1RH|YH2l>rV!$9*jy zU2iuC<$LafzV7VS8>%;f+m`6Ot7Z7$VkX9Sb{=BFh_WFy{LEJZ?to$kBL4;H7$Jt< ze~CF|WsIgqRExRZiCpR&?_zDT7`dV9Y!ebY(n`LkN-5pAW8>KShi3&hR$gvqWm<(! z>Vo1PYZe5Zr>Z|z`}o~P>U8}7#kIIw=De$p*|;Y*U6$tr)~iGb-)=mYWNuVWiizoo|6rHnZs4^*=31>6NYtD5G|4X{*inPc%2Fvx6{@vSD@A ze?#t#@ebvw{H}a7*RCKpa78KxQ_4*XV}bVy7?D|sXXAn2Nk`evDRGzliWBNqi~(s3 zb&Q}AG4X^#9Sxvm+IkM+l9*XRle55sIc03~$|3W=N=(Jw8-wXT!ME=-xwSh1H7iVDU|LxBDzZ;VQn$noeLnK=h%d4R2+&g3@Y}` zfE7#cwC+#JMFft_j7|tihzJz_6}2Y@;irr!QO8iXfDEB#dk`Vrwg2E^+0fvd~t11J?0@`qk&Sqec7l| z_ubroO~&y1Lz*hP7Guxy#=ODeMjffI+z9c8^!Nk_Gf2_MeKcJcK&QfH54e*?BspVP zpHr|?AOMj!AtK2sy(hEO@t`$hFX1!SZ6Xg$V!)T~A~`FaiM!DAzpcp*1ZfDhkuXJo z1=TKUuK*6A61V%Zw$w(&BIU7nRp%?F1*-BTtS9#w=xou%SnO3OyX-RG#VsN+=D*hG zu)LVm3dT<=`DG|(;6E+7qN*tt3lZiw()?Ce&loEu8b9gCIs+EeN`AbbUa1=;xhwff z!+nGgQq;%PCh|X&U5GM~K`8!gFLv1rG6}xNplUu0R6NGmHvmQiWWYK9P;^?SCTv#a zq5%Ur@FRf=@mQof=2rb0Aq?1CkRIo!FjI$wICnu7m;(MxxT7Ar_OY&5s)Kf7 zV!5|w6HVGa+w-aTSH{2K%-7?v!MRgOY*3- z>13Mp^KlVoQRB&9Y4Q>Ew_o1VV(Z^-{Po=8W8vzrOUO;Zzr}~4dYY6eA6TAOj`da^ zET{-dZz=ArdgX&b&H7lK2bz?V#j&t@2h2h)^1(j+KD*ISs=_hed?3lfbz1elxIy%W zU4+KqcnkX5!J@MC4OVK-;ncdSA!LOPBnMAH@Ge%9w_p)sPItYb62C7%+7s6QBZ3!d z^5m;Y_Gc07ZpiS?p8{|d_P3t>W^Nghgh|)e9|J@~dV{VTsHYv^=Q4U%gq*>ace?+` zy{d*3Pe2Im7G)x~jhVfYYpi^LHg@)!G=KH$CF$@e5-@iiwbH?hOH11#28bRI&eZ)F z@_cI8yZA&@K&()z@|6Ws_P8lryf>`8!IiQri&*{a25@D)$6bM=XKuS@7X#7D9}H?8 z*ZQA`dM$$aJ64KCTN;A$DyWYkkFDg}xdAfKyyL`U3hkow_O^g#hUd79>~$gxM>F5q z<6n?zXH+rr!gMK~T(Gw&8`z1i*dWD65YMaV+s=4KPqPZLfq))XfoFL%(B*iXjr>%v zi^$Dy&stP=|CXzDPMqz{$KqwrJLL***7#@Qx)+M5!SHcvWcLkO{J%jEE%*v~b zKF80c0g3a?I0~SOu5=Vs{A~FCM@RrSaZ2RLFT6HeD3WpGhOt!W&(f1!bIS?>MM?{) zSAS=6FJC>ut)XP6Z zdjnmHl_vF9MrVa?*!r; ztEUVmV8z~o!X5r>-a1;nZUV04ZH=9EFB}eG$efSL)|QRBI2m(4uyw8|@0PU$fXZtB zTc^aTDOK0lk5@Wva`MpB!5(4?7|4&9rov!!b;}YBJZC*^>%ibDNQ9obgR+y|GX5y3 z7Sb~HiVVKm=QXluu{zr zV=@KU<;RDSKNJxg(aG9?Hb$zCMPT3rxu*MIy|%^gZ_NgkDeJZZ z*Wbi07#O&swoDqAN>cOWO+`3i8U-do4zxjKyc8OqJ*@nu7|r{ zv|F0>w@>l=^@9rrwTDoX@KcPkF9m3)NzZ$$ArNMd|0%ki1@SIms6iNbE$sexEDW@} z{buHp{y$a=H*fV(zzz+qLF4AR{xuTw>9oXWo1uhF`CbBt2{2lVd!DM2AmoVi&*Vy( z-T7CVd<7D> zhCqxhW-<-+J`f4hcDCgL@`-yLn~8%PXZ~jtOm=|xAf7C>JWMBO1!_T%w1z4c2!lnK zVS09juL#$HC$8>;b71@)2}9z)3ZsxP-Iq^zJF8uT2jsF`u_ZqPVjs+s3Na3aPaf<> z!?f2lQhlVxJP=?qdy{)bTwVT#0B|XTi3aNGu1k4xZ%^@yVUj?;R!{ZHu;01YiY=4Z z#)I=+n&ULzSMbky^7G$XWG24~W)iIUtR=N9-+5{_p$2yftG&x9?)X`my=SZ#Zg^rW{wbjH{SnA!yIx#y0XX=8W~h?@ zGNL>q^IwzvCu^pmzLlTq_bKHcOG$=JUo9-unF4XGSCLSp>J$sL#O8*{IwQbjbiiE{ z3(@1Rjq?OxR(qKSq$8XHng7z>&baR+=Itj9SeJp}>@AusB#kx=;4&+>>dhO>mjv$g2Y+#x8ihyf1^zDr9)1k*knlKs z+DA@1^Y`lczs_pzH%VCf=sjRwT3(|N$C_+j*d4*SZ2X|+s8(=RLQ0A$RHK}S-kbn- z`jt5Wwb(|*OyQ|{xmi32;o7~aV&)sil8o0-s=B(Cbq!}VB{&*1G$j^$^lW$R#DJ$Gr?#%{EtSw&)TiU5nN?-|g2l&&oO-iM$<$w}kY14Z&CzWtFkdKj1=7Mh2st z!_!Y-Vj7wfuJA-@m=@dN*UKV$M@L61#pX+ul^|{zd{h{-j*5;h}1s@?}S8s;m&;Dw|VX_-sH-~BK8##m&U^i z1SWeYf7k_)mpUVtG3lw@(2<_f5~VZsY%-d%5Z741cOPU=Mv~_r^JX4$X~I+*a1|hVcTZ z%6Cih@=EYuU-R>~Dvp%W!RqW9N+w(R`GSA|zOg7>Xht46!;-N5wMyNy%@q)VtRAV> z2P>7vhP7^*r9Ea&$_bQZt~-*`1`#a#+#D&>s}CaQ zEm4B*98vf6?{6_x8_xj%Jzxb(ju$-ZGxI%;*aG(wa0;t`AWw71=*n#Q>){Uv-E~#0 zX1{;!4x*#o|7#H|>zIgmPI&x}G+_ z!Q&STS19tQl-csm%w^j|^Ql-C+gPJJKdqP54iy-X$tQl_e@{R9+E9T&f>dVd;EXQ2 zX!^4SN7TtvcLMFEhlA*BZ0w1Tabl>-2Uj@H0d3?&9&d$!@ zZrtKVrJ0IKBfW8#jMTi#&AX~s9uDVDzhKn*O3Rg*Aollgb&@M;3RaIMTo50w&B?+E zVDw}xc?^jm-erQDcva9_^2J_T9x~QDpTeY;3n#0BDeDodV)ojyNu;15Q`9LoU4a-) zuG0t|!u(_OviGyK9Cx$8R?1i#R2f5iwKC9K&(t1JF!#Auby8Ww*t3NzbGr|@jjr3W znN}BvxAaNMr%s)66o-pTV!IFZ!o}YSxN26e_TFwx42|zP>oz`4B0lyJz_hr<*vEIL z#QV2r%)7D?jqSP|4jpoxVy^kR0YkB~^L$H7*G@OCmytSo!L`KQ2CiEd9|t#K;Uqkl zS>MM(3eqP<^94;>lT9}hP>3brZPNcOGOvRwJOC2W?qcUWgBQl>ZflC zzQX$YY|X#R%2GVj-ybY4`lVU~HPfVe`&Lz0eM3W3eC!=_zKpOr`>gXBJM9mPCO*tO zpRProk1O9*CbBI{szZbHpR0HySGg&b8gFB>@glZKv(fksAG%3#s=MqCa`yU>O*^;T zZf0vM^^x*ZDr#!HEaB+kMyL>cZsjALIzYVFv(zPwo@0#tD+bV5#fcoW*bE> zsf!u0vF5YN z4kT)$ctkH5_?*UTEJh@JQKWE>T{sYUwa%=tXSENHzw4Ga{jE=lbd)!(fH8u-Wz-oK zC_ZMywS{#dPD{KFUDIDs*oCVXuyg2p7yZr1v6MEEQil;07Ny<@sbQ_rE_I(He0A_k z&1z;mu% zS@};KMpteHxA7LGqtk67}q3t-bAsA^PIL* zcWByv>rz6vV=j`t-TWLG=~Lp&`b!mMM69Uz1X)A+YjQH<*L!|S{V(a2y6sNnD@#+i zmyo_uPu-Rlf>H~b`Rw>%SDDk_R_jW$MZ2_I#UC^q$97@-elX09zrD+qMmN!lzUQI$ z@cnkoJ@ylKa#IR=aLT&EH=?d3IkA1-ykq|7+cgn8lV1AM zyg%BO+9XeqlIveYBb!awdX)^I^dj5~cu7Y=;Cx7Bds3x9u4AIEu_-S*SR7ZP zi#DvWPvZ3^Dy|PjeB(Up=kh`MFJ%Ob5P>1W%1U;dwq|2} z?&=AQudSs^q?^L=soz9-%Z}7c1@K9v+)xNCo)!`u-N)+C^>~~~^hRn#n#U48XwFL0 zXy~Bf#xv`dQKzEWgJVk?#v0V`cfS;`CyB1j>vP?GB4Sw9I-W^5)03P>k2--YkmD96 zg06(9vPD`N_4^%@J-lrsp2SwZiu=X-_b)B=Rt;>wj%=|AF)1xA&8ezFy}WWOY{UcC z;}&R(H#NqS;_Z45Is+F+PET%ib#*2Cg=32Fm3VpK_!`<@|Fq>o4>nyA`1o(JuQc)s zmgA+zF1L8iqvlCt?`I-T46ZLaCQ1~qS2X2Bsb-2^y!fsk?+yoPe^%r;9^9boCS2`b zu^4u6F_op|zDR7mlY3jt+IW4a=>aTf#Mc#iMZ11-YMcM$1AS_hyRZ6}d)L^`Ue12$ zJaHnVfpAeQPq2Ez{eoioGiwT|hVo{h+f$)1jGlJC1>KK_mFW!Pb9Wq?F2x+-zTLd= z&C)<4$m!PzKHg*_S#iAWtyYcCv|Oiu&*`+$IMfVZh14s*){nW(fr=g(m}fPS6;KQbV{DR0KFP|eDJGSkpRej3lUZM1 zFCrwrXux{T1J0Ofc33uc*+E?nD@4dQqqSGLbbbBtjPi+&E1{!Xp7Ol-e;cqEEi73g=%?0(PX2w zwDGK9+E;(Ws?5GGk(ng^PSYE^D@m6lGB!rDg2Q6y*l-=O-6$+2 zW$1u&rI?6F?$D4kxpdwOp7ZutTIVTfeX;1yUVPr$y6c(}5zAyWW=kM5C#QJWJUlDG z;H)yU8BfFLKOTLiLH$h7z&u@3&}qbvU${m~(QR6Y_)^%V1wnvJG5eYP@4nZ;cW-RH z%_}21Ik%{VmcKo?cb$U3n%Ghh^9nq-N zh{BQw+m7GV7^VmletNihgPERU-H7FS)*2^wPo9^z8XZrcmoR3;?90+p3bJ!@DgsYG zJ~jby@?QZG#m&M10&vbTH$AHA7@u8dvRd=v9c^wMFlZr;xJ=LCSWCol`YkEn@j*Ik zI^XO~J}?#hI#p!YHB`cLHXI5s)7J4^wn0e~jeYkUb^c5iW*+3Ozd0gQC#YX>(d}v? ztGJDtNm714Uf1ny-Zr(8C=;wm&vjrI$tMzZz$k#BBe~W)$m6;Zv3ZjhKhx~x zQM&5D{im|$*!@GbHosjLNlbcq{JD)9Uskkj>f6o`4B<&o|5nhU<F$<$eC>*{i<++&zB>9)>F`rUpK#s*ln z%v;Y6EPOafLqiB0c0JR3sy_{HX#aKKyM^eX)HivbjzR}Mw^cVRv(`W4r|YEOUp#We zH+#im1=l>_GxEi`Z8{dhBrBbqqU#P+ylkfv-YmWNnUCAsqpx|5CZb|4a5Gt{vtN|L zd%25boe3=tnyzQ`;Zn!YVoJx1ju>r{eIRsI7faIhZUIe~sR!7cjC`FwxScDmsUD~a zrI@2VX6{~>=pJ2hgesscJtx^#?T{U^mplL>~%JCy-L>NHAij?w5ZkyrO|1|M7@dq>WyEUO2Q6i%aNDw zP>RD0L$~N(E!% z>IbtgWQZdixW{OjRr)xtL@KVU+9Uj!TRePC>lYkr&i<9={a~~t?r;6M!@v3!C2Xz1 zy3h!Ymgu1}-cpzqZ1z2wa8pX;V&TG=xWh(V^75{{@GL$c!vl+-iwWIs+CWIzt z|2!*c0mZ5BQ##aceBI=GwZ|)jD*vzuuL9jE} z(1}`Gj&SsMl%TxL6yf&FQuMFQMHP85;s+aiGJr4L}*F`2T~bIXAD=oS{PjXzxN9+dO{d3_)s{Q$#Uz= z9ew0>0+$}2VOzfC8Qde7E((FT)jD{d&Q-H^FD&&2jDa6V4%p5kKL1kidt(PR*^SY+ z62=C6Fp+bUrg9JJ&=hm$kha8ExzF0F2Qm`96K3mHT{er@CxfUz+&8PTEDaNvh*{1qhJSFgV&m zd5bz5M4MVT#|hK!6&RA*W>}U(fl6qqr%F19h(tuY+;--~3fZ6XY_8iQa3_;>uCu0R ziQ6v3HHPtpO7T7Jv;dae0*F6W)V7cZ%I?7Csq|c`f;dGS=(e~pBSs6|Pu-!+aoCUV zr@4}7rVr`c0J1uCu$J6JUO=s2*tEEubgH}Gap5> zV_v4v%TfVz{_9HN*{V$ICj&J@lKp)psCiclOTAJxv21?k^t-47Ocf{AHztzYA*zVH zl7sJ5-XOe=eNAlsRFWZ}#>5t5^ZWG@&bTfWm8yzj&v4lRugE!af$scZfv&sQe0ySx z_p-5Nrfj!Q)ux$Dg;2G&_gh6*mHJn|;jA1)vhv(w+{A1fNw3)rE* zceK{4kQu9P>`owa-35+krBWmtbcMe7c;xO=k;$11I3r9rcHkZIkHVVY`NVM5n)-OU z#Q&q~X5u)?3sWpV4}GG$%rYBntl4ZuCw}K${!t@ZNUlU{Ogcl5gT^BjiQQbpza`#z z05=wJIDr*@+w~Y0d13t9P5u&9p>I*%H9rFI&m55`?((OP;Z8_$M*Tc80%wBBb@NA} zYSDBsKuV&Amp(Lv{rAsR(ff>Y&v5>x=fKcU)ptK0{ny!Wd^`j%-O)0*(LRVRYD%AV z7XRf(Ib062Lc<(D1?#vOX(4zO*oKYddKTLm;`%RQ`lnlL!(=eNqVWV0DJCZE`nfFl zI?m}gymQ8*Q7dfZ^5!?r`E~^rWl(?Le(KGvwr8U2ILzaM{eCS|SCve3oF9J{$*IO2 zd)q^7#A%v%Rp6gUuFgZ|`Qui3`h+R@$9=I&kf z1&?N3iQ;+s#J!8}#uCPI6588^R#xqu-MJ$d<~#(z-?sXmAm*T2hL^rC35uASFK?yg z&cA=Sudacue12efq|ImX&9J%R=IUCtq0e@+rrPuu&LloRlt&oGpE4aX?lC*lbJZM) z0f0x3xa_~v)AJsCwhpCD6?k56X$Qo$2Q6U1xDkRUS~5{bGErZofj0xdiMjBoJ~80l zYAguZTUm7@k%Z=AmS(emIr%Fo%nZvcp=Z#?3iFd zf25V@L#*}s@j-BWY#~yhPAkl=mt0|<$?+SN zWrEVTWG~m-q3Ib;S`UT1g*cpv&U`DIf!}+QIOjH&Dq?xmUjqj3jHX!piZ+ZJA)T2O z!!Ufep+On(*ShI_hmO2r%%VFmejXWr2?SD-?^AVkU+`tIojhQa+E`dT&64qqi69<&#r6W`jdBdOyDHeku;i%n1!+w7b{j)6=9oHx%gS?5>Z}9IW z#}OxZH+*z?^nKFVuADYXf$bpn7JG!#>6;Ff3%rPjKJI~Cyzyt0QdWVAswxwku9I-2 zv!d`uLXmJMc-ZKB_wLzq#E=Y)V6I%w$##m*N_d?qp`zj#`uzEK6RM!U#y0ANn0pw$ z=7%@h#-Nv~%zi939wlDW*3^u>?fClVVze(emg%5**1-#Q+` z(seP_-oE?k%AL#QT#4-!8X<`ZG<5niD>#3upL^$9kCwfiD_1UbWvBY}*c=!w0W0zgELTt--*?(7;`KV<2G|UyC={gQ?UiSd1E#Jlz*a_cMMZx$ zcM@@T?Q?vQ)b?xmAY(%=oQ}rh*@!Y6xd}%ymiFoM_wg~%WP^yd+wNBVzTz{Iy;}bR zZjs(T8qSHlk^_bT#ZMDZ@v-AP{7(qq{gKARz-FT&eG`78eum5Vj8f{O0vXde)ym9zuQ`Azb)`zyx5oBlaMd!|jODbRBQKL(|0*Owtn{^dGu8 z$C3P2cIk^Zn7A~_2l-tGgRc#m9hmBvM@@Ocl?ryajQ71WOepx3=9^%;asVU~a%ZY` zkQ))V~9f(WAxb<@e)#Ygg>y$;UGtQ2a+Nm{W6yxtD6(z(#zfxc0878W|BVn( zZNf5Xpa~1Jn>hcw8fuAwp!lJ4ygL7(4?ycggNx7Oxk*!v;8(@18kpcY4@VSFYGgZInd_e}@#PY98#?*-HE zZ+W!bgIKmE8hsyu?Z9#fA@=IVmnGRtkx{oYg_SZptC6jvbG+s7ZlLg9u|7S0qM>su zp;f)BQbr_rbES-a8k zDA&NC%VFnx?Y{!IYai;(rPz_nEi9)K@-}jVR=x$AG&haFa&Ol=_5Hg@MC8n5yNkza zz{;oMHb#@X#+O8|#5cZd*yl1dxQ%1zpPkc?Hk&3^SDUq7YkHdCu_}=@>Z$WFd|ye{ z>Aa7u51GSGOI(^4Wj=h6;66K7bb1Zl8sc~^Q5BTnPG0s9kJLQry&d7az3xcanfGrk z7P1$@N?Hik{U;mpLl zdiFnQ3AEse^co~5fyHlBdGtf9e5R(Ha4AZ7Yv2z|Y=WNcLILoI}I*ZiDnF;XqGD~HrNeBb=zUrxwy0JyC zbA4ug5hI`NT8={jYoj?vM- z|FaKYU+~ENb$1_LKkPdurYRxRhVYhmb)6905uok8Evp+nGAFn_5En;Wr5<~BecYm| zWq!$N$x`(mq#q40IQ3ytYsXt1*TSDa&-KiGT1V?IzqA!yoKi$w{IH)U*^AFzvAjdH zK$)KiyH&tcEj_F0CnvRwG=dMV1qWR`vJrS+zq-7~X2^1(scoOu`L*1cM|EUar`1ug z@BF6P{EJp#?dH9As33;FwtrOD9^!!&m8L)hw(fw6Z%@oP2PiU|SwE&TlMcK)sB1XQ zqNl_Wxr=yly}rOQ&4ZZx<3nj@quxKWqe#N6#vc}CPin(V`KEL_`WKq)(@j!vLtTf0Nf*sTh0VFn{_^hc zvAzPwJn==yN5YPIh0$|(=%%i#4@gST6muDy_xn(T*d|l9oqejHTk(d9fBfu6gV$TDDb|j;dd%Gig*AjtE^))`rDoNVQ2Rp4jPw0~mjFv7!> zP3I4ThGLu=Eh{B_t^;=#87{9)I7S%sr;NcsaP6M;^P0+TMgG`dz?ed_#L|Fr$JGy1K1 zMG3xOVK|76tbh%W4R(%3pI#EwjUprAU_ri_(45Efl?h|n{WR)Qh--^S>_A!QSsHcb zXoF+O`CY-j%GR4~xx)bA?gSN7woCL1bZYFH?E4mNXjH@?(dse~sB=jQplK?MCK(Ow zk8XNOnT9(g`Ib?0 zOO^4?7mje=I<$&DXQAvI^SAC>t|(PYaCr@?s(Md^>)Ov6|IDH3vS(gApOGv@%x$AI z`DZm$Z-f?|5?=Hmu7Dkq4~cZBCX*pO=;npsF9}2v&+@z9_nq(DGhS`%O6O?fQ|S9cl`@-HyCituc#-DkvzCul_I;{(WlBKJ z#w?qNffF+}taQtW-m9&-mZLucrF{qQRG_CTBiWB3zKg0(bjcl}YCWFB&q=c^Y;5H6 z=~T9^%Uua$7dXPp;#=dn15nUb16gRABl&ihI5ldm?h2U)7CJpPOY*g&aRNrYA4*gzUMF zj^I{0_6Y8=HJ1yHH3_ULSeq1a$ha5-DJx|u;S9->>J3!YJ3L0faPF1MG(!Yu->3D} zVbN5o&Qq6Z#c?a$48-j5dU(nsp6C+HOhdyUt>wJ)BTNvvWb`kuFJ#KH)$F)OUVR1D z2?L$pit=4@57JJSA zt<)?!DB757{h+hT4Qi_WSxa)Ttk&Q^i7Aa)6nl?%;8w zd5&@d>;sDa(@EDOPEOhmy&ia|B_CHMT%mWTYrjqCD(gX0>&e>o1U*u26FciPnN!8( z8tJ6X85Z++5AEt76WlFyItN>Qn$Xkfs0`G6m?=0j#Nej*P3T zppOBE07#S|WoC8z83p%535wrbQab$^BE@XRE5qv?glAw|%3+QgM6iON9IwIt2f5KB z9Z!=BmgF|Ib+1UaXLM=)1gzjcpEzY$>EtSEX1cF&c2DIe^MA08f0KzU2wu7->p^!j zUN_^(*cYr7-t&UEQ>#jGA~xX5*nol22T8W7iFBxT$R%M~zrE+)vzbxD?MJngnO2eF zkhfy(jqL&IssCzC(}JMQUev~?6vPiIF-a{nV*D9&^j9@`Spg^6xH=|-t+{d6h_H2o zdd^mT+dppLo_Au)&YUPzf<^O=h>9d$aoRjG$k2r#qlV)H7cbGv-C3DQk0h^7(zDizB`* z>bFVi07N%z_YX&zC?a^>NhKZ=w@Tx#@A)ccZp%Md0`7r9`*iKf6J6nYvl83qS*qD9 z@q#;x`JJ`sk&bvVcGMC_7+{;q9~$R#5xrAQQC#eS;dCF1PidB{F4Y> z#F?>fy(=#m%qi8fG#2x)c}V#-k!ulJe!%=3q(5v`KnS~BA;w1pKM5y~l3-Cl0{H>B zaxQ?$fv*vvqOYqw#D6?6nJ}8w+qrY~x$C-(>^NS>F^0EM;%tKO=iJ^&f4`1Q)a;%` zaXr1gWxf$SH?z99t5<(qilY3v{oTa!JevCTcVbBa-|elOxZmV!-0P+-Hj(M>m*t!| zekr%kn@wth=&lf_*hYaGDysH}!R2is>$Od^GgHq`Y7NF&pQgjtVWQ_HA(!b5m{)3S zY;0t-Ce#xTgI`|Z(<0}%_Dr><8=7A*YxW_l+v`L&)Y$TQAV(&1(faG#t4*vELSDk# zlt|^gPh-MF@|wK!q)sdoebX)#OWh!+36lq*^1@UKR{k6-_3`!9j zq`Bt$)8{=gwg)k^537 zfq~>X93;+G{1#~FbO!x+;dl3uQzashorV7(7R@nW$7q^3I_-vH3xFN$IN%1rZSnf% z$DoHS9Cn>o;xMoTA8%N#0sp7x!$@6ZsI}Fgpsp7x+&xI5{&n_rR@HRg&t`%VG z8NgMGxsmh$#xESFZUTH;ygm@Uki*(cq$VL$dT3d4m)%N=ZsX$I#FE*M{-VeW$^4P= zp&gB(NCN9${$6f@Q&Uq?gD=lJgD+>~(YedN&P-%Va4#xlR(;aii3T&~K2~>60ldOh zM9B2+{s)z(c4^RO5$rJ@~=0-VI)tD>DM| zjiDDMxRNHH+;AXj3)!fc(y-FhB4${QEO+-9YQbm}^nNPO; z_M7EkqhlNVa%MQMD%H9f?_Uw-3mA6?4tML9`L3ga0$u}!fGHOPdk)6q34>s1RH-bH z_Q2e}s{!vwArN|Bn>XH-73`Oyxe(UYLJ+ydUaI4iTjKUXg2?_uZAz@%^uo)xhtLINRNHId?(OovwxeX5@ z)3$9W(1?&W|L0inC)e7sIH=r***r=SiZWQt$VSovDFFcX=8Y(@VA17eB`Ofaqh{T{ zzFTcR^_LDW4$xC0{mS1*^^(Hr1AliwpW1|0O#q|2|Ha>nA%)8@0EDu=W$MAs1oE{T zQLlyL_c(z*{bdYWav)l_Sct`rLjM0L?WB6ClAJx5-FxIHO3%f?eQ%ll;qU(k++N{@ literal 0 HcmV?d00001 From cf7b0fac51f1bacbdc45bb50e0dde6c3f23391a1 Mon Sep 17 00:00:00 2001 From: Samuel Foo Enze Date: Mon, 27 Jul 2026 23:23:44 +0200 Subject: [PATCH 28/38] Demo obstacle-triggered replanning with the layered global map (#3) * Adds support for replanning when a progress error is raised This PR adds support for replanning when a progress error is raised. We also add similar nav2 support for handling such progress errors. Signed-off-by: Arjo Chakravarty * Add launch testing Signed-off-by: Arjo Chakravarty * fix(nav2): stage lifecycle startup after robot readiness * fix(path-server): conservatively downsample fine occupancy grids * fix(nav2-traffic): resend unchanged targets for new plans * fix(nav2-traffic): ignore aborts from superseded goals * fix(plan-executor): replan when map updates block active routes * fix(plan-executor): align incremental targets with route direction * fix(map-demo): retain scans while filtering the robot body * feat(map-demo): add simple and warehouse replan scenarios * feat(map-demo): color-code robots, regions, goals, and paths * feat(map-demo): automate obstacle spawning and replan checks * feat(map-demo): launch integrated obstacle replan scenarios * docs: describe obstacle-driven replanning workflow --------- Signed-off-by: SamuelFoo Co-authored-by: Arjo Chakravarty Signed-off-by: Samuel Foo Enze --- discourse/3-layered-global-occupancy-map.md | 7 + map_server/README.md | 2 +- .../rmf_layered_map_server_demo/README.md | 45 +- .../launch/replan_obstacle.launch.py | 39 ++ .../maps/single_room.png | Bin 0 -> 536 bytes .../maps/single_room.yaml | 7 + .../rmf_layered_map_server_demo/package.xml | 7 + .../region_update_visualizer.py | 21 +- .../replan_obstacle_demo.py | 525 ++++++++++++++++++ .../replan_obstacle_launch.py | 292 ++++++++++ .../replan_scenarios.py | 91 +++ .../scan_region_publisher.py | 118 +++- .../rviz/nav2_observations.rviz | 99 ++++ .../rmf_layered_map_server_demo/setup.py | 6 +- .../test/test_replan_obstacle_demo.py | 195 +++++++ .../test/test_scan_region_publisher.py | 77 ++- .../worlds/single_room.sdf | 74 +++ nav2_integration/README.md | 5 +- .../demo_world/demo_world/staged_nav2_init.py | 216 +++++++ nav2_integration/demo_world/package.xml | 6 + nav2_integration/demo_world/setup.py | 1 + .../src/inner_navigation_client.rs | 176 ++++-- .../rmf_nav2_traffic/src/safe_zone.rs | 112 +++- .../sp_demo_nav2_bringup/README.md | 7 +- .../cloned_multi_tb3_simulation_launch.py | 62 ++- path_server/README.md | 5 +- path_server/rmf_path_server/src/lib.rs | 39 +- path_server/rmf_path_server/src/planner.rs | 97 +++- .../test/test_path_server_error.py | 231 ++++++++ path_server/rmf_plan_executor/src/lib.rs | 395 ++++++++++++- path_server/rmf_plan_executor/src/main.rs | 9 +- 31 files changed, 2857 insertions(+), 109 deletions(-) create mode 100644 map_server/rmf_layered_map_server_demo/launch/replan_obstacle.launch.py create mode 100644 map_server/rmf_layered_map_server_demo/maps/single_room.png create mode 100644 map_server/rmf_layered_map_server_demo/maps/single_room.yaml create mode 100644 map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_demo.py create mode 100644 map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py create mode 100644 map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_scenarios.py create mode 100644 map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py create mode 100644 map_server/rmf_layered_map_server_demo/worlds/single_room.sdf create mode 100644 nav2_integration/demo_world/demo_world/staged_nav2_init.py create mode 100644 path_server/rmf_path_server_test/test/test_path_server_error.py diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md index 3dbec68..256685e 100644 --- a/discourse/3-layered-global-occupancy-map.md +++ b/discourse/3-layered-global-occupancy-map.md @@ -24,6 +24,7 @@ observations, can be added later. * The Rust map server composes a static occupancy grid and active observations into `/map` * The Nav2 demo converts scans from three robots into clear ray sectors and occupied endpoint regions, then displays the source contributions and map +* The replanning demo fuses two robots' scans and replans when an updated map blocks an active route * The observation messages live in `rmf_layered_map_msgs`, leaving `rmf_prototype_msgs` unchanged @@ -133,6 +134,12 @@ The demo launches Nav2 localization, planning, and control for the fixed goal lo The scan-to-region conversion uses one convex sector per sampled beam instead of expanding a Bresenham line into many point regions. Scan sampling reduces the number of sectors, publication throttling limits the snapshot rate, and the observation range limits represented returns. The TTL controls how quickly stale observations expire. Each publisher logs its input beams and clear and obstacle regions so message density, update rate, and visual fidelity can be compared. The current demo remains a 2D occupancy approximation; it does not fuse probabilistic confidence or compress adjacent sectors into larger regions. +# Two-Robot Replanning Demo + +The replanning launch starts with a simple room by default and retains the warehouse layout as a regression scenario. It fuses both robots' scans into `/map`, spawns a blocking bar, and displays robot-colored poses, goals, observations, and Nav2 paths alongside the green RMF plans. + +The plan executor checks each remaining route against the composed map and publishes `CODE_PATH_BLOCKED` after a short debounce. The path server replans on a conservatively downsampled `1.0 m` grid, while the Nav2 bridge ignores superseded aborts and resends unchanged targets when a new plan requires them. + # Example Flow A local costmap or LiDAR observation node can publish a replacement snapshot by: diff --git a/map_server/README.md b/map_server/README.md index ee71057..15978b6 100644 --- a/map_server/README.md +++ b/map_server/README.md @@ -12,4 +12,4 @@ ros2 launch rmf_layered_map_server_demo demo.launch.py Use `use_rviz:=False` for a headless run. -See [`rmf_layered_map_server_demo/README.md`](rmf_layered_map_server_demo/README.md) for the three-robot Nav2 observation demo. +See [`rmf_layered_map_server_demo/README.md`](rmf_layered_map_server_demo/README.md) for the three-robot observation demo and two-robot replanning demo. diff --git a/map_server/rmf_layered_map_server_demo/README.md b/map_server/rmf_layered_map_server_demo/README.md index 136dac7..7ea5acb 100644 --- a/map_server/rmf_layered_map_server_demo/README.md +++ b/map_server/rmf_layered_map_server_demo/README.md @@ -1,6 +1,6 @@ # Layered Map Server Demos -This package contains two demonstrations of the layered map server. +This package contains three demonstrations of the layered map server. ## TTL Smoke Test @@ -31,3 +31,46 @@ By default, the demo opens three robot-local RViz windows and one combined-map v * Use `map` and `params_file` to override the warehouse map and shared Nav2 parameters. * `beam_stride:=1` and `publish_period_sec:=0.5` control scan sampling. * `max_observation_range:=2.5` and `ttl_sec:=10.0` control range and retention. + +## Two-Robot Obstacle Replanning Demo + +This demo connects the layered map server to the path server, plan executor, and Nav2 traffic bridge. It exercises the `CODE_PATH_BLOCKED` replanning flow from [PR #39](https://github.com/open-rmf/next_gen_prototype/pull/39). + +The default scenario is a simple room with no aisles: + +```bash +ros2 launch rmf_layered_map_server_demo replan_obstacle.launch.py +``` + +Two robots start with non-overlapping scan regions and separate goals. Once their initial plans are active, the demo spawns a red bar across both scan regions. The bar meets the right wall to form a dead end and leaves a detour around its left end. + +The global RViz view shows robot-colored poses, goals, region contributions, and Nav2 paths alongside the green RMF plans. + +Use the warehouse scenario to reproduce the original two-aisle case: + +```bash +ros2 launch rmf_layered_map_server_demo replan_obstacle.launch.py \ + scenario:=warehouse +``` + +This scenario keeps the original robot poses and 15 m shelf-connected bar. + +Run only one scenario per ROS/Gazebo domain because both use the same robot and map topic names. For concurrent runs, set different `ROS_DOMAIN_ID` and `GZ_PARTITION` values. + +The expected sequence in the global RViz window is: + +1. Both initial plans appear on the static map. +2. The long bar is spawned and its detected portions enter the combined `/map`. +3. After a 300 ms debounce, the plan executor publishes `PlanError.CODE_PATH_BLOCKED` for the blocked route. +4. The path server publishes a new plan using the updated map. + +The path server conservatively downsamples the `0.1 m` simple-room map and the `0.03 m` warehouse map to its `1.0 m` PiBT planning resolution. Any planning cell containing an occupied source cell remains occupied. Replan reports have a two-second cooldown to prevent oscillation. + +Useful launch arguments: + +* `use_nav2_rviz:=True` opens the robot-local Nav2 views. +* `use_global_rviz:=False` runs without the combined RViz view. +* `spawn_delay_sec:=1.0` controls the delay after initial plans are received. +* `scenario_timeout_sec:=180.0` controls how long the demo waits for replanning. +* `obstacle_memory_sec:=-1.0` retains obstacle endpoints for the scanner's lifetime. Use `0.0` to disable memory or a positive value for finite retention. +* `self_filter_radius:=0.22` excludes scan returns inside the robot body. diff --git a/map_server/rmf_layered_map_server_demo/launch/replan_obstacle.launch.py b/map_server/rmf_layered_map_server_demo/launch/replan_obstacle.launch.py new file mode 100644 index 0000000..95785aa --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/launch/replan_obstacle.launch.py @@ -0,0 +1,39 @@ +# 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 launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration + +from rmf_layered_map_server_demo.replan_obstacle_launch import ( + generate_replan_launch_description, +) + + +def _launch_scenario(context): + scenario = LaunchConfiguration('scenario').perform(context) + return generate_replan_launch_description(scenario).entities + + +def generate_launch_description(): + """Launch a replanning scenario.""" + return LaunchDescription([ + DeclareLaunchArgument( + 'scenario', + default_value='simple', + choices=['simple', 'warehouse'], + description='Replanning environment to launch.', + ), + OpaqueFunction(function=_launch_scenario), + ]) diff --git a/map_server/rmf_layered_map_server_demo/maps/single_room.png b/map_server/rmf_layered_map_server_demo/maps/single_room.png new file mode 100644 index 0000000000000000000000000000000000000000..06068fb565f58d8ea48281837a3342deef061a61 GIT binary patch literal 536 zcmeAS@N?(olHy`uVBq!ia0vp^CxCbX2OE&|{I9$cNO2Z;L>2>S5MX3zsT2ot7*Bb+ zIEGZrd3#xr?}!5r%fY|@mkXDvcxj)Rw$4i`Ddp1hImI(?zCI>zaC^^3_A_Sre;7BP zm99UK^Ld{84|k?U1p$sxg05lk!BW8ZcP=~{U@>#;9oy+p?=wq+xbs2r>*?y}vd$@? F2>@6MbVC3D literal 0 HcmV?d00001 diff --git a/map_server/rmf_layered_map_server_demo/maps/single_room.yaml b/map_server/rmf_layered_map_server_demo/maps/single_room.yaml new file mode 100644 index 0000000..701c96d --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/maps/single_room.yaml @@ -0,0 +1,7 @@ +image: single_room.png +mode: trinary +resolution: 0.1 +origin: [-10.0, -8.0, 0.0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.1 diff --git a/map_server/rmf_layered_map_server_demo/package.xml b/map_server/rmf_layered_map_server_demo/package.xml index 0ec46c5..90d38f7 100644 --- a/map_server/rmf_layered_map_server_demo/package.xml +++ b/map_server/rmf_layered_map_server_demo/package.xml @@ -13,13 +13,20 @@ rclpy ament_index_python geometry_msgs + lifecycle_msgs gz_tools_vendor launch launch_ros nav_msgs + nav2_common nav2_msgs rmf_layered_map_msgs rmf_layered_map_server + rmf_nav2_traffic + rmf_path_server + rmf_path_visualizer + rmf_plan_executor + rmf_simple_destination_server rmf_prototype_msgs rviz2 sensor_msgs diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py index bae3500..c0f5575 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py @@ -23,14 +23,16 @@ from rmf_prototype_msgs.msg import Region from visualization_msgs.msg import Marker, MarkerArray +from .replan_scenarios import ROBOT_COLORS + SOURCE_COLORS = ( - (0.96, 0.26, 0.21), + ROBOT_COLORS['robot0'][:3], + ROBOT_COLORS['robot1'][:3], (0.18, 0.80, 0.44), - (0.20, 0.60, 0.98), - (1.00, 0.65, 0.15), (0.61, 0.35, 0.71), (0.10, 0.74, 0.80), + (0.96, 0.26, 0.21), ) CLEAR_COLOR_BLEND = 0.5 CLEAR_MARKER_Z = 0.04 @@ -183,10 +185,15 @@ def apply_update(self, update): if latest_stamp is not None and stamp_nsec < latest_stamp: return MarkerArray() - color = self.source_colors.setdefault( - key, - SOURCE_COLORS[len(self.source_colors) % len(SOURCE_COLORS)], - ) + color = self.source_colors.get(key) + if color is None: + robot_color = ROBOT_COLORS.get(update.source.robot_name) + color = ( + robot_color[:3] + if robot_color is not None + else SOURCE_COLORS[len(self.source_colors) % len(SOURCE_COLORS)] + ) + self.source_colors[key] = color marker_array = MarkerArray() self.markers_by_source[key] = [ marker diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_demo.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_demo.py new file mode 100644 index 0000000..6601633 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_demo.py @@ -0,0 +1,525 @@ +# 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 math import atan2, cos, pi, sin +import subprocess +import time + +from geometry_msgs.msg import Point +from lifecycle_msgs.msg import State +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import NavigateToPose +from nav_msgs.msg import OccupancyGrid, Odometry +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from rmf_prototype_msgs.msg import Plan +from visualization_msgs.msg import Marker, MarkerArray + +from .replan_scenarios import get_scenario, ROBOT_COLORS + + +ROBOT_MARKER_Z = 0.40 +GOAL_MARKER_Z = 0.30 + + +def make_navigation_goal(x, y, yaw=pi / 2.0): + """Create a map-frame Nav2 goal.""" + goal = NavigateToPose.Goal() + goal.pose.header.frame_id = 'map' + goal.pose.pose.position.x = float(x) + goal.pose.pose.position.y = float(y) + goal.pose.pose.orientation.z = sin(yaw / 2.0) + goal.pose.pose.orientation.w = cos(yaw / 2.0) + return goal + + +def plan_signature(plan): + """Return a plan's waypoint coordinates.""" + return tuple((waypoint.position[0], waypoint.position[1]) for waypoint in plan.waypoints) + + +def _point(x, y, z=0.1): + point = Point() + point.x = float(x) + point.y = float(y) + point.z = float(z) + return point + + +def _base_marker(marker_id, namespace, marker_type, color): + marker = Marker() + marker.header.frame_id = 'map' + marker.ns = namespace + marker.id = marker_id + marker.type = marker_type + marker.action = Marker.ADD + marker.pose.orientation.w = 1.0 + marker.scale.x = 1.0 + marker.scale.y = 1.0 + marker.scale.z = 1.0 + marker.color.r, marker.color.g, marker.color.b, marker.color.a = color + return marker + + +def triangle_marker(marker_id, namespace, x, y, yaw, color): + """Create a robot marker.""" + marker = _base_marker(marker_id, namespace, Marker.TRIANGLE_LIST, color) + local_points = ((0.65, 0.0), (-0.45, 0.4), (-0.45, -0.4)) + for local_x, local_y in local_points: + world_x = x + cos(yaw) * local_x - sin(yaw) * local_y + world_y = y + sin(yaw) * local_x + cos(yaw) * local_y + marker.points.append(_point(world_x, world_y, ROBOT_MARKER_Z)) + return marker + + +def star_marker( + marker_id, + x, + y, + color=(0.25, 0.75, 0.12, 1.0), + namespace='goal', +): + """Create a star-shaped goal marker.""" + marker = _base_marker( + marker_id, + namespace, + Marker.TRIANGLE_LIST, + color, + ) + ring = [] + for index in range(10): + radius = 0.8 if index % 2 == 0 else 0.35 + angle = pi / 2.0 + index * pi / 5.0 + ring.append( + _point( + x + radius * cos(angle), + y + radius * sin(angle), + GOAL_MARKER_Z, + ) + ) + center = _point(x, y, GOAL_MARKER_Z) + for index, point in enumerate(ring): + marker.points.extend((center, point, ring[(index + 1) % len(ring)])) + return marker + + +def bar_marker(marker_id, scenario): + """Create the obstacle marker.""" + marker = _base_marker( + marker_id, + 'spawned_obstacle', + Marker.CUBE, + (0.95, 0.05, 0.05, 0.9), + ) + marker.pose.position.x = scenario.bar_center[0] + marker.pose.position.y = scenario.bar_center[1] + marker.pose.position.z = 0.1 + marker.scale.x = scenario.bar_size[0] + marker.scale.y = scenario.bar_size[1] + marker.scale.z = 0.2 + return marker + + +def box_sdf(name, x, y, size_x, size_y, size_z, color): + """Build SDF for a static box.""" + rgba = ' '.join(str(channel) for channel in color) + return ( + "" + f"" + f'{x} {y} {size_z / 2.0} 0 0 0' + 'true' + "" + "" + f'{size_x} {size_y} {size_z}' + f'{rgba}{rgba}' + '' + "" + f'{size_x} {size_y} {size_z}' + '' + '' + '' + '' + ) + + +def yaw_from_odometry(odometry): + """Return the planar yaw from odometry.""" + orientation = odometry.pose.pose.orientation + return atan2( + 2.0 * (orientation.w * orientation.z + orientation.x * orientation.y), + 1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z), + ) + + +def spawn_bar(world_name, scenario): + """Spawn the obstacle in Gazebo.""" + sdf = box_sdf( + scenario.bar_name, + scenario.bar_center[0], + scenario.bar_center[1], + *scenario.bar_size, + color=(0.95, 0.05, 0.05, 1.0), + ) + request = f'sdf: "{sdf}"' + return subprocess.run( + [ + 'gz', + 'service', + '-s', + f'/world/{world_name}/create', + '--reqtype', + 'gz.msgs.EntityFactory', + '--reptype', + 'gz.msgs.Boolean', + '--timeout', + '1000', + '--req', + request, + ], + capture_output=True, + check=False, + text=True, + timeout=3.0, + ) + + +def spawn_succeeded(result): + """Check that Gazebo accepted the entity.""" + output = f'{result.stdout}\n{result.stderr}'.lower() + return result.returncode == 0 and 'data: true' in output + + +class ReplanObstacleDemo(Node): + """Run the two-robot replanning demo.""" + + def __init__(self): + super().__init__('replan_obstacle_demo') + scenario_name = self.declare_parameter('scenario', 'simple').value + self.scenario = get_scenario(scenario_name) + self.robot_layout = { + robot.name: robot.pose for robot in self.scenario.robots + } + self.robot_goals = { + robot.name: robot.goal for robot in self.scenario.robots + } + self.world_name = self.declare_parameter( + 'world_name', self.scenario.world_name + ).value + self.spawn_delay_sec = self.declare_parameter( + 'spawn_delay_sec', 4.0 + ).value + self.timeout_sec = self.declare_parameter( + 'scenario_timeout_sec', 180.0 + ).value + self.started_at = time.monotonic() + self.spawn_at = None + self.next_spawn_attempt = None + self.state = 'waiting_for_inputs' + self.timeout_logged = False + self.map_ready = False + self.bar_spawned = False + self.odometry = {} + self.initial_versions = {} + self.initial_paths = {} + self.replan_versions = {} + self.changed_paths = {} + + reliable_transient_qos = QoSProfile( + depth=10, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, + ) + self.navigation_clients = {} + self.inner_navigation_clients = {} + self.nav2_lifecycle_clients = {} + self.nav2_state_futures = {} + self.nav2_active = set() + self.goal_futures = {} + self.goal_handles = {} + self._demo_subscriptions = [] + for robot_name in self.robot_layout: + self.navigation_clients[robot_name] = ActionClient( + self, + NavigateToPose, + f'/{robot_name}/navigate_to_pose', + ) + # Wait for Nav2 to activate before sending outer goals. + self.inner_navigation_clients[robot_name] = ActionClient( + self, + NavigateToPose, + f'/{robot_name}/inner/navigate_to_pose', + ) + self.nav2_lifecycle_clients[robot_name] = self.create_client( + GetState, + f'/{robot_name}/inner/bt_navigator/get_state', + ) + self._demo_subscriptions.append( + self.create_subscription( + Plan, + f'/{robot_name}/plan', + lambda msg, name=robot_name: self.receive_plan(name, msg), + reliable_transient_qos, + ) + ) + self._demo_subscriptions.append( + self.create_subscription( + Odometry, + f'/{robot_name}/odom', + lambda msg, name=robot_name: self.receive_odometry(name, msg), + 10, + ) + ) + + self._demo_subscriptions.append( + self.create_subscription( + OccupancyGrid, + '/map', + self.receive_map, + reliable_transient_qos, + ) + ) + self.marker_publisher = self.create_publisher( + MarkerArray, + '/replan_scenario/markers', + reliable_transient_qos, + ) + self.timer = self.create_timer(0.25, self.tick) + self.marker_timer = self.create_timer(0.5, self.publish_markers) + + def receive_map(self, msg): + self.map_ready = ( + msg.info.width > 0 + and msg.info.height > 0 + and msg.info.resolution > 0.0 + ) + + def receive_odometry(self, robot_name, msg): + self.odometry[robot_name] = msg + + def receive_plan(self, robot_name, msg): + version = msg.plan_id.plan_version + signature = plan_signature(msg) + if robot_name not in self.initial_versions: + self.initial_versions[robot_name] = version + self.initial_paths[robot_name] = signature + self.get_logger().info( + f'Received initial {robot_name} plan v{version} ' + f'with {len(signature)} waypoints' + ) + elif version > self.initial_versions[robot_name]: + self.replan_versions[robot_name] = version + self.changed_paths[robot_name] = ( + signature != self.initial_paths[robot_name] + ) + self.get_logger().info( + f'Received replanned {robot_name} plan v{version}; ' + f'route changed={self.changed_paths[robot_name]}' + ) + + if ( + self.state == 'waiting_for_initial_plans' + and len(self.initial_versions) == len(self.robot_layout) + ): + self.spawn_at = time.monotonic() + self.spawn_delay_sec + self.state = 'waiting_to_spawn' + self.get_logger().info( + f'Initial plans are active; spawning the blocking bar in ' + f'{self.spawn_delay_sec:.1f}s' + ) + + if ( + self.state == 'waiting_for_replan' + and 'robot1' in self.replan_versions + ): + if self.changed_paths.get('robot1', False): + versions = ', '.join( + f'{name}=v{self.replan_versions[name]}' + for name in sorted(self.replan_versions) + ) + self.get_logger().info( + f'Replan succeeded ({versions}); ' + 'robot1 now avoids the spawned obstacle' + ) + self.state = 'complete' + else: + self.get_logger().error( + 'Plan version increased without a route change' + ) + self.state = 'failed' + + def receive_goal_response(self, robot_name, future): + try: + goal_handle = future.result() + except Exception as error: + self.get_logger().error( + f'Outer navigation request for {robot_name} failed: {error}' + ) + self.state = 'failed' + return + if not goal_handle.accepted: + self.get_logger().error( + f'Outer navigation request for {robot_name} was rejected' + ) + self.state = 'failed' + return + self.goal_handles[robot_name] = goal_handle + self.get_logger().info( + f'Outer navigation request for {robot_name} was accepted' + ) + + def nav2_servers_are_active(self): + """Check whether both Nav2 servers are active.""" + for robot_name, client in self.nav2_lifecycle_clients.items(): + future = self.nav2_state_futures.get(robot_name) + if future is not None and future.done(): + try: + response = future.result() + except Exception as error: + self.get_logger().warning( + f'Cannot read {robot_name} Nav2 lifecycle state: {error}' + ) + else: + if response.current_state.id == State.PRIMARY_STATE_ACTIVE: + self.nav2_active.add(robot_name) + del self.nav2_state_futures[robot_name] + + if ( + robot_name not in self.nav2_active + and robot_name not in self.nav2_state_futures + and client.service_is_ready() + ): + self.nav2_state_futures[robot_name] = client.call_async( + GetState.Request() + ) + + return len(self.nav2_active) == len(self.robot_layout) + + def tick(self): + now = time.monotonic() + if ( + not self.timeout_logged + and self.state not in ('complete', 'failed') + and now - self.started_at > self.timeout_sec + ): + timed_out_state = self.state + self.timeout_logged = True + self.state = 'failed' + self.get_logger().error( + f'Demo timed out in state {timed_out_state}; ' + f'initial plans={sorted(self.initial_versions)}, ' + f'replans={sorted(self.replan_versions)}' + ) + return + + if self.state == 'waiting_for_inputs': + action_servers_ready = all( + client.server_is_ready() + for client in self.navigation_clients.values() + ) and all( + client.server_is_ready() + for client in self.inner_navigation_clients.values() + ) + nav2_active = self.nav2_servers_are_active() + if ( + self.map_ready + and len(self.odometry) == len(self.robot_layout) + and action_servers_ready + and nav2_active + ): + for robot_name, (goal_x, goal_y) in self.robot_goals.items(): + goal = make_navigation_goal(goal_x, goal_y) + future = self.navigation_clients[robot_name].send_goal_async(goal) + future.add_done_callback( + lambda result, name=robot_name: self.receive_goal_response( + name, result + ) + ) + self.goal_futures[robot_name] = future + self.state = 'waiting_for_initial_plans' + self.get_logger().info('Sent navigation requests') + return + + if self.state == 'waiting_to_spawn' and now >= self.spawn_at: + if self.next_spawn_attempt is not None and now < self.next_spawn_attempt: + return + try: + result = spawn_bar(self.world_name, self.scenario) + except (OSError, subprocess.TimeoutExpired) as error: + self.get_logger().warning(f'Waiting to spawn obstacle: {error}') + self.next_spawn_attempt = now + 2.0 + return + + if not spawn_succeeded(result): + detail = result.stderr.strip() or result.stdout.strip() + if not detail: + detail = 'Gazebo did not confirm entity creation' + self.get_logger().warning(f'Waiting to spawn obstacle: {detail}') + self.next_spawn_attempt = now + 2.0 + return + + self.bar_spawned = True + self.state = 'waiting_for_replan' + self.get_logger().info( + f'Spawned bar at {self.scenario.bar_center}; ' + 'waiting for CODE_PATH_BLOCKED' + ) + + def publish_markers(self): + markers = [ + star_marker( + 100 + index, + *robot.goal, + color=ROBOT_COLORS[robot.name], + namespace=f'{robot.name}_goal', + ) + for index, robot in enumerate(self.scenario.robots) + ] + for index, (robot_name, initial_pose) in enumerate( + self.robot_layout.items() + ): + if robot_name in self.odometry: + pose = self.odometry[robot_name].pose.pose + x = pose.position.x + y = pose.position.y + yaw = yaw_from_odometry(self.odometry[robot_name]) + else: + x, y, yaw = initial_pose + color = ROBOT_COLORS[robot_name] + markers.append( + triangle_marker( + index, + f'{robot_name}_pose', + x, + y, + yaw, + color, + ) + ) + if self.bar_spawned: + markers.append(bar_marker(200, self.scenario)) + self.marker_publisher.publish(MarkerArray(markers=markers)) + + +def main(): + rclpy.init() + node = ReplanObstacleDemo() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py new file mode 100644 index 0000000..3d1d8b0 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py @@ -0,0 +1,292 @@ +# 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, + EmitEvent, + ExecuteProcess, + RegisterEventHandler, + TimerAction, +) +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 +from launch_ros.parameter_descriptions import ParameterValue +from nav2_common.launch import RewrittenYaml + +from .replan_scenarios import get_scenario + + +def _robots_argument(scenario): + """Format robot poses for Nav2 bringup.""" + return '; '.join( + f'{robot.name}={{x: {robot.pose[0]}, y: {robot.pose[1]}, ' + f'yaw: {robot.pose[2]}}}' + for robot in scenario.robots + ) + + +def generate_replan_launch_description(scenario_name): + """Build the replanning launch description.""" + scenario_config = get_scenario(scenario_name) + demo_share = get_package_share_directory('rmf_layered_map_server_demo') + nav2_share = get_package_share_directory('sp_demo_nav2_bringup') + + if scenario_name == 'simple': + default_map = os.path.join(demo_share, 'maps', 'single_room.yaml') + default_world = os.path.join(demo_share, 'worlds', 'single_room.sdf') + else: + default_map = os.path.join(nav2_share, 'maps', 'warehouse.yaml') + default_world = os.path.join( + get_package_share_directory('nav2_minimal_tb4_sim'), + 'worlds', + 'warehouse.sdf', + ) + + map_file = LaunchConfiguration('map') + world_file = LaunchConfiguration('world') + world_name = LaunchConfiguration('world_name') + params_file = LaunchConfiguration('params_file') + use_nav2_rviz = LaunchConfiguration('use_nav2_rviz') + use_global_rviz = LaunchConfiguration('use_global_rviz') + spawn_delay_sec = LaunchConfiguration('spawn_delay_sec') + scenario_timeout_sec = LaunchConfiguration('scenario_timeout_sec') + obstacle_memory_sec = LaunchConfiguration('obstacle_memory_sec') + self_filter_radius = LaunchConfiguration('self_filter_radius') + demo_params_file = RewrittenYaml( + source_file=params_file, + param_rewrites={ + ( + 'controller_server.ros__parameters.general_goal_checker.' + 'stateful' + ): 'False', + ( + 'controller_server.ros__parameters.general_goal_checker.' + 'xy_goal_tolerance' + ): '0.10', + ( + 'controller_server.ros__parameters.FollowPath.' + 'xy_goal_tolerance' + ): '0.10', + }, + convert_types=True, + ) + + declarations = [ + DeclareLaunchArgument( + 'map', + default_value=default_map, + description=f'Static map for the {scenario_name} scenario.', + ), + DeclareLaunchArgument( + 'world', + default_value=default_world, + description=f'Gazebo world for the {scenario_name} scenario.', + ), + DeclareLaunchArgument( + 'world_name', + default_value=scenario_config.world_name, + description='Gazebo world used by the obstacle spawner.', + ), + DeclareLaunchArgument( + 'params_file', + default_value=os.path.join( + nav2_share, 'params', 'nav2_multirobot_params_all.yaml' + ), + description='Nav2 parameters shared by both robots.', + ), + DeclareLaunchArgument( + 'use_nav2_rviz', + default_value='False', + description='Whether to open one local Nav2 RViz per robot.', + ), + DeclareLaunchArgument( + 'use_global_rviz', + default_value='True', + description='Whether to open the combined map and plan view.', + ), + DeclareLaunchArgument( + 'spawn_delay_sec', + default_value='1.0', + description='Delay between initial plans and obstacle spawn.', + ), + DeclareLaunchArgument( + 'scenario_timeout_sec', + default_value='180.0', + description='Timeout for observing a replan.', + ), + DeclareLaunchArgument( + 'obstacle_memory_sec', + default_value='-1.0', + description='Obstacle retention in seconds; negative keeps points.', + ), + DeclareLaunchArgument( + 'self_filter_radius', + default_value='0.22', + description='Radius excluded from each robot scan.', + ), + ] + + nav2_simulation = ExecuteProcess( + cmd=[ + 'ros2', + 'launch', + 'sp_demo_nav2_bringup', + 'cloned_multi_tb3_simulation_launch.py', + f'robots:={_robots_argument(scenario_config)}', + ['map:=', map_file], + ['world:=', world_file], + ['params_file:=', demo_params_file], + ['use_rviz:=', use_nav2_rviz], + 'use_navigation:=True', + 'staged_startup:=True', + ], + output='screen', + ) + + layered_map_server = Node( + package='rmf_layered_map_server', + executable='rmf_layered_map_server', + output='screen', + remappings=[('/map/static', '/robot0/inner/map')], + ) + region_visualizer = Node( + condition=IfCondition(use_global_rviz), + package='rmf_layered_map_server_demo', + executable='region_update_visualizer', + output='screen', + ) + observation_nodes = [ + Node( + package='rmf_layered_map_server_demo', + executable='scan_region_publisher', + namespace=f'{robot.name}/inner', + output='screen', + parameters=[{ + 'robot_name': robot.name, + 'map_frame': 'map', + 'map_name': scenario_config.map_name, + 'scan_topic': f'/{robot.name}/inner/scan', + 'beam_stride': 1, + 'publish_period_sec': 0.5, + 'ttl_sec': 10.0, + 'reset_source': True, + 'obstacle_memory_sec': ParameterValue( + obstacle_memory_sec, value_type=float + ), + 'obstacle_memory_resolution': 0.1, + 'self_filter_radius': ParameterValue( + self_filter_radius, value_type=float + ), + 'max_observation_range': scenario_config.scan_radius, + }], + remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], + ) + for robot in scenario_config.robots + ] + + path_server = Node( + package='rmf_path_server', + executable='rmf_path_server', + name='rmf_path_server', + output='both', + ) + destination_server = Node( + package='rmf_simple_destination_server', + executable='rmf_simple_destination_server', + name='rmf_simple_destination_server', + output='both', + ) + plan_executor = Node( + package='rmf_plan_executor', + executable='rmf_plan_executor', + name='rmf_plan_executor', + output='both', + ) + nav2_traffic = Node( + package='rmf_nav2_traffic', + executable='nav2_traffic', + name='nav2_traffic', + output='both', + parameters=[{'use_sim_time': True}], + ) + path_visualizer = Node( + condition=IfCondition(use_global_rviz), + package='rmf_path_visualizer', + executable='rmf_path_visualizer', + name='rmf_path_visualizer', + output='both', + ) + scenario = TimerAction( + period=2.0, + actions=[ + Node( + package='rmf_layered_map_server_demo', + executable='replan_obstacle_demo', + output='screen', + parameters=[{ + 'scenario': scenario_name, + 'world_name': ParameterValue(world_name, value_type=str), + 'spawn_delay_sec': ParameterValue( + spawn_delay_sec, value_type=float + ), + 'scenario_timeout_sec': ParameterValue( + scenario_timeout_sec, value_type=float + ), + }], + ), + ], + ) + + global_rviz = Node( + condition=IfCondition(use_global_rviz), + package='rviz2', + executable='rviz2', + name='replan_obstacle_rviz', + arguments=[ + '-d', + os.path.join(demo_share, 'rviz', 'nav2_observations.rviz'), + ], + parameters=[{'use_sim_time': False}], + output='screen', + ) + global_rviz_exit_handler = RegisterEventHandler( + condition=IfCondition(use_global_rviz), + event_handler=OnProcessExit( + target_action=global_rviz, + on_exit=EmitEvent(event=Shutdown(reason='global RViz exited')), + ), + ) + + return LaunchDescription([ + *declarations, + nav2_simulation, + layered_map_server, + region_visualizer, + *observation_nodes, + destination_server, + path_server, + plan_executor, + nav2_traffic, + path_visualizer, + scenario, + global_rviz, + global_rviz_exit_handler, + ]) diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_scenarios.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_scenarios.py new file mode 100644 index 0000000..276f68b --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_scenarios.py @@ -0,0 +1,91 @@ +# 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 dataclasses import dataclass +from math import pi + + +ROBOT_COLORS = { + 'robot0': (0.12, 0.55, 0.85, 1.0), + 'robot1': (1.0, 0.42, 0.08, 1.0), +} + + +@dataclass(frozen=True) +class RobotLayout: + """Initial pose and destination for one demo robot.""" + + name: str + pose: tuple[float, float, float] + goal: tuple[float, float] + + +@dataclass(frozen=True) +class ReplanScenario: + """Geometry and resource names for a replanning scenario.""" + + name: str + map_name: str + world_name: str + robots: tuple[RobotLayout, ...] + scan_radius: float + bar_name: str + bar_center: tuple[float, float] + bar_size: tuple[float, float, float] + + +SIMPLE_SCENARIO = ReplanScenario( + name='simple', + map_name='single_room', + world_name='single_room', + robots=( + RobotLayout('robot0', (-3.0, -4.5, pi / 2.0), (0.0, 6.0)), + RobotLayout('robot1', (5.5, -4.5, pi / 2.0), (6.0, 4.0)), + ), + scan_radius=4.0, + bar_name='simple_replan_bar', + bar_center=(1.75, -1.5), + bar_size=(15.5, 0.5, 1.0), +) + +WAREHOUSE_SCENARIO = ReplanScenario( + name='warehouse', + map_name='warehouse', + world_name='warehouse', + robots=( + RobotLayout('robot0', (-12.0, -21.0, pi / 2.0), (2.5, -15.0)), + RobotLayout('robot1', (3.5, -21.0, pi / 2.0), (3.5, -15.0)), + ), + scan_radius=3.5, + bar_name='warehouse_replan_bar', + bar_center=(-2.5, -18.5), + bar_size=(15.0, 0.5, 1.0), +) + + +SCENARIOS = { + SIMPLE_SCENARIO.name: SIMPLE_SCENARIO, + WAREHOUSE_SCENARIO.name: WAREHOUSE_SCENARIO, +} + + +def get_scenario(name): + """Return a scenario by name.""" + try: + return SCENARIOS[name] + except KeyError as error: + choices = ', '.join(sorted(SCENARIOS)) + raise ValueError( + f'Unknown replanning scenario {name!r}; choose one of: {choices}' + ) from error diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py index 96a0571..58843aa 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py @@ -12,6 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +from math import atan2, cos, sin + import rclpy from rclpy.node import Node from rclpy.qos import ( @@ -32,6 +34,95 @@ from .scan_conversion import scan_regions +def _yaw_from_quaternion(quaternion): + return atan2( + 2.0 * ( + quaternion.w * quaternion.z + + quaternion.x * quaternion.y + ), + 1.0 - 2.0 * ( + quaternion.y * quaternion.y + + quaternion.z * quaternion.z + ), + ) + + +class ObstacleMemory: + """Retain obstacle points across scan updates.""" + + def __init__( + self, + retention_sec=0.0, + resolution=0.1, + self_filter_radius=0.0, + ): + self.retention_sec = float(retention_sec) + self.resolution = float(resolution) + self.self_filter_radius = float(self_filter_radius) + self._points = {} + + @property + def enabled(self): + return self.retention_sec != 0.0 + + def remember(self, obstacle_points, transform, now_sec): + filter_radius_squared = self.self_filter_radius ** 2 + if self.self_filter_radius > 0.0: + obstacle_points = [ + (x, y) + for x, y in obstacle_points + if x * x + y * y > filter_radius_squared + ] + + if not self.enabled: + return list(obstacle_points) + + yaw = _yaw_from_quaternion(transform.rotation) + cosine = cos(yaw) + sine = sin(yaw) + tx = transform.translation.x + ty = transform.translation.y + + if self.retention_sec > 0.0: + oldest = now_sec - self.retention_sec + self._points = { + key: value + for key, value in self._points.items() + if value[2] > oldest + } + + for local_x, local_y in obstacle_points: + global_x = tx + cosine * local_x - sine * local_y + global_y = ty + sine * local_x + cosine * local_y + key = ( + round(global_x / self.resolution), + round(global_y / self.resolution), + ) + self._points[key] = (global_x, global_y, now_sec) + + if self.self_filter_radius > 0.0: + self._points = { + key: value + for key, value in self._points.items() + if ( + (value[0] - tx) ** 2 + + (value[1] - ty) ** 2 + > filter_radius_squared + ) + } + + # Region points are relative to the current robot pose. + remembered = [] + for global_x, global_y, _ in self._points.values(): + dx = global_x - tx + dy = global_y - ty + remembered.append(( + cosine * dx + sine * dy, + -sine * dx + cosine * dy, + )) + return remembered + + def make_scan_patches(clear_polygons, obstacle_points, ttl_sec): """Build clear and obstacle patches for one converted laser scan.""" patches = [] @@ -76,6 +167,15 @@ def __init__(self): self.reset_source = self.declare_parameter( 'reset_source', False ).value + self.obstacle_memory_sec = self.declare_parameter( + 'obstacle_memory_sec', 0.0 + ).value + self.obstacle_memory_resolution = self.declare_parameter( + 'obstacle_memory_resolution', 0.1 + ).value + self.self_filter_radius = self.declare_parameter( + 'self_filter_radius', 0.0 + ).value self.publish_period_sec = self.declare_parameter( 'publish_period_sec', 0.5 ).value @@ -92,8 +192,17 @@ def __init__(self): raise ValueError('publish_period_sec must not be negative') if self.beam_stride < 1: raise ValueError('beam_stride must be at least one') + if self.obstacle_memory_resolution <= 0.0: + raise ValueError('obstacle_memory_resolution must be positive') + if self.self_filter_radius < 0.0: + raise ValueError('self_filter_radius must not be negative') self.source_id = f'{self.robot_name}/scan' + self.obstacle_memory = ObstacleMemory( + self.obstacle_memory_sec, + self.obstacle_memory_resolution, + self.self_filter_radius, + ) self.last_publish_stamp_sec = None self.pending_scan = None self.publish_count = 0 @@ -121,7 +230,9 @@ def __init__(self): self.get_logger().info( f'Converting {self.scan_topic} into region updates for ' f'{self.robot_name} (stride={self.beam_stride}, ' - f'period={self.publish_period_sec:.2f}s)' + f'period={self.publish_period_sec:.2f}s, ' + f'obstacle_memory={self.obstacle_memory_sec:.1f}s, ' + f'self_filter_radius={self.self_filter_radius:.2f}m)' ) def publish_scan(self, scan): @@ -172,6 +283,11 @@ def publish_pending_scan(self): self.max_observation_range, self.beam_stride, ) + obstacle_points = self.obstacle_memory.remember( + obstacle_points, + transform.transform, + self.get_clock().now().nanoseconds / 1e9, + ) update = self.make_update(transform, clear_polygons, obstacle_points) self.region_update_publisher.publish(update) self.last_publish_stamp_sec = stamp_sec diff --git a/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz index 3babd04..607eae0 100644 --- a/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz +++ b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz @@ -7,6 +7,9 @@ Panels: - /Global Options1 - /Combined Global Map1 - /Region Contributions1 + - /Scenario Robots, Goals, and Obstacle1 + - /Robot 0 Nav2 Path1 + - /Robot 1 Nav2 Path1 Splitter Ratio: 0.5 Tree Height: 595 - Class: rviz_common/Views @@ -69,6 +72,102 @@ Visualization Manager: Reliability Policy: Reliable Value: /map/region_markers Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Scenario Robots, Goals, and Obstacle + Namespaces: + robot0_goal: true + robot0_pose: true + robot1_goal: true + robot1_pose: true + spawned_obstacle: true + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /replan_scenario/markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Robot 0 RMF Plan + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot0/path_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Robot 1 RMF Plan + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot1/path_markers + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 31; 140; 217 + Enabled: true + Head Diameter: 0.02 + Head Length: 0.02 + Length: 0.3 + Line Style: Lines + Line Width: 0.08 + Name: Robot 0 Nav2 Path + Offset: + X: 0 + Y: 0 + Z: 0.08 + Pose Color: 31; 140; 217 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.005 + Shaft Length: 0.02 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot0/inner/plan + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 107; 20 + Enabled: true + Head Diameter: 0.02 + Head Length: 0.02 + Length: 0.3 + Line Style: Lines + Line Width: 0.08 + Name: Robot 1 Nav2 Path + Offset: + X: 0 + Y: 0 + Z: 0.08 + Pose Color: 255; 107; 20 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.005 + Shaft Length: 0.02 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot1/inner/plan + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 diff --git a/map_server/rmf_layered_map_server_demo/setup.py b/map_server/rmf_layered_map_server_demo/setup.py index 39c6bc3..003c2fa 100644 --- a/map_server/rmf_layered_map_server_demo/setup.py +++ b/map_server/rmf_layered_map_server_demo/setup.py @@ -12,8 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os from glob import glob +import os from setuptools import find_packages, setup @@ -28,7 +28,9 @@ ['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, 'maps'), glob('maps/*')), (os.path.join('share', package_name, 'rviz'), glob('rviz/*.rviz')), + (os.path.join('share', package_name, 'worlds'), glob('worlds/*')), ], install_requires=['setuptools'], zip_safe=True, @@ -44,6 +46,8 @@ 'layered_map_demo_publisher = rmf_layered_map_server_demo.demo_publisher:main', 'nav2_goal_publisher = ' 'rmf_layered_map_server_demo.nav2_goal_publisher:main', + 'replan_obstacle_demo = ' + 'rmf_layered_map_server_demo.replan_obstacle_demo:main', 'region_update_visualizer = ' 'rmf_layered_map_server_demo.region_update_visualizer:main', 'scan_region_publisher = ' diff --git a/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py b/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py new file mode 100644 index 0000000..3456ce9 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py @@ -0,0 +1,195 @@ +# 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 math import hypot, pi +import subprocess + +import pytest +from rmf_layered_map_msgs.msg import MapRegionPatch, MapRegionUpdate +from rmf_layered_map_server_demo.region_update_visualizer import ( + RegionMarkerState, + SOURCE_COLORS, +) +from rmf_layered_map_server_demo.replan_obstacle_demo import ( + box_sdf, + GOAL_MARKER_Z, + make_navigation_goal, + ROBOT_COLORS, + ROBOT_MARKER_Z, + spawn_succeeded, + star_marker, + triangle_marker, +) +from rmf_layered_map_server_demo.replan_scenarios import ( + get_scenario, + SIMPLE_SCENARIO, + WAREHOUSE_SCENARIO, +) +from rmf_prototype_msgs.msg import Region +from visualization_msgs.msg import Marker + + +def test_navigation_goal_uses_the_map_frame(): + goal = make_navigation_goal(*SIMPLE_SCENARIO.robots[1].goal) + + assert goal.pose.header.frame_id == 'map' + assert goal.pose.pose.position.x == 6.0 + assert goal.pose.pose.position.y == 4.0 + assert goal.pose.pose.orientation.z == pytest.approx( + goal.pose.pose.orientation.w + ) + + +def test_box_sdf(): + sdf = box_sdf( + 'bar', + *SIMPLE_SCENARIO.bar_center, + *SIMPLE_SCENARIO.bar_size, + color=(0.95, 0.05, 0.05, 1.0), + ) + + assert "" in sdf + assert '15.5 0.5 1.0' in sdf + assert '0.95 0.05 0.05 1.0' in sdf + + +@pytest.mark.parametrize( + ('returncode', 'stdout', 'expected'), + ( + (0, 'data: true', True), + (0, 'data: false', False), + (1, 'data: true', False), + ), +) +def test_spawn_succeeded_requires_a_positive_reply( + returncode, + stdout, + expected, +): + result = subprocess.CompletedProcess([], returncode, stdout, '') + + assert spawn_succeeded(result) is expected + + +def _region_update(robot_name): + update = MapRegionUpdate() + update.source.header.stamp.sec = 1 + update.source.header.frame_id = 'map' + update.source.source_id = f'{robot_name}/scan' + update.source.robot_name = robot_name + update.source.map_name = 'single_room' + update.source.robot_pose.orientation.w = 1.0 + update.reset_source = True + patch = MapRegionPatch() + patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + region = Region() + region.hint = Region.HINT_POINT + region.points = [1.0, 2.0] + patch.regions = [region] + update.patches = [patch] + return update + + +def test_region_colors_match_robots_regardless_of_arrival_order(): + state = RegionMarkerState() + + robot1_marker = state.apply_update(_region_update('robot1')).markers[0] + robot0_marker = state.apply_update(_region_update('robot0')).markers[0] + other_marker = state.apply_update(_region_update('other_robot')).markers[0] + + for marker, color in ( + (robot0_marker, ROBOT_COLORS['robot0'][:3]), + (robot1_marker, ROBOT_COLORS['robot1'][:3]), + (other_marker, SOURCE_COLORS[2]), + ): + assert ( + marker.color.r, + marker.color.g, + marker.color.b, + ) == pytest.approx(color) + + +def test_scenario_markers(): + triangle = triangle_marker( + 1, + 'robot', + 5.5, + -4.5, + pi / 2.0, + (1.0, 0.4, 0.0, 0.8), + ) + star = star_marker( + 2, + *SIMPLE_SCENARIO.robots[1].goal, + color=ROBOT_COLORS['robot1'], + ) + + assert triangle.type == Marker.TRIANGLE_LIST + assert len(triangle.points) == 3 + assert triangle.points[0].y > -4.5 + assert all(point.z == ROBOT_MARKER_Z for point in triangle.points) + assert triangle.scale.x == 1.0 + assert triangle.scale.y == 1.0 + assert triangle.scale.z == 1.0 + assert star.type == Marker.TRIANGLE_LIST + assert len(star.points) == 30 + assert all(point.z == GOAL_MARKER_Z for point in star.points) + assert ( + star.color.r, + star.color.g, + star.color.b, + star.color.a, + ) == ROBOT_COLORS['robot1'] + assert star.scale.x == 1.0 + + +def test_simple_room_dead_end_layout(): + robot0, robot1 = SIMPLE_SCENARIO.robots + center_distance = hypot( + robot1.pose[0] - robot0.pose[0], + robot1.pose[1] - robot0.pose[1], + ) + assert center_distance > 2.0 * SIMPLE_SCENARIO.scan_radius + + bar_left = ( + SIMPLE_SCENARIO.bar_center[0] - SIMPLE_SCENARIO.bar_size[0] / 2.0 + ) + bar_right = ( + SIMPLE_SCENARIO.bar_center[0] + SIMPLE_SCENARIO.bar_size[0] / 2.0 + ) + assert bar_left == -6.0 + assert bar_right == 9.5 + + for robot in SIMPLE_SCENARIO.robots: + closest_x = min(max(robot.pose[0], bar_left), bar_right) + distance = hypot( + closest_x - robot.pose[0], + SIMPLE_SCENARIO.bar_center[1] - robot.pose[1], + ) + assert distance < SIMPLE_SCENARIO.scan_radius + + goal0, goal1 = (robot.goal for robot in SIMPLE_SCENARIO.robots) + goal_distance = hypot(goal1[0] - goal0[0], goal1[1] - goal0[1]) + assert goal0 == (0.0, 6.0) + assert goal1 == (6.0, 4.0) + assert goal_distance > 6.0 + assert goal0[1] != goal1[1] + + +def test_warehouse_scenario_layout(): + assert get_scenario('warehouse') is WAREHOUSE_SCENARIO + assert WAREHOUSE_SCENARIO.robots[0].pose[:2] == (-12.0, -21.0) + assert WAREHOUSE_SCENARIO.robots[1].pose[:2] == (3.5, -21.0) + assert WAREHOUSE_SCENARIO.bar_center == (-2.5, -18.5) + assert WAREHOUSE_SCENARIO.bar_size == (15.0, 0.5, 1.0) diff --git a/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py index b6394d0..81aac18 100644 --- a/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py +++ b/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py @@ -12,11 +12,23 @@ # See the License for the specific language governing permissions and # limitations under the License. +from geometry_msgs.msg import Transform from rmf_layered_map_msgs.msg import MapRegionPatch -from rmf_layered_map_server_demo.scan_region_publisher import make_scan_patches +from rmf_layered_map_server_demo.scan_region_publisher import ( + make_scan_patches, + ObstacleMemory, +) from rmf_prototype_msgs.msg import Region +def _transform(x=0.0, y=0.0): + transform = Transform() + transform.translation.x = x + transform.translation.y = y + transform.rotation.w = 1.0 + return transform + + def test_scan_patches_clear_rays_before_marking_obstacle_endpoints(): patches = make_scan_patches( [(0.0, 0.0, 1.0, -0.1, 1.0, 0.1)], @@ -32,3 +44,66 @@ def test_scan_patches_clear_rays_before_marking_obstacle_endpoints(): assert patches[0].regions[0].hint == Region.HINT_CONVEX_POLYGON assert patches[1].occupancy_value == 100 assert patches[1].regions[0].hint == Region.HINT_POINT + + +def test_obstacle_memory_filters_points_inside_robot_body(): + memory = ObstacleMemory( + retention_sec=0.0, + self_filter_radius=0.22, + ) + + points = memory.remember( + [(0.1, 0.0), (0.3, 0.0)], + _transform(), + now_sec=1.0, + ) + + assert points == [(0.3, 0.0)] + + +def test_obstacle_memory_prunes_points_the_robot_moves_over(): + memory = ObstacleMemory( + retention_sec=-1.0, + self_filter_radius=0.22, + ) + assert memory.remember( + [(1.0, 0.0)], + _transform(), + now_sec=1.0, + ) == [(1.0, 0.0)] + + assert memory.remember( + [], + _transform(x=1.0), + now_sec=2.0, + ) == [] + + +def test_obstacle_memory_keeps_points_when_the_scan_window_moves(): + memory = ObstacleMemory(retention_sec=-1.0, resolution=0.1) + first = memory.remember( + [(1.0, 0.0), (2.0, 0.0)], + _transform(), + now_sec=1.0, + ) + moved = memory.remember( + [(1.0, 0.0)], + _transform(x=5.0), + now_sec=2.0, + ) + + assert sorted(first) == [(1.0, 0.0), (2.0, 0.0)] + assert sorted(moved) == [(-4.0, 0.0), (-3.0, 0.0), (1.0, 0.0)] + + +def test_finite_obstacle_memory_expires_old_scan_windows(): + memory = ObstacleMemory(retention_sec=5.0, resolution=0.1) + memory.remember([(1.0, 0.0)], _transform(), now_sec=1.0) + + remembered = memory.remember( + [(2.0, 0.0)], + _transform(), + now_sec=7.0, + ) + + assert remembered == [(2.0, 0.0)] diff --git a/map_server/rmf_layered_map_server_demo/worlds/single_room.sdf b/map_server/rmf_layered_map_server_demo/worlds/single_room.sdf new file mode 100644 index 0000000..52664ef --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/worlds/single_room.sdf @@ -0,0 +1,74 @@ + + + + + 0.003 + 1000.0 + 1.0 + + + + + + + ogre2 + + + + + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + -0.5 0.1 -0.9 + + + + true + + + 0 0 1100 100 + + + 0 0 1100 100 + 0.8 0.8 0.8 10.8 0.8 0.8 1 + + + + + + true + -9.75 0 0.5 0 0 0 + + 0.5 16 1 + 0.5 16 1 + + + + true + 9.75 0 0.5 0 0 0 + + 0.5 16 1 + + 0.5 16 1 + 0.05 0.35 0.8 10.05 0.35 0.8 1 + + + + + true + 0 -7.75 0.5 0 0 0 + + 20 0.5 1 + 20 0.5 1 + + + + true + 0 7.75 0.5 0 0 0 + + 20 0.5 1 + 20 0.5 1 + + + + diff --git a/nav2_integration/README.md b/nav2_integration/README.md index 4285d29..54d7ef8 100644 --- a/nav2_integration/README.md +++ b/nav2_integration/README.md @@ -51,10 +51,11 @@ graph TD - **`async_request_new_goal`**: An asynchronous service that sends a new `NavigateToPose` goal to Nav2. - **`update_goal_client`**: Updates the `InnerNavigationClient` component with the new goal handle. - **`async_monitor_ongoing_navigation`**: Monitors the progress of the navigation goal, handling feedback and final results (Succeeded, Aborted, Cancelled). -- **`process_navigation_result`**: Processes the final result of the navigation request. If aborted, it prepares to retry by requesting a new goal; otherwise, it passes the result for cleanup. +- **`process_navigation_result`**: Publishes `CODE_PATH_BLOCKED` for an aborted current goal and ignores results from superseded goals. - **`cleanup_goal_client`**: Cleans up the goal client state in the component upon completion or failure. - **`log_inner_navigation_error`**: Logs any errors encountered during goal cancellation or request. +An unchanged incremental target is resent when it belongs to a new plan, while cancellation or abort results from an older goal cannot trigger another replan. ## NavigationServices Workflow Diagram @@ -203,4 +204,4 @@ ros2 action send_goal robot1/navigate_to_pose nav2_msgs/action/NavigateToPose "{ } } }" -``` \ No newline at end of file +``` diff --git a/nav2_integration/demo_world/demo_world/staged_nav2_init.py b/nav2_integration/demo_world/demo_world/staged_nav2_init.py new file mode 100644 index 0000000..e230d35 --- /dev/null +++ b/nav2_integration/demo_world/demo_world/staged_nav2_init.py @@ -0,0 +1,216 @@ +"""Start a simulated robot's Nav2 stack in readiness order.""" + +from math import cos, sin +import sys + +from lifecycle_msgs.msg import State +from lifecycle_msgs.srv import GetState + +from nav2_msgs.srv import ManageLifecycleNodes, SetInitialPose + +from nav_msgs.msg import Odometry + +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.node import Node +from rclpy.task import Future +from rclpy.time import Time + +from tf2_ros import Buffer, TransformListener + + +class StagedNav2Init(Node): + """Activate Nav2 after the simulated robot and transforms are ready.""" + + def __init__(self, x, y, yaw): + """Initialize the coordinator.""" + super().__init__('staged_nav2_init') + self.x = x + self.y = y + self.yaw = yaw + self.done = Future() + self.stage = 'waiting_for_robot' + self.pending = False + self.odom_received = False + + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.create_subscription(Odometry, 'odom', self.receive_odometry, 10) + self.localization_manager = self.create_client( + ManageLifecycleNodes, + 'lifecycle_manager_localization/manage_nodes', + ) + self.navigation_manager = self.create_client( + ManageLifecycleNodes, + 'lifecycle_manager_navigation/manage_nodes', + ) + self.amcl_state = self.create_client(GetState, 'amcl/get_state') + self.bt_state = self.create_client(GetState, 'bt_navigator/get_state') + self.pose_client = self.create_client( + SetInitialPose, + 'set_initial_pose', + ) + self.timer = self.create_timer(0.5, self.tick) + + def receive_odometry(self, _): + """Record that Gazebo is publishing odometry.""" + self.odom_received = True + + def transform_ready(self, target, source): + """Check whether a transform is available.""" + return self.tf_buffer.can_transform(target, source, Time()) + + def start_manager(self, client, next_stage, label): + """Request lifecycle startup.""" + if not client.service_is_ready(): + return + request = ManageLifecycleNodes.Request() + request.command = ManageLifecycleNodes.Request.STARTUP + self.pending = True + future = client.call_async(request) + future.add_done_callback( + lambda result: self.manager_started( + result, + next_stage, + label, + ) + ) + + def manager_started(self, future, next_stage, label): + """Handle a lifecycle startup response.""" + self.pending = False + try: + response = future.result() + except Exception as error: + self.get_logger().warning(f'Cannot start {label}: {error}') + return + if not response.success: + self.get_logger().warning(f'{label} startup failed; retrying') + return + self.stage = next_stage + + def wait_for_active(self, client, next_stage): + """Poll a lifecycle node until it is active.""" + if not client.service_is_ready(): + return + self.pending = True + future = client.call_async(GetState.Request()) + future.add_done_callback( + lambda result: self.state_received(result, next_stage) + ) + + def state_received(self, future, next_stage): + """Handle a lifecycle state response.""" + self.pending = False + try: + state = future.result().current_state + except Exception as error: + self.get_logger().warning(f'Cannot read lifecycle state: {error}') + return + if state.id == State.PRIMARY_STATE_ACTIVE: + self.stage = next_stage + + def send_pose(self): + """Send the robot's initial pose.""" + if not self.pose_client.service_is_ready(): + return + request = SetInitialPose.Request() + request.pose.header.frame_id = 'map' + request.pose.header.stamp = self.get_clock().now().to_msg() + request.pose.pose.pose.position.x = self.x + request.pose.pose.pose.position.y = self.y + request.pose.pose.pose.orientation.z = sin(self.yaw / 2.0) + request.pose.pose.pose.orientation.w = cos(self.yaw / 2.0) + request.pose.pose.covariance = [0.1] * 36 + self.pending = True + future = self.pose_client.call_async(request) + future.add_done_callback(self.pose_sent) + + def pose_sent(self, future): + """Continue after setting the initial pose.""" + self.pending = False + try: + future.result() + except Exception as error: + self.get_logger().warning(f'Cannot set initial pose: {error}') + return + self.stage = 'waiting_for_map_transform' + + def tick(self): + """Advance the startup sequence.""" + if self.pending: + return + + if self.stage == 'waiting_for_robot': + if ( + self.odom_received + and self.transform_ready('odom', 'base_link') + ): + self.get_logger().info( + 'Robot transforms are ready; starting localization' + ) + self.stage = 'starting_localization' + return + + if self.stage == 'starting_localization': + self.start_manager( + self.localization_manager, + 'waiting_for_amcl', + 'localization', + ) + return + + if self.stage == 'waiting_for_amcl': + self.wait_for_active(self.amcl_state, 'setting_pose') + return + + if self.stage == 'setting_pose': + self.send_pose() + return + + if self.stage == 'waiting_for_map_transform': + if self.transform_ready('map', 'base_link'): + self.get_logger().info( + 'Localization is ready; starting navigation' + ) + self.stage = 'starting_navigation' + return + + if self.stage == 'starting_navigation': + self.start_manager( + self.navigation_manager, + 'waiting_for_navigation', + 'navigation', + ) + return + + if self.stage == 'waiting_for_navigation': + self.wait_for_active(self.bt_state, 'ready') + return + + if self.stage == 'ready': + self.get_logger().info('Nav2 startup complete') + self.timer.cancel() + self.done.set_result(True) + + +def main(): + """Run the staged startup coordinator.""" + rclpy.init() + x = float(sys.argv[1]) if len(sys.argv) > 1 else 0.0 + y = float(sys.argv[2]) if len(sys.argv) > 2 else 0.0 + yaw = float(sys.argv[3]) if len(sys.argv) > 3 else 0.0 + node = StagedNav2Init(x, y, yaw) + executor = SingleThreadedExecutor() + executor.add_node(node) + try: + executor.spin_until_future_complete(node.done) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/nav2_integration/demo_world/package.xml b/nav2_integration/demo_world/package.xml index 0c00df3..591cda7 100644 --- a/nav2_integration/demo_world/package.xml +++ b/nav2_integration/demo_world/package.xml @@ -7,6 +7,12 @@ arjoc TODO: License declaration + lifecycle_msgs + nav2_msgs + nav_msgs + rclpy + tf2_ros + ament_copyright ament_flake8 ament_pep257 diff --git a/nav2_integration/demo_world/setup.py b/nav2_integration/demo_world/setup.py index 202f7d0..bfc42c0 100644 --- a/nav2_integration/demo_world/setup.py +++ b/nav2_integration/demo_world/setup.py @@ -21,6 +21,7 @@ entry_points={ 'console_scripts': [ 'set_init = demo_world.localization_init:main', + 'staged_nav2_init = demo_world.staged_nav2_init:main', 'robot_client = demo_world.path_reporter:main', ], }, diff --git a/nav2_integration/rmf_nav2_traffic/src/inner_navigation_client.rs b/nav2_integration/rmf_nav2_traffic/src/inner_navigation_client.rs index d90f0ec..6f798f9 100644 --- a/nav2_integration/rmf_nav2_traffic/src/inner_navigation_client.rs +++ b/nav2_integration/rmf_nav2_traffic/src/inner_navigation_client.rs @@ -1,4 +1,4 @@ -use crate::Nav2Agent; +use crate::{safe_zone::PlanErrorPublisher, Nav2Agent}; use bevy::prelude::*; use bevy_ros2::{RclrsExecutorCommands, RclrsNode, RosActionClient}; use crossflow::{prelude::*, service::Service}; @@ -40,6 +40,7 @@ impl InnerNavigationTarget { pub struct ActiveInnerGoal { pub goal_client: GoalClient, pub safe_zone_id: SafeZoneId, + superseded: bool, } impl ActiveInnerGoal { @@ -47,6 +48,7 @@ impl ActiveInnerGoal { Self { goal_client, safe_zone_id, + superseded: false, } } @@ -61,6 +63,14 @@ impl ActiveInnerGoal { pub fn id(&self) -> &SafeZoneId { &self.safe_zone_id } + + pub fn is_superseded(&self) -> bool { + self.superseded + } + + pub fn mark_superseded(&mut self) { + self.superseded = true; + } } #[derive(Component, Clone)] @@ -377,7 +387,7 @@ fn await_external_cancellation( srv: ContinuousService<(), (), StreamOf>, mut orders: ContinuousQuery<(), (), StreamOf>, mut cancel_requests: EventReader, - inner_nav_clients: Query<&InnerNavigationClient>, + mut inner_nav_clients: Query<&mut InnerNavigationClient>, ) { let Some(mut orders) = orders.get_mut(&srv.key) else { return; @@ -391,14 +401,14 @@ fn await_external_cancellation( "Received external cancellation request for agent {:?}", request.agent.index() ); - let Some(existing_goal) = inner_nav_clients - .get(request.agent) - .ok() - .and_then(|inner_client| inner_client.goal().as_ref()) - else { + let Ok(mut inner_client) = inner_nav_clients.get_mut(request.agent) else { continue; }; - let client = existing_goal.client(); + let Some(existing_goal) = inner_client.goal_mut().as_mut() else { + continue; + }; + existing_goal.mark_superseded(); + let client = existing_goal.client().clone(); orders.for_each(|order| { order.streams().send(CancelInnerNavigation { agent: request.agent, @@ -423,16 +433,15 @@ enum CheckExistingGoalResult { /// workflow can proceed to request a new goal without cancellation. fn check_existing_goal( Blocking { request, .. }: Blocking, - inner_nav_clients: Query<&InnerNavigationClient>, + mut inner_nav_clients: Query<&mut InnerNavigationClient>, ) -> Result { // TODO(@xiyuoh) Create a replan mechanism instead of cancelling goal on every // new request let mut replan_and_cancel = false; - let Some(existing_goal) = inner_nav_clients - .get(request.agent) - .ok() - .and_then(|inner_client| inner_client.goal().as_ref()) - else { + let Ok(mut inner_client) = inner_nav_clients.get_mut(request.agent) else { + return Ok(CheckExistingGoalResult::NoExistingGoal(request)); + }; + let Some(existing_goal) = inner_client.goal_mut().as_mut() else { // No existing goal, proceed to request for new goal return Ok(CheckExistingGoalResult::NoExistingGoal(request)); }; @@ -453,14 +462,15 @@ fn check_existing_goal( replan_and_cancel = true; } - let client = existing_goal.client(); - if replan_and_cancel { + // Do not treat cancellation of this goal as a new blockage. + existing_goal.mark_superseded(); + let client = existing_goal.client().clone(); return Ok(CheckExistingGoalResult::ReplanAndCancel( CancelInnerNavigation { agent: request.agent, new_request: Some(request), - cancel_client: client.clone(), + cancel_client: client, }, )); } @@ -725,6 +735,7 @@ fn process_navigation_result( }: Blocking, mut cancelling_inner: Query<&mut CancellingInnerNavigation>, inner_nav_clients: Query<&InnerNavigationClient>, + plan_error_publishers: Query<&PlanErrorPublisher>, ) -> Result { match result { Ok(_) => return Err(result), @@ -734,26 +745,50 @@ fn process_navigation_result( return Err(result); }; let target = &handle.request; - let target_pose = target.target_pose.clone(); + let active_goal = inner_nav_clients + .get(target.agent) + .ok() + .and_then(|inner_client| inner_client.goal().as_ref()); + + if !should_publish_path_blocked( + &target.safe_zone_id, + active_goal.map(|goal| (goal.id(), goal.is_superseded())), + ) { + info!( + "[{:?}] Ignoring stale Nav2 abort {:?}", + target.agent.index(), + target.safe_zone_id, + ); + return Err(result); + } - if let Ok(inner_nav_client) = inner_nav_clients.get(target.agent) { - if let Some(active_goal) = inner_nav_client.goal() { - if active_goal.id() != &target.safe_zone_id { - debug!( - "[{:?}] Aborted goal is stale, not retrying", + if let Ok(publisher) = plan_error_publishers.get(target.agent) { + let plan_error = ros_env::rmf_prototype_msgs::msg::PlanError { + error: ros_env::rmf_prototype_msgs::msg::Error { + code: ros_env::rmf_prototype_msgs::msg::PlanError::CODE_PATH_BLOCKED, + message: format!( + "Nav2 goal aborted for agent {:?}: path blocked within safe zone", target.agent.index() - ); - return Err(result); - } + ), + parameters: String::new(), + }, + plan_id: handle.request.safe_zone_id.plan_id.clone(), + }; + if let Err(e) = publisher.publisher.publish(plan_error) { + error!( + "Failed to publish PlanError for agent {:?}: {:?}", + target.agent.index(), + e + ); + } else { + warn!( + "[{:?}] Goal aborted by Nav2! Published PlanError CODE_PATH_BLOCKED to ~/plan/error.", + target.agent.index() + ); } } - debug!("[{:?}] Goal aborted. Retrying", target.agent.index()); - return Ok(InnerNavigationRequest { - agent: target.agent, - safe_zone_id: target.safe_zone_id.clone(), - target_pose, - }); + return Err(result); } InnerNavigationErrorKind::GoalCancelledError => { // Only mark cancellation success for external cancellation @@ -774,6 +809,16 @@ fn process_navigation_result( return Err(result); } +fn should_publish_path_blocked( + completed_goal_id: &SafeZoneId, + active_goal: Option<(&SafeZoneId, bool)>, +) -> bool { + matches!( + active_goal, + Some((active_goal_id, false)) if active_goal_id == completed_goal_id + ) +} + /// Clears existing goal clients for this agent after verifying that the /// completed goal matches the currently tracked active goal, to avoid /// accidentally clearing a newly requested goal. @@ -803,9 +848,8 @@ fn cleanup_goal_client( if is_current { inner_nav_client.reset_goal(); } else { - warn!( - "[{:?}] Found an incompatible SafeZoneId {:?} while attempting - to cleanup goal client!", + debug!( + "[{:?}] Ignoring stale Nav2 goal cleanup {:?}", handle.request.agent.index(), handle.request.safe_zone_id, ); @@ -817,3 +861,65 @@ fn cleanup_goal_client( ); } } + +#[cfg(test)] +mod tests { + use super::*; + + fn safe_zone_id(session: u8, plan_version: u64, safe_zone_version: u64) -> SafeZoneId { + let mut id = SafeZoneId::default(); + id.plan_id.destination_session.uuid[0] = session; + id.plan_id.plan_version = plan_version; + id.safe_zone_version = safe_zone_version; + id + } + + #[test] + fn current_abort_publishes_path_blocked() { + let completed = safe_zone_id(1, 3, 7); + + assert!(should_publish_path_blocked( + &completed, + Some((&completed, false)), + )); + } + + #[test] + fn intentionally_superseded_abort_is_ignored() { + let completed = safe_zone_id(1, 3, 7); + + assert!(!should_publish_path_blocked( + &completed, + Some((&completed, true)), + )); + } + + #[test] + fn abort_from_older_plan_is_ignored() { + let completed = safe_zone_id(1, 3, 7); + let active = safe_zone_id(1, 4, 1); + + assert!(!should_publish_path_blocked( + &completed, + Some((&active, false)), + )); + } + + #[test] + fn abort_from_older_safe_zone_is_ignored() { + let completed = safe_zone_id(1, 3, 7); + let active = safe_zone_id(1, 3, 8); + + assert!(!should_publish_path_blocked( + &completed, + Some((&active, false)), + )); + } + + #[test] + fn abort_without_an_active_goal_is_ignored() { + let completed = safe_zone_id(1, 3, 7); + + assert!(!should_publish_path_blocked(&completed, None)); + } +} diff --git a/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs b/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs index f5a8b3d..6dc369a 100644 --- a/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs +++ b/nav2_integration/rmf_nav2_traffic/src/safe_zone.rs @@ -6,7 +6,7 @@ use bevy::prelude::*; use bevy_ros2::{RclrsNode, RosPublisher, RosSubscription}; use ros_env::{ nav2_msgs::msg::Costmap, - rmf_prototype_msgs::msg::{Progress, Region, SafeZone}, + rmf_prototype_msgs::msg::{PlanError, Progress, Region, SafeZone}, }; use std::sync::Arc; @@ -25,6 +25,11 @@ pub struct ProgressPublisher { pub publisher: Arc>, } +#[derive(Component)] +pub struct PlanErrorPublisher { + pub publisher: Arc>, +} + #[derive(Component, Debug, Clone, Default, Deref)] pub struct CurrentSafeZone(pub Option); @@ -41,13 +46,25 @@ impl CurrentSafeZone { self.as_ref().is_some_and(|sz| sz.id == other.id) } + pub fn should_update_target(&self, other: &SafeZone) -> bool { + let Some(current) = self.as_ref() else { + return true; + }; + + // Resend an unchanged target when it belongs to a new plan. + if current.id.plan_id != other.id.plan_id { + return true; + } + + self.distancesq_to_target(other) >= 0.5 + } + pub fn distancesq_to_target(&self, other: &SafeZone) -> f64 { - let Some(safe_zone) = self.0.clone() else { - // TODO(arjoc): Clean up lifetimes + let Some(safe_zone) = self.as_ref() else { return f64::INFINITY; }; - let Some((sx, sy)) = Self::get_point(&safe_zone) else { + let Some((sx, sy)) = Self::get_point(safe_zone) else { return f64::INFINITY; }; @@ -86,7 +103,8 @@ impl Plugin for SafeZoneSubscriptionPlugin { app.add_systems(PreUpdate, update_incremental_target) .add_observer(create_safe_zone_subscriber) .add_observer(create_costmap_publisher) - .add_observer(create_progress_publisher); + .add_observer(create_progress_publisher) + .add_observer(create_plan_error_publisher); } } @@ -145,6 +163,23 @@ fn create_progress_publisher( }); } +fn create_plan_error_publisher( + trigger: Trigger, + mut commands: Commands, + agents: Query<&Nav2Agent>, + node: Res, +) { + let e = trigger.target(); + let Ok(agent_name) = agents.get(e).map(|agent| agent.name.clone()) else { + return; + }; + let topic = agent_name + "/plan/error"; + let publisher = Arc::new(RosPublisher::::new(&node, topic)); + commands.entity(e).insert(PlanErrorPublisher { + publisher: Arc::clone(&publisher), + }); +} + fn update_incremental_target( mut nav_target: EventWriter, mut subscriptions: Query< @@ -198,7 +233,7 @@ fn update_incremental_target( continue; }; - if current_safe_zone.distancesq_to_target(&safe_zone) < 0.5 { + if !current_safe_zone.should_update_target(&safe_zone) { continue; } debug!( @@ -284,3 +319,68 @@ fn next_target(safe_zone: &SafeZone) -> Option<(f32, f32, f32)> { xy.zip(yaw).map(|((x, y), yaw)| (x, y, yaw)) } + +#[cfg(test)] +mod tests { + use super::*; + use ros_env::rmf_prototype_msgs::msg::TargetRegion; + + fn safe_zone( + session: u8, + plan_version: u64, + safe_zone_version: u64, + x: f32, + y: f32, + ) -> SafeZone { + let mut safe_zone = SafeZone::default(); + safe_zone.id.plan_id.destination_session.uuid[0] = session; + safe_zone.id.plan_id.plan_version = plan_version; + safe_zone.id.safe_zone_version = safe_zone_version; + + let mut target = TargetRegion::default(); + target.region.hint = Region::HINT_POINT; + target.region.points = vec![x, y]; + safe_zone.incremental_target.regions.push(target); + safe_zone + } + + #[test] + fn first_target_is_sent() { + let current = CurrentSafeZone::default(); + let next = safe_zone(1, 0, 0, 4.0, 4.0); + + assert!(current.should_update_target(&next)); + } + + #[test] + fn nearby_target_from_same_plan_is_suppressed() { + let current = CurrentSafeZone(Some(safe_zone(1, 3, 7, 4.0, 4.0))); + let next = safe_zone(1, 3, 8, 4.25, 4.0); + + assert!(!current.should_update_target(&next)); + } + + #[test] + fn moved_target_from_same_plan_is_sent() { + let current = CurrentSafeZone(Some(safe_zone(1, 3, 7, 4.0, 4.0))); + let next = safe_zone(1, 3, 8, 5.0, 4.0); + + assert!(current.should_update_target(&next)); + } + + #[test] + fn same_target_from_new_plan_is_sent() { + let current = CurrentSafeZone(Some(safe_zone(1, 3, 7, 4.0, 4.0))); + let next = safe_zone(1, 4, 0, 4.0, 4.0); + + assert!(current.should_update_target(&next)); + } + + #[test] + fn same_target_from_new_destination_session_is_sent() { + let current = CurrentSafeZone(Some(safe_zone(1, 3, 7, 4.0, 4.0))); + let next = safe_zone(2, 3, 0, 4.0, 4.0); + + assert!(current.should_update_target(&next)); + } +} diff --git a/nav2_integration/sp_demo_nav2_bringup/README.md b/nav2_integration/sp_demo_nav2_bringup/README.md index da45591..16294bf 100644 --- a/nav2_integration/sp_demo_nav2_bringup/README.md +++ b/nav2_integration/sp_demo_nav2_bringup/README.md @@ -24,13 +24,12 @@ This is how to launch multi-robot simulation with simple command line. Please se #### Cloned -This allows to bring up multiple robots, cloning a single robot N times at different positions in the map. The parameter are loaded from `nav2_multirobot_params_all.yaml` file by default. -The multiple robots that consists of name and initial pose in YAML format will be set on the command-line. The format for each robot is `robot_name={x: 0.0, y: 0.0, yaw: 0.0, roll: 0.0, pitch: 0.0, yaw: 0.0}`. +This launch file clones one robot at multiple poses and loads `nav2_multirobot_params_all.yaml` by default. Pass each robot as `robot_name={x: 0.0, y: 0.0, z: 0.0, roll: 0.0, pitch: 0.0, yaw: 0.0}`. -Please refer to below examples. +Set `staged_startup:=True` to start robots sequentially. Each stack waits for odometry and transforms before localization, initial-pose, and navigation activation. ```shell -ros2 launch nav2_bringup cloned_multi_tb3_simulation_launch.py robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" +ros2 launch sp_demo_nav2_bringup cloned_multi_tb3_simulation_launch.py robots:="robot1={x: 1.0, y: 1.0, yaw: 1.5707}; robot2={x: 1.0, y: 1.0, yaw: 1.5707}" ``` #### Unique diff --git a/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index 0b06c61..f655f04 100644 --- a/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -30,8 +30,8 @@ OpaqueFunction, RegisterEventHandler, ) -from launch.conditions import IfCondition -from launch.event_handlers import OnShutdown +from launch.conditions import IfCondition, UnlessCondition +from launch.event_handlers import OnProcessExit, OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, TextSubstitution from launch_ros.actions import Node @@ -64,6 +64,7 @@ def generate_launch_description(): use_robot_state_pub = LaunchConfiguration('use_robot_state_pub') use_rviz = LaunchConfiguration('use_rviz') use_navigation = LaunchConfiguration('use_navigation') + staged_startup = LaunchConfiguration('staged_startup') log_settings = LaunchConfiguration('log_settings', default='true') # Declare the launch arguments @@ -115,6 +116,12 @@ def generate_launch_description(): description='Whether to enable the Nav2 planning and control stack', ) + declare_staged_startup_cmd = DeclareLaunchArgument( + 'staged_startup', + default_value='False', + description='Start each robot stack after its transforms are ready.', + ) + # Start Gazebo with plugin providing the robot spawning service world_sdf = tempfile.mktemp(prefix='nav2_', suffix='.sdf') world_sdf_xacro = ExecuteProcess( @@ -133,11 +140,14 @@ def generate_launch_description(): # Define commands for launching the navigation instances bringup_cmd_group = [] + staged_groups = [] + staged_init_nodes = [] for robot_index, robot_name in enumerate(robots_list): init_pose = robots_list[robot_name] namespace = robot_name + '/inner' - group = GroupAction( - [ + + def make_group(robot_autostart, condition, ready_node=None): + actions = [ LogInfo( msg=[ 'Launching namespace=', @@ -168,7 +178,7 @@ def generate_launch_description(): 'map': map_yaml_file, 'use_sim_time': 'True', 'params_file': params_file, - 'autostart': autostart, + 'autostart': robot_autostart, 'use_rviz': 'False', 'use_simulator': 'False', 'headless': 'False', @@ -191,9 +201,29 @@ def generate_launch_description(): }.items(), ), ] - ) + if ready_node is not None: + actions.append(ready_node) + return GroupAction(actions, condition=condition) - bringup_cmd_group.append(group) + bringup_cmd_group.append( + make_group(autostart, UnlessCondition(staged_startup)) + ) + staged_init = Node( + package='demo_world', + executable='staged_nav2_init', + arguments=[ + str(init_pose['x']), + str(init_pose['y']), + str(init_pose['yaw']), + ], + namespace=namespace, + output='screen', + remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], + ) + staged_init_nodes.append(staged_init) + staged_groups.append( + make_group('False', IfCondition(staged_startup), staged_init) + ) set_env_vars_resources = AppendEnvironmentVariable( 'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')) @@ -215,12 +245,14 @@ def generate_launch_description(): ld.add_action(declare_rviz_config_file_cmd) ld.add_action(declare_use_robot_state_pub_cmd) ld.add_action(declare_use_navigation_cmd) + ld.add_action(declare_staged_startup_cmd) # initial localization node for robot_name in robots_list: init_pose = robots_list[robot_name] namespace = robot_name + '/inner' ld.add_action(Node( + condition=UnlessCondition(staged_startup), package='demo_world', executable='set_init', arguments=[str(init_pose['x']), str(init_pose['y']), str(init_pose['yaw'])], @@ -259,4 +291,20 @@ def generate_launch_description(): for cmd in bringup_cmd_group: ld.add_action(cmd) + for staged_init, next_group in zip( + staged_init_nodes, + staged_groups[1:], + ): + ld.add_action( + RegisterEventHandler( + condition=IfCondition(staged_startup), + event_handler=OnProcessExit( + target_action=staged_init, + on_exit=[next_group], + ), + ) + ) + if staged_groups: + ld.add_action(staged_groups[0]) + return ld diff --git a/path_server/README.md b/path_server/README.md index b1542fe..c1db57f 100644 --- a/path_server/README.md +++ b/path_server/README.md @@ -1,7 +1,8 @@ # Path Server -This folder contains a path server implementation. It works by triggering a replan any time a new destination event comes in. The replan should only include robots that are actively moving. -Currently it uses a grid-world PIBT based planner, but it is designed to work with any MapfPlanner implementation. +This folder contains the path server and plan executor. The path server plans for new destinations and replans active robots that report `PlanError.CODE_PATH_BLOCKED`. + +The plan executor checks each remaining route against updates to `/map`, with a 300 ms debounce and two-second cooldown. The grid-world PiBT planner conservatively downsamples maps finer than `1.0 m`; other `MapfPlanner` implementations can be substituted. ## Path Server Demo Dashboard diff --git a/path_server/rmf_path_server/src/lib.rs b/path_server/rmf_path_server/src/lib.rs index abaab44..eaa73e8 100644 --- a/path_server/rmf_path_server/src/lib.rs +++ b/path_server/rmf_path_server/src/lib.rs @@ -18,7 +18,7 @@ use ros_env::{ nav_msgs::msg::{OccupancyGrid, Odometry}, rmf_prototype_msgs::{ self, - msg::{Destination, Plan, PlanId, TrafficDependency, Waypoint}, + msg::{Destination, Plan, PlanError, PlanId, TrafficDependency, Waypoint}, }, }; use std::{ @@ -131,6 +131,19 @@ impl PlanServer

{ .insert(robot_id.to_string(), msg.clone()); } + pub fn handle_plan_error(&mut self, robot_id: &str, msg: PlanError) { + if msg.error.code == PlanError::CODE_PATH_BLOCKED { + rclrs::log_warn!( + self.node.logger(), + "Received CODE_PATH_BLOCKED for robot {}. Enqueuing replan...", + robot_id + ); + if let Some(dest) = self.active_destinations.get(robot_id).cloned() { + self.replan_queue.push((robot_id.to_string(), dest)); + } + } + } + pub fn replan(&mut self) { // 1. Check if any planning results have arrived from the background thread if let Ok(receiver) = self.plan_receiver.get_mut() { @@ -472,6 +485,7 @@ impl PlanServer

{ pub struct RobotPathConnections { pub _destination_subscription: rclrs::WorkerSubscription>, pub _odom_subscription: rclrs::WorkerSubscription>, + pub _plan_error_subscription: rclrs::WorkerSubscription>, } pub struct DiscoveryServer { @@ -607,11 +621,34 @@ pub fn start_path_server( } }; + let robot_id_clone3 = robot_id.to_string(); + let plan_error_topic = robot_id.to_string() + "/plan/error"; + let plan_error_sub = match server + .destinations_worker + .create_subscription::( + plan_error_topic.as_str(), + move |dest_server: &mut PlanServer

, error_msg: PlanError| { + dest_server.handle_plan_error(&robot_id_clone3, error_msg); + }, + ) { + Ok(sub) => sub, + Err(err) => { + rclrs::log_error!( + server.node.logger(), + "Failed to create plan error subscription on DestinationsWorker for {}: {:?}", + robot_id, + err + ); + return; + } + }; + server.active_robots.insert( robot_id.to_string(), RobotPathConnections { _destination_subscription: destination_sub, _odom_subscription: odom_sub, + _plan_error_subscription: plan_error_sub, }, ); } diff --git a/path_server/rmf_path_server/src/planner.rs b/path_server/rmf_path_server/src/planner.rs index 8b2130a..22323cc 100644 --- a/path_server/rmf_path_server/src/planner.rs +++ b/path_server/rmf_path_server/src/planner.rs @@ -21,6 +21,8 @@ use std::collections::HashMap; use std::sync::atomic::AtomicBool; use std::sync::Arc; +const MIN_PLANNING_RESOLUTION: f32 = 1.0; + #[derive(Clone, Debug, Default)] pub struct Map { pub grid: OccupancyGrid, @@ -79,6 +81,44 @@ impl PibtPlanner { } } +fn planning_grid(grid: &OccupancyGrid) -> (usize, usize, f32, f32, f32, Vec>) { + let source_width = grid.info.width as usize; + let source_height = grid.info.height as usize; + let source_resolution = grid.info.resolution; + let resolution = source_resolution.max(MIN_PLANNING_RESOLUTION); + let width = ((source_width as f32 * source_resolution) / resolution) + .ceil() + .max(1.0) as usize; + let height = ((source_height as f32 * source_resolution) / resolution) + .ceil() + .max(1.0) as usize; + let mut cells = vec![vec![0; height]; width]; + + for source_x in 0..source_width { + for source_y in 0..source_height { + let value = grid + .data + .get(source_y * source_width + source_x) + .copied() + .unwrap_or(-1); + if value > 50 || value == -1 { + let x = ((source_x as f32 * source_resolution) / resolution).floor() as usize; + let y = ((source_y as f32 * source_resolution) / resolution).floor() as usize; + cells[x.min(width - 1)][y.min(height - 1)] = 1; + } + } + } + + ( + width, + height, + resolution, + grid.info.origin.position.x as f32, + grid.info.origin.position.y as f32, + cells, + ) +} + impl MapfPlanner for PibtPlanner { fn plan( &self, @@ -97,20 +137,7 @@ impl MapfPlanner for PibtPlanner { map.grid.info.width > 0 && map.grid.info.height > 0 && map.grid.info.resolution > 0.0; let (width, height, resolution, offset_x, offset_y, grid) = if use_map { - let w = map.grid.info.width as usize; - let h = map.grid.info.height as usize; - let r = map.grid.info.resolution; - let ox = map.grid.info.origin.position.x as f32; - let oy = map.grid.info.origin.position.y as f32; - - let mut g = vec![vec![0; h]; w]; - for x in 0..w { - for y in 0..h { - let ros_val = map.grid.data[y * w + x]; - g[x][y] = if ros_val > 50 || ros_val == -1 { 1 } else { 0 }; - } - } - (w, h, r, ox, oy, g) + planning_grid(&map.grid) } else { let mut min_x = f32::MAX; let mut min_y = f32::MAX; @@ -249,3 +276,45 @@ impl MapfPlanner for PibtPlanner { Ok(trajectories) } } + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn fine_occupancy_cells_are_conservatively_downsampled() { + let mut map = OccupancyGrid::default(); + map.info.resolution = 0.25; + map.info.width = 8; + map.info.height = 4; + map.info.origin.position.x = -1.0; + map.info.origin.position.y = -2.0; + map.data = vec![0; 32]; + map.data[2 * 8 + 5] = 100; + + let (width, height, resolution, offset_x, offset_y, cells) = planning_grid(&map); + + assert_eq!(width, 2); + assert_eq!(height, 1); + assert_eq!(resolution, 1.0); + assert_eq!(offset_x, -1.0); + assert_eq!(offset_y, -2.0); + assert_eq!(cells, vec![vec![0], vec![1]]); + } + + #[test] + fn native_grid_is_kept_when_it_is_already_coarse() { + let mut map = OccupancyGrid::default(); + map.info.resolution = 2.0; + map.info.width = 2; + map.info.height = 2; + map.data = vec![0, 100, 0, 0]; + + let (width, height, resolution, _, _, cells) = planning_grid(&map); + + assert_eq!(width, 2); + assert_eq!(height, 2); + assert_eq!(resolution, 2.0); + assert_eq!(cells, vec![vec![0, 0], vec![1, 0]]); + } +} diff --git a/path_server/rmf_path_server_test/test/test_path_server_error.py b/path_server/rmf_path_server_test/test/test_path_server_error.py new file mode 100644 index 0000000..e9e23f3 --- /dev/null +++ b/path_server/rmf_path_server_test/test/test_path_server_error.py @@ -0,0 +1,231 @@ +# 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 +import pytest +import rclpy +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from rmf_prototype_msgs.msg import ( + Destination, + DestinationConstraints, + Participant, + ParticipantList, + Plan, + PlanError, + Region, + TargetRegion, +) + + +@pytest.mark.launch_test +def generate_test_description(): + path_server = launch_ros.actions.Node( + package='rmf_path_server', + executable='rmf_path_server', + output='screen' + ) + + robot_1_sim = launch_ros.actions.Node( + package='rmf_mock_robot_sim', + executable='rmf_mock_robot_sim', + output='screen', + parameters=[{ + 'robot_name': 'robot_1', + 'speed': 2.0, + 'update_rate': 20.0, + 'initial_x': 0.0, + 'initial_y': 0.0, + 'initial_yaw': 0.0, + 'publish_discovery': False, + }] + ) + + return LaunchDescription([ + path_server, + robot_1_sim, + launch_testing.actions.ReadyToTest(), + ]), { + 'path_server': path_server, + 'robot_1_sim': robot_1_sim, + } + + +class TestPathServerError(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node('test_path_server_error_node') + + def tearDown(self): + self.node.destroy_node() + + def create_destination(self, session_id, x, y, size): + msg = Destination() + msg.session.uuid = [session_id] * 16 + constraint = DestinationConstraints() + target_region = TargetRegion() + target_region.region.hint = Region.HINT_AXIS_ALIGNED_RECTANGLE + target_region.region.points = [ + float(x), + float(y), + float(x + size), + float(y + size), + ] + constraint.regions.append(target_region) + msg.constraints = constraint + return msg + + def test_plan_error_behavior(self): + received_plans = [] + + reliable_transient_qos = QoSProfile( + depth=10, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.RELIABLE, + ) + + def plan_cb(msg): + received_plans.append(msg) + + self.node.create_subscription( + Plan, + 'robot_1/plan', + plan_cb, + qos_profile=reliable_transient_qos + ) + + dest_pub = self.node.create_publisher( + Destination, + 'robot_1/destination', + qos_profile=reliable_transient_qos + ) + + error_pub = self.node.create_publisher( + PlanError, + 'robot_1/plan/error', + qos_profile=reliable_transient_qos + ) + + discovery_qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST + ) + + discovery_pub = self.node.create_publisher( + ParticipantList, + '/destination/discovery', + qos_profile=discovery_qos + ) + + discovery_msg = ParticipantList() + p1 = Participant() + p1.name = 'robot_1' + p1.components = [] + discovery_msg.participants.append(p1) + + time.sleep(2.0) + + dest = self.create_destination(1, 5.0, 0.0, 1.0) + + # 1. Publish discovery and destination to receive initial plan + for _ in range(5): + discovery_pub.publish(discovery_msg) + dest_pub.publish(dest) + rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(0.1) + + start_time = time.time() + timeout = 10.0 + while time.time() - start_time < timeout: + discovery_pub.publish(discovery_msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + if received_plans: + break + time.sleep(0.1) + + self.assertGreater( + len(received_plans), 0, 'Did not receive initial plan for robot_1' + ) + initial_plan = received_plans[-1] + initial_version = initial_plan.plan_id.plan_version + + # 2. Test publishing an unhandled error code (e.g. CODE_UNRECOGNIZED_ACTION) + # Verify that it does NOT trigger a replan + unhandled_error_msg = PlanError() + unhandled_error_msg.error.code = PlanError.CODE_UNRECOGNIZED_ACTION + unhandled_error_msg.error.message = 'Unrecognized action test' + unhandled_error_msg.plan_id = initial_plan.plan_id + + error_pub.publish(unhandled_error_msg) + spin_start = time.time() + while time.time() - spin_start < 1.0: + discovery_pub.publish(discovery_msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + time.sleep(0.1) + + self.assertEqual( + received_plans[-1].plan_id.plan_version, + initial_version, + 'Unhandled error code should not trigger a replan' + ) + + # 3. Publish CODE_PATH_BLOCKED PlanError to trigger replanning + plan_error_msg = PlanError() + plan_error_msg.error.code = PlanError.CODE_PATH_BLOCKED + plan_error_msg.error.message = 'Path is blocked' + plan_error_msg.plan_id = initial_plan.plan_id + + num_plans_before_error = len(received_plans) + error_pub.publish(plan_error_msg) + + start_time = time.time() + new_plan_received = False + while time.time() - start_time < timeout: + discovery_pub.publish(discovery_msg) + rclpy.spin_once(self.node, timeout_sec=0.1) + if len(received_plans) > num_plans_before_error: + latest_plan = received_plans[-1] + if latest_plan.plan_id.plan_version > initial_version: + new_plan_received = True + break + time.sleep(0.1) + + self.assertTrue( + new_plan_received, + 'Expected new plan with incremented version after publishing CODE_PATH_BLOCKED' + ) + latest_plan = received_plans[-1] + self.assertEqual( + latest_plan.plan_id.plan_version, + initial_version + 1, + f'Plan version should be incremented from {initial_version} to {initial_version + 1}' + ) + self.assertEqual( + list(latest_plan.plan_id.destination_session.uuid), + list(initial_plan.plan_id.destination_session.uuid), + 'Session UUID should remain the same across replans for the same destination session' + ) diff --git a/path_server/rmf_plan_executor/src/lib.rs b/path_server/rmf_plan_executor/src/lib.rs index 601cd5f..d53df13 100644 --- a/path_server/rmf_plan_executor/src/lib.rs +++ b/path_server/rmf_plan_executor/src/lib.rs @@ -22,17 +22,23 @@ use ros_env::builtin_interfaces; use ros_env::geometry_msgs::msg::Pose; use ros_env::nav2_msgs; use ros_env::nav2_msgs::msg::Costmap; -use ros_env::nav_msgs::msg::Odometry; +use ros_env::nav_msgs::msg::{OccupancyGrid, Odometry}; use ros_env::rmf_prototype_msgs; use ros_env::rmf_prototype_msgs::msg::{ - DestinationConstraints, Plan, PlanRelease, SafeZone, SafeZoneId, TargetOrientation, + DestinationConstraints, Plan, PlanError, PlanId, PlanRelease, SafeZone, SafeZoneId, + TargetOrientation, }; use ros_env::std_msgs; use std::{ collections::{BTreeMap, HashMap}, sync::Arc, + time::{Duration, Instant}, }; +const BLOCKAGE_DEBOUNCE: Duration = Duration::from_millis(300); +const REPLAN_COOLDOWN: Duration = Duration::from_secs(2); +const OCCUPIED_THRESHOLD: i8 = 50; + pub struct RobotState { pub radius: f32, pub latest_odom: Option, @@ -40,6 +46,48 @@ pub struct RobotState { pub waypoint_follower: Option, pub safe_zone_version: u64, pub last_incremental_target_wp: Option, + blockage_monitor: BlockageMonitor, +} + +#[derive(Default)] +struct BlockageMonitor { + blocked_since: Option, + reported_plan: Option, + last_reported_at: Option, +} + +impl BlockageMonitor { + fn begin_plan(&mut self) { + self.blocked_since = None; + } + + fn observe(&mut self, blocked: bool, plan_id: &PlanId, now: Instant) -> bool { + if !blocked { + self.blocked_since = None; + return false; + } + + if self.reported_plan.as_ref() == Some(plan_id) { + return false; + } + + let blocked_since = self.blocked_since.get_or_insert(now); + if now.duration_since(*blocked_since) < BLOCKAGE_DEBOUNCE { + return false; + } + + if self + .last_reported_at + .is_some_and(|last| now.duration_since(last) < REPLAN_COOLDOWN) + { + return false; + } + + self.reported_plan = Some(plan_id.clone()); + self.last_reported_at = Some(now); + self.blocked_since = None; + true + } } pub struct PlanExecutor { @@ -50,11 +98,38 @@ pub struct PlanExecutor { pub active_robots: BTreeMap, pub plan_release_publishers: HashMap>, pub safezone_publishers: HashMap>, + pub plan_error_publishers: HashMap>, pub grid: Arc, pub grid_width: u32, pub grid_height: u32, pub grid_resolution: f32, pub grid_origin: Pose, + pub latest_map: Option, +} + +fn target_yaw(plan: &Plan, target_idx: usize) -> f32 { + let Some(target) = plan.waypoints.get(target_idx) else { + return 0.0; + }; + let [target_x, target_y] = target.position; + + for waypoint in plan.waypoints[..target_idx].iter().rev() { + let dx = target_x - waypoint.position[0]; + let dy = target_y - waypoint.position[1]; + if dx.hypot(dy) > 1e-3 { + return dy.atan2(dx); + } + } + + for waypoint in plan.waypoints.iter().skip(target_idx + 1) { + let dx = waypoint.position[0] - target_x; + let dy = waypoint.position[1] - target_y; + if dx.hypot(dy) > 1e-3 { + return dy.atan2(dx); + } + } + + 0.0 } impl PlanExecutor { @@ -66,11 +141,13 @@ impl PlanExecutor { active_robots: BTreeMap::new(), plan_release_publishers: HashMap::new(), safezone_publishers: HashMap::new(), + plan_error_publishers: HashMap::new(), grid: Arc::new(Grid2D::new(vec![vec![0; 20]; 20], 1.0)), grid_width: 20, grid_height: 20, grid_resolution: 1.0, grid_origin: origin, + latest_map: None, } } @@ -91,6 +168,7 @@ impl PlanExecutor { waypoint_follower: None, safe_zone_version: 0, last_incremental_target_wp: None, + blockage_monitor: BlockageMonitor::default(), }, ); self.reindex_followers(); @@ -106,6 +184,7 @@ impl PlanExecutor { ); self.plan_release_publishers.remove(robot_id); self.safezone_publishers.remove(robot_id); + self.plan_error_publishers.remove(robot_id); self.reindex_followers(); } } @@ -164,37 +243,50 @@ impl PlanExecutor { state.plan = Some(msg); state.safe_zone_version = 0; state.last_incremental_target_wp = None; + state.blockage_monitor.begin_plan(); // Reindex because we updated the plan self.reindex_followers(); } + pub fn handle_map(&mut self, msg: OccupancyGrid) { + self.latest_map = Some(msg); + let robot_ids: Vec<_> = self.active_robots.keys().cloned().collect(); + for robot_id in robot_ids { + self.update_route_blockage(&robot_id); + } + } + pub fn handle_odometry(&mut self, robot_id: &str, msg: Odometry) { let current_x = msg.pose.pose.position.x as f32; let current_y = msg.pose.pose.position.y as f32; - let Some(state) = self.active_robots.get_mut(robot_id) else { - return; - }; - - state.latest_odom = Some(msg.clone()); - - if let Some(fw) = &mut state.waypoint_follower { - let before = fw.get_semantic_waypoint().trajectory_index; - let position = Isometry2::new(Vector2::new(current_x, current_y), 0.0); - fw.update_position_estimate(&position, 0.5); - let after = fw.get_semantic_waypoint().trajectory_index; - rclrs::log_debug!( - self.node.logger(), - "[Executor Debug] Robot {} pos=({}, {}) index before={}, after={}", - robot_id, - current_x, - current_y, - before, - after - ); + { + let Some(state) = self.active_robots.get_mut(robot_id) else { + return; + }; + + state.latest_odom = Some(msg.clone()); + + if let Some(fw) = &mut state.waypoint_follower { + let before = fw.get_semantic_waypoint().trajectory_index; + let position = Isometry2::new(Vector2::new(current_x, current_y), 0.0); + fw.update_position_estimate(&position, 0.5); + let after = fw.get_semantic_waypoint().trajectory_index; + rclrs::log_debug!( + self.node.logger(), + "[Executor Debug] Robot {} pos=({}, {}) index before={}, after={}", + robot_id, + current_x, + current_y, + before, + after + ); + } } + self.update_route_blockage(robot_id); + if !self.ready_to_execute() { return; } @@ -412,6 +504,7 @@ impl PlanExecutor { let target_x = plan.waypoints[released_wp_idx].position[0]; let target_y = plan.waypoints[released_wp_idx].position[1]; + let target_yaw = target_yaw(plan, released_wp_idx); let costmap = Self::to_costmap_msg( &positions, @@ -433,16 +526,17 @@ impl PlanExecutor { let safe_zone = SafeZone { incremental_target: DestinationConstraints { regions: vec![rmf_prototype_msgs::msg::TargetRegion { - tolerance: 0.2, + tolerance: 0.1, region: rmf_prototype_msgs::msg::Region { points: vec![target_x, target_y], hint: rmf_prototype_msgs::msg::Region::HINT_POINT, }, orientations: vec![TargetOrientation { - orientation_radians: 0.0, // TODO(@xiyuoh) + // Avoid turning in place at hold points. + orientation_radians: target_yaw, spread_radians: 0.0, tolerance_radians: 0.0, - }], // TODO(@xiyuoh) calculate actual orientation + }], }], nodes: vec![], }, @@ -474,6 +568,82 @@ impl PlanExecutor { } } + fn update_route_blockage(&mut self, robot_id: &str) { + let Some(map) = self.latest_map.as_ref() else { + return; + }; + + let Some(state) = self.active_robots.get_mut(robot_id) else { + return; + }; + let (Some(plan), Some(odom), Some(follower)) = ( + state.plan.as_ref(), + state.latest_odom.as_ref(), + state.waypoint_follower.as_mut(), + ) else { + return; + }; + + let remaining = follower.remaining_trajectory(); + let mut route = Vec::with_capacity(remaining.len() + 1); + route.push(( + odom.pose.pose.position.x as f32, + odom.pose.pose.position.y as f32, + )); + route.extend(remaining); + + let blocked = route_intersects_map(map, &route, state.radius); + let plan_id = plan.plan_id.clone(); + if !state + .blockage_monitor + .observe(blocked, &plan_id, Instant::now()) + { + return; + } + + let publisher = match self.plan_error_publishers.entry(robot_id.to_string()) { + std::collections::hash_map::Entry::Occupied(entry) => entry.into_mut(), + std::collections::hash_map::Entry::Vacant(entry) => { + let topic = format!("{robot_id}/plan/error"); + match self.node.create_publisher(topic.as_str()) { + Ok(publisher) => entry.insert(publisher), + Err(error) => { + rclrs::log_error!( + self.node.logger(), + "Failed to create plan error publisher for {}: {:?}", + robot_id, + error + ); + return; + } + } + } + }; + + let error = PlanError { + error: rmf_prototype_msgs::msg::Error { + code: PlanError::CODE_PATH_BLOCKED, + message: format!("Updated map blocks the remaining route for {robot_id}"), + parameters: String::new(), + }, + plan_id, + }; + if let Err(error) = publisher.publish(error) { + rclrs::log_error!( + self.node.logger(), + "Failed to publish path blockage for {}: {:?}", + robot_id, + error + ); + } else { + rclrs::log_warn!( + self.node.logger(), + "Updated map blocks the remaining route for {}. Requesting a replan.", + robot_id + ); + } + } + fn ready_to_execute(&self) -> bool { if self.active_robots.is_empty() { return false; @@ -527,10 +697,185 @@ impl PlanExecutor { } } +fn route_intersects_map(map: &OccupancyGrid, route: &[(f32, f32)], radius: f32) -> bool { + if route.len() < 2 || map.info.resolution <= 0.0 { + return false; + } + + let width = map.info.width as isize; + let height = map.info.height as isize; + if width == 0 || height == 0 { + return false; + } + + let resolution = map.info.resolution; + let q = &map.info.origin.orientation; + let yaw = (2.0 * (q.w * q.z + q.x * q.y)).atan2(1.0 - 2.0 * (q.y * q.y + q.z * q.z)) as f32; + let cos_yaw = yaw.cos(); + let sin_yaw = yaw.sin(); + let origin_x = map.info.origin.position.x as f32; + let origin_y = map.info.origin.position.y as f32; + let clearance = radius.max(0.0) + resolution * std::f32::consts::FRAC_1_SQRT_2; + let clearance_squared = clearance * clearance; + + let to_map = |(x, y): (f32, f32)| { + let dx = x - origin_x; + let dy = y - origin_y; + (cos_yaw * dx + sin_yaw * dy, -sin_yaw * dx + cos_yaw * dy) + }; + + for segment in route.windows(2) { + let start = to_map(segment[0]); + let end = to_map(segment[1]); + let min_x = (((start.0.min(end.0) - clearance) / resolution).floor() as isize).max(0); + let max_x = + (((start.0.max(end.0) + clearance) / resolution).floor() as isize).min(width - 1); + let min_y = (((start.1.min(end.1) - clearance) / resolution).floor() as isize).max(0); + let max_y = + (((start.1.max(end.1) + clearance) / resolution).floor() as isize).min(height - 1); + + for y in min_y..=max_y { + for x in min_x..=max_x { + let index = y as usize * width as usize + x as usize; + if map.data.get(index).copied().unwrap_or(-1) <= OCCUPIED_THRESHOLD { + continue; + } + + let center = ((x as f32 + 0.5) * resolution, (y as f32 + 0.5) * resolution); + if distance_squared_to_segment(center, start, end) <= clearance_squared { + return true; + } + } + } + } + + false +} + +fn distance_squared_to_segment(point: (f32, f32), start: (f32, f32), end: (f32, f32)) -> f32 { + let segment = (end.0 - start.0, end.1 - start.1); + let length_squared = segment.0 * segment.0 + segment.1 * segment.1; + if length_squared <= f32::EPSILON { + return (point.0 - start.0).powi(2) + (point.1 - start.1).powi(2); + } + + let offset = (point.0 - start.0, point.1 - start.1); + let t = ((offset.0 * segment.0 + offset.1 * segment.1) / length_squared).clamp(0.0, 1.0); + let closest = (start.0 + t * segment.0, start.1 + t * segment.1); + (point.0 - closest.0).powi(2) + (point.1 - closest.1).powi(2) +} + #[cfg(test)] mod tests { + use super::{ + route_intersects_map, target_yaw, BlockageMonitor, BLOCKAGE_DEBOUNCE, REPLAN_COOLDOWN, + }; use mapf_post::na::{Isometry2, Vector2}; use mapf_post::{Trajectory, WaypointFollower}; + use ros_env::{ + nav_msgs::msg::OccupancyGrid, + rmf_prototype_msgs::msg::{Plan, PlanId, Waypoint}, + }; + use std::time::Instant; + + fn plan_with_positions(positions: &[[f32; 2]]) -> Plan { + Plan { + waypoints: positions + .iter() + .map(|position| Waypoint { + position: *position, + ..Default::default() + }) + .collect(), + ..Default::default() + } + } + + #[test] + fn target_yaw_ignores_stationary_wait_waypoints() { + let plan = plan_with_positions(&[[3.0, 4.0], [3.0, 4.0], [3.0, 4.0], [3.0, 5.0]]); + + assert!((target_yaw(&plan, 3) - std::f32::consts::FRAC_PI_2).abs() < 1e-6); + } + + #[test] + fn target_yaw_uses_departure_direction_at_trajectory_start() { + let plan = plan_with_positions(&[[3.0, 4.0], [3.0, 4.0], [2.0, 4.0]]); + + assert!((target_yaw(&plan, 0) - std::f32::consts::PI).abs() < 1e-6); + } + + #[test] + fn stationary_trajectory_has_neutral_yaw() { + let plan = plan_with_positions(&[[3.0, 4.0], [3.0, 4.0]]); + + assert_eq!(target_yaw(&plan, 1), 0.0); + } + + #[test] + fn occupied_cell_on_remaining_route_is_blocked() { + let mut map = OccupancyGrid::default(); + map.info.resolution = 1.0; + map.info.width = 10; + map.info.height = 10; + map.info.origin.orientation.w = 1.0; + map.data = vec![0; 100]; + map.data[5 * 10 + 5] = 100; + + assert!(route_intersects_map(&map, &[(1.5, 5.5), (8.5, 5.5)], 0.25)); + assert!(!route_intersects_map(&map, &[(1.5, 8.5), (8.5, 8.5)], 0.25)); + } + + #[test] + fn blockage_must_persist_before_reporting() { + let mut monitor = BlockageMonitor::default(); + let plan_id = PlanId::default(); + let start = Instant::now(); + + assert!(!monitor.observe(true, &plan_id, start)); + assert!(!monitor.observe(true, &plan_id, start + BLOCKAGE_DEBOUNCE / 2)); + assert!(monitor.observe(true, &plan_id, start + BLOCKAGE_DEBOUNCE)); + let later = start + BLOCKAGE_DEBOUNCE + REPLAN_COOLDOWN; + assert!(!monitor.observe(true, &plan_id, later)); + } + + #[test] + fn clear_route_resets_the_debounce_window() { + let mut monitor = BlockageMonitor::default(); + let plan_id = PlanId::default(); + let start = Instant::now(); + + assert!(!monitor.observe(true, &plan_id, start)); + assert!(!monitor.observe(false, &plan_id, start + BLOCKAGE_DEBOUNCE)); + assert!(!monitor.observe(true, &plan_id, start + BLOCKAGE_DEBOUNCE)); + let before_debounce = start + BLOCKAGE_DEBOUNCE * 3 / 2; + assert!(!monitor.observe(true, &plan_id, before_debounce)); + assert!(monitor.observe(true, &plan_id, start + BLOCKAGE_DEBOUNCE * 2)); + } + + #[test] + fn cooldown_delays_a_blockage_on_the_next_plan() { + let mut monitor = BlockageMonitor::default(); + let first_plan = PlanId::default(); + let second_plan = PlanId { + plan_version: 1, + ..Default::default() + }; + let start = Instant::now(); + + assert!(!monitor.observe(true, &first_plan, start)); + assert!(monitor.observe(true, &first_plan, start + BLOCKAGE_DEBOUNCE)); + monitor.begin_plan(); + let during_cooldown = start + BLOCKAGE_DEBOUNCE * 2; + assert!(!monitor.observe(true, &second_plan, during_cooldown)); + let after_debounce = start + BLOCKAGE_DEBOUNCE * 3; + assert!(!monitor.observe(true, &second_plan, after_debounce)); + assert!(monitor.observe( + true, + &second_plan, + start + BLOCKAGE_DEBOUNCE + REPLAN_COOLDOWN, + )); + } #[test] fn test_robot_2_follower() { diff --git a/path_server/rmf_plan_executor/src/main.rs b/path_server/rmf_plan_executor/src/main.rs index 1bb2c1d..8533b48 100644 --- a/path_server/rmf_plan_executor/src/main.rs +++ b/path_server/rmf_plan_executor/src/main.rs @@ -14,7 +14,7 @@ use rclrs::{Context, CreateBasicExecutor, IntoPrimitiveOptions, SpinOptions}; use rmf_plan_executor::PlanExecutor; -use ros_env::nav_msgs::msg::Odometry; +use ros_env::nav_msgs::msg::{OccupancyGrid, Odometry}; use ros_env::rmf_prototype_msgs::msg::{ParticipantList, Plan}; use std::collections::HashMap; @@ -73,6 +73,13 @@ fn main() -> Result<(), Box> { }, )?; + let _map_subscription = executor_worker.create_subscription::( + "/map".transient_local().reliable(), + move |executor: &mut PlanExecutor, msg: OccupancyGrid| { + executor.handle_map(msg); + }, + )?; + // 2. Subscribe to discovery on the discovery worker to manage odom/plan subscriptions let _discovery_subscription = rmf_participant_discovery::create_discovery_subscription( &discovery_worker, From 3883286278fc1fd5d2a026c5c4f7887e6854da56 Mon Sep 17 00:00:00 2001 From: Samuel Foo Enze Date: Mon, 27 Jul 2026 23:41:18 +0200 Subject: [PATCH 29/38] Rasterize layered map updates for faster composition (#4) * refactor(map): rasterize dynamic updates on ingest Store per-source cell contributions instead of retained region geometry. Preserve expiry precedence and discard cells when static map geometry changes. Signed-off-by: SamuelFoo * feat(map): queue updates before static map Buffer up to 1024 validated region updates in FIFO order and replay them when the static grid arrives. Drop the oldest update when the queue is full. Signed-off-by: SamuelFoo * perf(map): add layered update benchmark Replay deterministic reset, overlapping, and moving update traces. Report phase latency, throughput, peak RSS, and a composed-grid checksum for A/B comparisons. Signed-off-by: SamuelFoo * docs(map): describe rasterization and benchmark Explain cell-based refresh and pre-map queue behavior. Add commands for the three benchmark scenarios and checksum-based A/B comparison. Signed-off-by: SamuelFoo --------- Signed-off-by: Samuel Foo Enze --- discourse/3-layered-global-occupancy-map.md | 15 +- map_server/rmf_layered_map_server/README.md | 21 +- .../src/bin/layered_map_benchmark.rs | 365 ++++++++++++++++++ map_server/rmf_layered_map_server/src/lib.rs | 360 ++++++++++++++--- 4 files changed, 686 insertions(+), 75 deletions(-) create mode 100644 map_server/rmf_layered_map_server/src/bin/layered_map_benchmark.rs diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md index 256685e..529f6e4 100644 --- a/discourse/3-layered-global-occupancy-map.md +++ b/discourse/3-layered-global-occupancy-map.md @@ -21,8 +21,7 @@ observations, can be added later. * Clear-space patches have TTLs, just like obstacle patches * A source can reset its previous observations without using a TTL * Robot-mounted sources include the observation-frame pose at observation time -* The Rust map server composes a static occupancy grid and active observations - into `/map` +* The Rust map server rasterizes incoming observations once and composes their active cell contributions with a static occupancy grid into `/map` * The Nav2 demo converts scans from three robots into clear ray sectors and occupied endpoint regions, then displays the source contributions and map * The replanning demo fuses two robots' scans and replans when an updated map blocks an active route * The observation messages live in `rmf_layered_map_msgs`, leaving @@ -114,21 +113,15 @@ accepts point and axis-aligned rectangle regions. # Layered Map Server -`rmf_layered_map_server` keeps the static occupancy grid separate from dynamic -observations and publishes their composition on `/map`. It validates source -timestamps and frames, ignores updates that are older than the latest accepted -update from the same source, supports source resets, and removes observations -after their TTL expires. +`rmf_layered_map_server` keeps the static occupancy grid separate from dynamic observations and publishes their composition on `/map`. It validates source timestamps and frames, ignores updates older than the latest accepted update from the same source, supports source resets, and removes observations after their TTL expires. Each region is transformed and rasterized on arrival. Repeated observations refresh cell contributions instead of stacking region snapshots. Older evidence is retained only when it outlives a newer contribution. -The server applies clear patches before obstacle patches from the same update. -During composition, obstacle observations win over clear observations so -occupied space is not accidentally erased by another active source. +The server applies clear patches before obstacle patches from the same update. During composition, obstacle observations win over clear observations so occupied space is not accidentally erased by another active source. Updates received before the static grid are held in a bounded FIFO queue and rasterized when the grid arrives. A static grid geometry change discards active cell contributions because their indices refer to the old geometry. # Three-Robot Nav2 Demo The launch file starts three robots in different free corners of the warehouse, cycles them through fixed Nav2 goals, and spawns one deterministic Gazebo box near each robot. Each observation node subscribes to its robot's local `sensor_msgs/LaserScan`, filters invalid or out-of-range returns, and publishes free-space ray sectors as convex polygons and occupied endpoints as point regions. -Each scan adds temporary clear and obstacle patches to a rolling observation history. The map server rasterizes clear sectors before obstacle endpoints so a measured hit remains occupied. Active obstacle evidence still wins over clear evidence until its TTL expires. An update may set `reset_source` so the new scan replaces all active observations from that source instead of being added to its history. The observation-frame pose is recorded in the shared `map` frame so the map server can transform the scan-local regions before rasterizing them. +Each scan adds temporary clear and obstacle patches. The map server rasterizes them on arrival and refreshes matching cell contributions rather than retaining region messages. It applies clear cells before obstacle cells so a measured hit remains occupied. Active obstacle evidence still wins over clear evidence until its TTL expires. An update may set `reset_source` so the new scan replaces all active observations from that source. The observation-frame pose is recorded in the shared `map` frame so the map server can transform scan-local regions before rasterizing them. The demo launches Nav2 localization, planning, and control for the fixed goal loops, but does not launch RMF planning. Robot-local RViz windows show Nav2 state, while another RViz window displays the combined global `/map`. The combined view overlays incoming region updates as colored markers grouped by source, with the same retention behavior as the map contributions. diff --git a/map_server/rmf_layered_map_server/README.md b/map_server/rmf_layered_map_server/README.md index 3b9c7bc..e6e5c34 100644 --- a/map_server/rmf_layered_map_server/README.md +++ b/map_server/rmf_layered_map_server/README.md @@ -10,7 +10,7 @@ Default topics: - `/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. +Dynamic observations expire according to their TTL, so transient obstacles do not remain in the global planning map forever. The server transforms and rasterizes each region on arrival, then stores cell contributions by source, map, update type, and cell. Repeated observations refresh matching cells instead of stacking region snapshots. Older evidence is retained only when it outlives a newer contribution. `MapRegionUpdate` messages contain a list of `MapRegionPatch` entries. A sensor snapshot that clears freespace and marks obstacles can publish clear and obstacle @@ -26,6 +26,25 @@ 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. +Region updates received before the static occupancy grid are held in a bounded FIFO queue and rasterized once the grid arrives. Replacing only the static grid data preserves active dynamic cells; changing its frame, dimensions, resolution, or origin discards them because their cell indices no longer describe the new grid. + +## Performance Benchmark + +The deterministic benchmark is installed by the normal colcon build and exercises the map server without ROS transport, visualization, or simulator overhead. It reports ingest, prune, composition, and total latency, plus throughput, peak RSS, and an output-grid checksum. + +Build and source the workspace, then run each supported update pattern: + +```bash +ros2 run rmf_layered_map_server layered_map_benchmark \ + --label candidate --scenario reset +ros2 run rmf_layered_map_server layered_map_benchmark \ + --label candidate --scenario rolling-overlap +ros2 run rmf_layered_map_server layered_map_benchmark \ + --label candidate --scenario rolling-moving +``` + +For an A/B comparison, run the same benchmark source and arguments against both revisions. Compare performance only when the checksums match. + ## Visualization Smoke Test Build and source the workspace: diff --git a/map_server/rmf_layered_map_server/src/bin/layered_map_benchmark.rs b/map_server/rmf_layered_map_server/src/bin/layered_map_benchmark.rs new file mode 100644 index 0000000..182bed7 --- /dev/null +++ b/map_server/rmf_layered_map_server/src/bin/layered_map_benchmark.rs @@ -0,0 +1,365 @@ +// 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 rmf_layered_map_server::LayeredMap; +use ros_env::{ + geometry_msgs::msg::{Point, Pose, Quaternion}, + nav_msgs::msg::{MapMetaData, OccupancyGrid}, + rmf_layered_map_msgs::msg::{MapObservationSource, MapRegionPatch, MapRegionUpdate}, + rmf_prototype_msgs::msg::Region, + std_msgs::msg::Header, +}; +use std::{ + collections::hash_map::DefaultHasher, + env, fs, + hash::{Hash, Hasher}, + hint::black_box, + io::{self, Write}, + time::{Duration, Instant}, +}; + +const NANOS_PER_SECOND: i128 = 1_000_000_000; +const UPDATES: usize = 240; +const ROUNDS: usize = 8; +const WARMUP_ROUNDS: usize = 2; +const BEAMS: usize = 90; +const SOURCES: usize = 2; +const EVENT_PERIOD_NSEC: i128 = 250_000_000; +const TTL_SEC: f64 = 10.0; +const MAP_WIDTH: u32 = 240; +const MAP_HEIGHT: u32 = 180; +const MAP_RESOLUTION: f32 = 0.1; +const SCAN_RADIUS: f64 = 4.0; + +#[derive(Clone, Copy)] +enum Scenario { + Reset, + RollingOverlap, + RollingMoving, +} + +impl Scenario { + fn parse(value: &str) -> Result { + match value { + "reset" => Ok(Self::Reset), + "rolling-overlap" => Ok(Self::RollingOverlap), + "rolling-moving" => Ok(Self::RollingMoving), + _ => Err(format!( + "unsupported scenario '{value}'; expected reset, rolling-overlap, or rolling-moving" + )), + } + } + + fn name(self) -> &'static str { + match self { + Self::Reset => "reset", + Self::RollingOverlap => "rolling-overlap", + Self::RollingMoving => "rolling-moving", + } + } +} + +struct Config { + label: String, + scenario: Scenario, +} + +#[derive(Clone)] +struct TraceEvent { + update: MapRegionUpdate, + now_nsec: i128, +} + +#[derive(Default)] +struct Samples { + ingest: Vec, + prune: Vec, + compose: Vec, + total: Vec, +} + +struct ReplayResult { + samples: Samples, + checksum: u64, +} + +fn main() -> Result<(), String> { + let config = parse_args()?; + println!( + "Running layered map benchmark ({})...", + config.scenario.name() + ); + io::stdout() + .flush() + .map_err(|error| format!("failed to flush benchmark output: {error}"))?; + + let trace = make_trace(&config); + + for _ in 0..WARMUP_ROUNDS { + black_box(replay(&trace, false).checksum); + } + + let mut samples = Samples::default(); + let mut expected_checksum = None; + for _ in 0..ROUNDS { + let result = replay(&trace, true); + if let Some(expected) = expected_checksum { + if result.checksum != expected { + return Err(format!( + "non-deterministic output checksum: expected {expected}, got {}", + result.checksum + )); + } + } else { + expected_checksum = Some(result.checksum); + } + samples.ingest.extend(result.samples.ingest); + samples.prune.extend(result.samples.prune); + samples.compose.extend(result.samples.compose); + samples.total.extend(result.samples.total); + } + + let measured_updates = UPDATES * ROUNDS; + let measured_seconds = samples.total.iter().map(Duration::as_secs_f64).sum::(); + let throughput = measured_updates as f64 / measured_seconds; + let peak_rss_kib = peak_rss_kib().unwrap_or(0); + + println!(); + println!("Results"); + println!(" Label {}", config.label); + println!(" Scenario {}", config.scenario.name()); + println!(" Workload {UPDATES} updates x {ROUNDS} rounds ({WARMUP_ROUNDS} warm-up)"); + println!(" Scan {BEAMS} beams x {SOURCES} sources"); + println!(" Throughput {throughput:.1} updates/s"); + println!(" Peak RSS {:.2} MiB", peak_rss_kib as f64 / 1024.0); + println!(" Checksum {}", expected_checksum.unwrap_or(0)); + println!(); + println!("Latency (ms)"); + println!( + " {:<10} {:>10} {:>10} {:>10} {:>10}", + "Phase", "Mean", "P50", "P95", "P99" + ); + print_stats("ingest", samples.ingest); + print_stats("prune", samples.prune); + print_stats("compose", samples.compose); + print_stats("total", samples.total); + + Ok(()) +} + +fn parse_args() -> Result { + let mut config = Config { + label: "unlabeled".to_string(), + scenario: Scenario::Reset, + }; + + let mut args = env::args().skip(1); + while let Some(flag) = args.next() { + let value = args + .next() + .ok_or_else(|| format!("missing value for argument '{flag}'"))?; + match flag.as_str() { + "--label" => config.label = value, + "--scenario" => config.scenario = Scenario::parse(&value)?, + _ => return Err(format!("unsupported argument '{flag}'")), + } + } + + Ok(config) +} + +fn make_trace(config: &Config) -> Vec { + (0..UPDATES) + .map(|event_index| { + let source_index = event_index % SOURCES; + let source_scan_index = event_index / SOURCES; + let now_nsec = NANOS_PER_SECOND + event_index as i128 * EVENT_PERIOD_NSEC; + TraceEvent { + update: make_update(config.scenario, source_index, source_scan_index, now_nsec), + now_nsec, + } + }) + .collect() +} + +fn make_update( + scenario: Scenario, + source_index: usize, + scan_index: usize, + stamp_nsec: i128, +) -> MapRegionUpdate { + let mut source = MapObservationSource { + header: Header { + frame_id: "map".to_string(), + ..Default::default() + }, + source_id: format!("robot_{source_index}/scan"), + robot_name: format!("robot_{source_index}"), + map_name: "benchmark_map".to_string(), + default_ttl_sec: TTL_SEC, + ..Default::default() + }; + source.header.stamp.sec = (stamp_nsec / NANOS_PER_SECOND) as i32; + source.header.stamp.nanosec = (stamp_nsec % NANOS_PER_SECOND) as u32; + source.robot_pose.position.x = match scenario { + Scenario::RollingMoving => 7.0 + (scan_index % 40) as f64 * 0.1, + Scenario::Reset | Scenario::RollingOverlap => 9.0, + }; + let source_fraction = source_index as f64 / SOURCES.saturating_sub(1).max(1) as f64; + source.robot_pose.position.y = 5.0 + 7.0 * source_fraction; + source.robot_pose.orientation.w = 1.0; + + let mut clear_patch = MapRegionPatch { + update_type: MapRegionPatch::UPDATE_CLEAR, + occupancy_value: 0, + ttl_sec: TTL_SEC, + ..Default::default() + }; + let mut obstacle_patch = MapRegionPatch { + update_type: MapRegionPatch::UPDATE_OBSTACLE, + occupancy_value: 100, + ttl_sec: TTL_SEC, + ..Default::default() + }; + + let angle_step = std::f64::consts::TAU / BEAMS as f64; + for beam in 0..BEAMS { + let start_angle = beam as f64 * angle_step; + let end_angle = (beam + 1) as f64 * angle_step; + let varying_radius = SCAN_RADIUS - 0.35 * ((beam + scan_index) % 7) as f64 / 6.0; + let start = ( + varying_radius * start_angle.cos(), + varying_radius * start_angle.sin(), + ); + let end = ( + varying_radius * end_angle.cos(), + varying_radius * end_angle.sin(), + ); + clear_patch.regions.push(Region { + hint: Region::HINT_CONVEX_POLYGON, + points: vec![ + 0.0, + 0.0, + start.0 as f32, + start.1 as f32, + end.0 as f32, + end.1 as f32, + ], + }); + + if beam % 6 == 0 { + obstacle_patch.regions.push(Region { + hint: Region::HINT_POINT, + points: vec![start.0 as f32, start.1 as f32], + }); + } + } + + MapRegionUpdate { + source, + reset_source: matches!(scenario, Scenario::Reset), + patches: vec![clear_patch, obstacle_patch], + } +} + +fn replay(trace: &[TraceEvent], record_samples: bool) -> ReplayResult { + let mut map = LayeredMap::new(Duration::from_secs(TTL_SEC as u64)); + map.set_static_map(static_grid()); + let mut samples = Samples::default(); + let mut hasher = DefaultHasher::new(); + + for event in trace.iter().cloned() { + let total_start = Instant::now(); + + let prune_start = Instant::now(); + black_box(map.prune_expired(event.now_nsec)); + let prune_elapsed = prune_start.elapsed(); + + let ingest_start = Instant::now(); + black_box(map.ingest_region_update(event.update, event.now_nsec)); + let ingest_elapsed = ingest_start.elapsed(); + + let compose_start = Instant::now(); + let grid = map.compose().expect("benchmark always has a static map"); + let compose_elapsed = compose_start.elapsed(); + + let total_elapsed = total_start.elapsed(); + grid.data.hash(&mut hasher); + black_box(&grid); + + if record_samples { + samples.prune.push(prune_elapsed); + samples.ingest.push(ingest_elapsed); + samples.compose.push(compose_elapsed); + samples.total.push(total_elapsed); + } + } + + ReplayResult { + samples, + checksum: hasher.finish(), + } +} + +fn static_grid() -> OccupancyGrid { + OccupancyGrid { + header: Header { + frame_id: "map".to_string(), + ..Default::default() + }, + info: MapMetaData { + resolution: MAP_RESOLUTION, + width: MAP_WIDTH, + height: MAP_HEIGHT, + origin: Pose { + position: Point::default(), + orientation: Quaternion { + w: 1.0, + ..Default::default() + }, + }, + ..Default::default() + }, + data: vec![0; (MAP_WIDTH * MAP_HEIGHT) as usize], + ..Default::default() + } +} + +fn print_stats(name: &str, mut values: Vec) { + values.sort_unstable(); + let mean_ms = + values.iter().map(Duration::as_secs_f64).sum::() * 1000.0 / values.len() as f64; + println!( + " {name:<10} {:>10.3} {:>10.3} {:>10.3} {:>10.3}", + mean_ms, + percentile(&values, 50).as_secs_f64() * 1000.0, + percentile(&values, 95).as_secs_f64() * 1000.0, + percentile(&values, 99).as_secs_f64() * 1000.0, + ); +} + +fn percentile(values: &[Duration], percentile: usize) -> Duration { + let index = ((values.len() - 1) * percentile).div_ceil(100); + values[index] +} + +fn peak_rss_kib() -> Option { + fs::read_to_string("/proc/self/status") + .ok()? + .lines() + .find_map(|line| { + let value = line.strip_prefix("VmHWM:")?; + value.split_whitespace().next()?.parse().ok() + }) +} diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs index 50214d6..2b176d5 100644 --- a/map_server/rmf_layered_map_server/src/lib.rs +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -19,26 +19,32 @@ use ros_env::{ rmf_layered_map_msgs::msg::{MapRegionPatch, MapRegionUpdate}, rmf_prototype_msgs::msg::Region, }; -use std::{collections::HashMap, time::Duration}; +use std::{ + collections::{HashMap, HashSet, VecDeque}, + time::Duration, +}; const NANOS_PER_SECOND: i128 = 1_000_000_000; const MAP_QOS_DEPTH: u32 = 10; +const MAX_PENDING_REGION_UPDATES: usize = 1024; -#[derive(Clone, Copy, Debug, PartialEq, Eq)] +#[derive(Clone, Copy, Debug, Hash, PartialEq, Eq)] enum DynamicUpdateType { Obstacle, Clear, } -#[derive(Clone, Debug)] -struct DynamicObservation { - source_id: String, - map_name: String, - frame_id: String, +#[derive(Clone, Debug, Hash, PartialEq, Eq)] +struct DynamicCellKey { update_type: DynamicUpdateType, + cell_index: usize, +} + +#[derive(Clone, Debug)] +struct DynamicCell { occupancy_value: i8, - regions: Vec, expires_at_nsec: i128, + sequence: u64, } #[derive(Clone, Debug, Hash, PartialEq, Eq)] @@ -50,9 +56,10 @@ struct ObservationSourceKey { #[derive(Clone, Debug)] pub struct LayeredMap { static_grid: Option, - dynamic_observations: Vec, + dynamic_cells: HashMap>>, latest_source_stamps: HashMap, default_ttl_nsec: i128, + next_sequence: u64, revision: u64, } @@ -60,9 +67,10 @@ impl LayeredMap { pub fn new(default_ttl: Duration) -> Self { Self { static_grid: None, - dynamic_observations: Vec::new(), + dynamic_cells: HashMap::new(), latest_source_stamps: HashMap::new(), default_ttl_nsec: duration_to_nsec(default_ttl), + next_sequence: 0, revision: 0, } } @@ -72,11 +80,18 @@ impl LayeredMap { } pub fn dynamic_observation_count(&self) -> usize { - self.dynamic_observations.len() + self.dynamic_cells.values().map(HashMap::len).sum() } pub fn set_static_map(&mut self, mut grid: OccupancyGrid) { normalize_grid_data(&mut grid); + if self + .static_grid + .as_ref() + .is_some_and(|current| !same_grid_geometry(current, &grid)) + { + self.dynamic_cells.clear(); + } self.static_grid = Some(grid); self.revision = self.revision.wrapping_add(1); } @@ -85,6 +100,9 @@ impl LayeredMap { if source_validation_error(&update, self.static_frame_id()).is_some() { return false; } + let Some(static_grid) = self.static_grid.as_ref() else { + return false; + }; let reset_source = update.reset_source; let key = ObservationSourceKey { @@ -105,14 +123,10 @@ impl LayeredMap { 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(); + changed |= self.dynamic_cells.remove(&key).is_some(); } 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 { @@ -128,16 +142,6 @@ impl LayeredMap { _ => continue, }; - let regions: Vec<_> = patch - .regions - .into_iter() - .filter(|region| region_validation_error(region).is_none()) - .map(|region| transform_region(region, &robot_pose)) - .collect(); - if 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); @@ -157,15 +161,35 @@ impl LayeredMap { 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(), - frame_id: frame_id.clone(), - update_type, - occupancy_value, - regions, - expires_at_nsec, - }); + let cell_indices = patch + .regions + .into_iter() + .filter(|region| region_validation_error(region).is_none()) + .map(|region| transform_region(region, &robot_pose)) + .flat_map(|region| rasterized_indices(static_grid, ®ion)) + .collect::>(); + if cell_indices.is_empty() { + continue; + } + + let sequence = self.next_sequence; + self.next_sequence = self.next_sequence.wrapping_add(1); + let source_cells = self.dynamic_cells.entry(key.clone()).or_default(); + for cell_index in cell_indices { + let history = source_cells + .entry(DynamicCellKey { + update_type, + cell_index, + }) + .or_default(); + // Keep older entries only when they may outlive the new one. + history.retain(|cell| cell.expires_at_nsec > expires_at_nsec); + history.push(DynamicCell { + occupancy_value, + expires_at_nsec, + sequence, + }); + } changed = true; } @@ -180,10 +204,18 @@ impl LayeredMap { } 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(); + let mut changed = false; + self.dynamic_cells.retain(|_, cells| { + cells.retain(|_, history| { + history.retain(|cell| { + let active = cell.expires_at_nsec > now_nsec; + changed |= !active; + active + }); + !history.is_empty() + }); + !cells.is_empty() + }); if changed { self.revision = self.revision.wrapping_add(1); } @@ -193,20 +225,26 @@ 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.frame_id == frame_id && obs.update_type == DynamicUpdateType::Clear) - { - rasterize_observation(&mut composed, observation); - } - for observation in self.dynamic_observations.iter().filter(|obs| { - obs.frame_id == frame_id && obs.update_type == DynamicUpdateType::Obstacle - }) { - rasterize_observation(&mut composed, observation); + for update_type in [DynamicUpdateType::Clear, DynamicUpdateType::Obstacle] { + let mut cells = self + .dynamic_cells + .values() + .flat_map(|source_cells| source_cells.iter()) + .filter_map(|(key, history)| { + if key.update_type != update_type { + return None; + } + history.last().map(|cell| (key, cell)) + }) + .collect::>(); + cells.sort_by_key(|(_, cell)| cell.sequence); + + for (key, cell) in cells { + if let Some(value) = composed.data.get_mut(key.cell_index) { + *value = cell.occupancy_value; + } + } } Some(composed) @@ -246,9 +284,23 @@ impl Default for LayeredMapServerConfig { } } +fn push_pending_region_update( + updates: &mut VecDeque, + update: MapRegionUpdate, +) -> Option { + let dropped = if updates.len() >= MAX_PENDING_REGION_UPDATES { + updates.pop_front() + } else { + None + }; + updates.push_back(update); + dropped +} + pub struct LayeredMapServer { node: Node, map: LayeredMap, + pending_region_updates: VecDeque, map_publisher: rclrs::Publisher, last_published_revision: Option, } @@ -264,6 +316,7 @@ impl LayeredMapServer { Ok(Self { node, map: LayeredMap::new(config.default_ttl), + pending_region_updates: VecDeque::new(), map_publisher, last_published_revision: None, }) @@ -271,6 +324,25 @@ impl LayeredMapServer { fn handle_static_map(&mut self, msg: OccupancyGrid) { self.map.set_static_map(msg); + let pending_updates = std::mem::take(&mut self.pending_region_updates); + let pending_count = pending_updates.len(); + if pending_count > 0 { + let now_nsec = self.now_nsec(); + let mut changed_count = 0; + for update in pending_updates { + log_region_update_errors(&self.node, &update, self.map.static_frame_id()); + if self.map.ingest_region_update(update, now_nsec) { + changed_count += 1; + } + } + rclrs::log!( + self.node.logger(), + "Rasterized {} queued region updates; {} changed the map", + pending_count, + changed_count + ); + } + rclrs::log!( self.node.logger(), "Received static map, revision {}", @@ -280,12 +352,36 @@ impl LayeredMapServer { } fn handle_region_update(&mut self, msg: MapRegionUpdate) { + if self.map.static_grid.is_none() { + log_region_update_errors(&self.node, &msg, None); + if source_validation_error(&msg, None).is_some() { + return; + } + + let source_id = msg.source.source_id.clone(); + if let Some(dropped) = push_pending_region_update(&mut self.pending_region_updates, msg) + { + rclrs::log_warn!( + self.node.logger(), + "Region update queue is full; dropped oldest update from '{}'", + dropped.source.source_id + ); + } + rclrs::log!( + self.node.logger(), + "Queued region update from '{}' until a static map is available ({} queued)", + source_id, + self.pending_region_updates.len() + ); + return; + } + 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!( self.node.logger(), - "Accepted region update, revision {}, active observations {}", + "Accepted region update, revision {}, active rasterized cells {}", self.map.revision(), self.map.dynamic_observation_count() ); @@ -298,7 +394,7 @@ impl LayeredMapServer { if self.map.prune_expired(now_nsec) { rclrs::log!( self.node.logger(), - "Pruned expired observations, revision {}, active observations {}", + "Pruned expired observations, revision {}, active rasterized cells {}", self.map.revision(), self.map.dynamic_observation_count() ); @@ -569,14 +665,12 @@ fn grid_len(grid: &OccupancyGrid) -> Option { 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 same_grid_geometry(lhs: &OccupancyGrid, rhs: &OccupancyGrid) -> bool { + lhs.header.frame_id == rhs.header.frame_id + && lhs.info.width == rhs.info.width + && lhs.info.height == rhs.info.height + && lhs.info.resolution == rhs.info.resolution + && lhs.info.origin == rhs.info.origin } fn rasterized_indices(grid: &OccupancyGrid, region: &Region) -> Vec { @@ -949,13 +1043,14 @@ mod tests { 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)] ), 0, )); + assert_eq!(map.dynamic_observation_count(), 0); let composed = map.compose().unwrap(); assert!(composed.data.iter().all(|cell| *cell == 0)); @@ -1167,4 +1262,143 @@ mod tests { assert_eq!(composed.data[1 * 5 + 1], 0); assert_eq!(composed.data[3 * 5 + 3], 100); } + + #[test] + fn repeated_regions_refresh_rasterized_cells_without_stacking() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + + let mut first = update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]); + first.patches[0].occupancy_value = 40; + first.patches[0].ttl_sec = 1.0; + assert!(map.ingest_region_update(first, 0)); + assert_eq!(map.dynamic_observation_count(), 1); + + let mut refreshed = update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]); + refreshed.source.header.stamp.sec = 2; + refreshed.patches[0].occupancy_value = 80; + refreshed.patches[0].ttl_sec = 10.0; + assert!(map.ingest_region_update(refreshed, 0)); + assert_eq!(map.dynamic_observation_count(), 1); + assert!(!map.prune_expired(3 * NANOS_PER_SECOND)); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 3 + 1], 80); + + assert!(map.prune_expired(13 * NANOS_PER_SECOND)); + assert_eq!(map.dynamic_observation_count(), 0); + } + + #[test] + fn longer_lived_cell_evidence_resurfaces_after_newer_evidence_expires() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + + let mut first = update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]); + first.patches[0].occupancy_value = 40; + first.patches[0].ttl_sec = 10.0; + assert!(map.ingest_region_update(first, 0)); + + let mut newer = update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]); + newer.source.header.stamp.sec = 2; + newer.patches[0].occupancy_value = 80; + newer.patches[0].ttl_sec = 1.0; + assert!(map.ingest_region_update(newer, 0)); + assert_eq!(map.dynamic_observation_count(), 1); + assert_eq!(map.compose().unwrap().data[1 * 3 + 1], 80); + + assert!(map.prune_expired(4 * NANOS_PER_SECOND)); + assert_eq!(map.dynamic_observation_count(), 1); + assert_eq!(map.compose().unwrap().data[1 * 3 + 1], 40); + + assert!(map.prune_expired(12 * NANOS_PER_SECOND)); + assert_eq!(map.dynamic_observation_count(), 0); + } + + #[test] + fn most_recent_source_wins_between_same_type_cell_contributions() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + + let mut first = update_from( + "robot_1/local_costmap", + MapRegionPatch::UPDATE_OBSTACLE, + vec![point(1.0, 1.0)], + ); + first.patches[0].occupancy_value = 40; + assert!(map.ingest_region_update(first, 0)); + + let mut second = update_from( + "robot_2/local_costmap", + MapRegionPatch::UPDATE_OBSTACLE, + vec![point(1.0, 1.0)], + ); + second.patches[0].occupancy_value = 80; + second.patches[0].ttl_sec = 1.0; + assert!(map.ingest_region_update(second, 0)); + assert_eq!(map.dynamic_observation_count(), 2); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 3 + 1], 80); + + assert!(map.prune_expired(3 * NANOS_PER_SECOND)); + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 3 + 1], 40); + } + + #[test] + fn static_map_updates_preserve_cells_only_when_geometry_matches() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + assert!(map.ingest_region_update( + update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]), + 0, + )); + + map.set_static_map(static_grid(3, 3, -1)); + + assert_eq!(map.dynamic_observation_count(), 1); + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 3 + 1], 100); + assert_eq!(composed.data[0], -1); + + map.set_static_map(static_grid(4, 4, 0)); + + assert_eq!(map.dynamic_observation_count(), 0); + assert!(map.compose().unwrap().data.iter().all(|cell| *cell == 0)); + } + + #[test] + fn updates_are_rejected_until_a_static_grid_can_rasterize_them() { + let mut map = LayeredMap::default(); + + assert!(!map.ingest_region_update( + update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]), + 0, + )); + assert_eq!(map.dynamic_observation_count(), 0); + } + + #[test] + fn pending_region_update_queue_is_bounded_and_fifo() { + let mut pending = VecDeque::new(); + + for index in 0..=MAX_PENDING_REGION_UPDATES { + let mut update = update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]); + update.source.source_id = format!("source_{index}"); + let dropped = push_pending_region_update(&mut pending, update); + if index < MAX_PENDING_REGION_UPDATES { + assert!(dropped.is_none()); + } else { + assert_eq!(dropped.unwrap().source.source_id, "source_0"); + } + } + + assert_eq!(pending.len(), MAX_PENDING_REGION_UPDATES); + assert_eq!(pending.front().unwrap().source.source_id, "source_1"); + assert_eq!( + pending.back().unwrap().source.source_id, + format!("source_{MAX_PENDING_REGION_UPDATES}") + ); + } } From d630fc9cd8e1e482bb744553f61c85d158954089 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Tue, 28 Jul 2026 21:01:08 +0200 Subject: [PATCH 30/38] fix(map): clear stale same-source obstacles Remove older obstacle cells when a newer clear region from the same source covers them. Preserve unobserved cells until TTL expiry and keep other sources independent. Signed-off-by: SamuelFoo --- map_server/rmf_layered_map_server/README.md | 8 +-- map_server/rmf_layered_map_server/src/lib.rs | 62 ++++++++++++++++++++ rmf_layered_map_msgs/msg/MapRegionPatch.msg | 3 +- rmf_layered_map_msgs/msg/MapRegionUpdate.msg | 4 +- 4 files changed, 67 insertions(+), 10 deletions(-) diff --git a/map_server/rmf_layered_map_server/README.md b/map_server/rmf_layered_map_server/README.md index e6e5c34..4c1b5bb 100644 --- a/map_server/rmf_layered_map_server/README.md +++ b/map_server/rmf_layered_map_server/README.md @@ -12,13 +12,7 @@ Default topics: Dynamic observations expire according to their TTL, so transient obstacles do not remain in the global planning map forever. The server transforms and rasterizes each region on arrival, then stores cell contributions by source, map, update type, and cell. Repeated observations refresh matching cells instead of stacking region snapshots. Older evidence is retained only when it outlives a newer contribution. -`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. +`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. Clear cells remove older obstacles from the same source, while current obstacle endpoints and obstacles reported by other sources remain occupied. Observations outside the clear regions remain active until their TTL expires. If the snapshot replaces the source's entire previous observation state, set `reset_source` so the server removes all 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. 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 diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs index 2b176d5..81990f0 100644 --- a/map_server/rmf_layered_map_server/src/lib.rs +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -176,6 +176,14 @@ impl LayeredMap { self.next_sequence = self.next_sequence.wrapping_add(1); let source_cells = self.dynamic_cells.entry(key.clone()).or_default(); for cell_index in cell_indices { + if update_type == DynamicUpdateType::Clear { + // Clear cells replace older obstacles from the same source. + source_cells.remove(&DynamicCellKey { + update_type: DynamicUpdateType::Obstacle, + cell_index, + }); + } + let history = source_cells .entry(DynamicCellKey { update_type, @@ -1168,6 +1176,60 @@ mod tests { assert_eq!(composed.data[0], -1); } + #[test] + fn clear_regions_replace_only_observed_same_source_obstacles() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(5, 5, 0)); + + assert!(map.ingest_region_update( + update( + MapRegionPatch::UPDATE_OBSTACLE, + vec![point(1.0, 1.0), point(3.0, 3.0)] + ), + 0, + )); + + let mut clear = update( + MapRegionPatch::UPDATE_CLEAR, + vec![rectangle(0.0, 0.0, 2.0, 2.0)], + ); + clear.source.header.stamp.sec = 2; + assert!(map.ingest_region_update(clear, NANOS_PER_SECOND)); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 5 + 1], 0); + assert_eq!(composed.data[3 * 5 + 3], 100); + + assert!(map.prune_expired(11 * NANOS_PER_SECOND)); + let composed = map.compose().unwrap(); + assert_eq!(composed.data[3 * 5 + 3], 0); + } + + #[test] + fn clear_regions_do_not_remove_other_source_obstacles() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(5, 5, 0)); + + assert!(map.ingest_region_update( + update_from( + "robot_2/local_costmap", + MapRegionPatch::UPDATE_OBSTACLE, + vec![point(1.0, 1.0)] + ), + 0, + )); + + let mut clear = update( + MapRegionPatch::UPDATE_CLEAR, + vec![rectangle(0.0, 0.0, 2.0, 2.0)], + ); + clear.source.header.stamp.sec = 2; + assert!(map.ingest_region_update(clear, NANOS_PER_SECOND)); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 5 + 1], 100); + } + #[test] fn newer_obstacles_survive_older_clear_updates_received_late() { let mut map = LayeredMap::default(); diff --git a/rmf_layered_map_msgs/msg/MapRegionPatch.msg b/rmf_layered_map_msgs/msg/MapRegionPatch.msg index d735041..4a686d5 100644 --- a/rmf_layered_map_msgs/msg/MapRegionPatch.msg +++ b/rmf_layered_map_msgs/msg/MapRegionPatch.msg @@ -3,7 +3,8 @@ # The regions should be interpreted as temporary occupied space. uint8 UPDATE_OBSTACLE=1 -# The regions should be interpreted as temporary free space. +# The regions should be interpreted as temporary free space. They replace older +# same-source obstacles but do not affect other sources. uint8 UPDATE_CLEAR=2 # UPDATE_OBSTACLE or UPDATE_CLEAR. diff --git a/rmf_layered_map_msgs/msg/MapRegionUpdate.msg b/rmf_layered_map_msgs/msg/MapRegionUpdate.msg index e067666..57dbc3d 100644 --- a/rmf_layered_map_msgs/msg/MapRegionUpdate.msg +++ b/rmf_layered_map_msgs/msg/MapRegionUpdate.msg @@ -10,6 +10,6 @@ MapObservationSource source bool reset_source # 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. +# patches before obstacle patches so older same-source obstacles are removed +# along clear rays before current obstacle endpoints are marked. MapRegionPatch[] patches From 6d4881b086bf6fc990cc4494ec4a4ce96b2f3dbd Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Tue, 28 Jul 2026 21:01:55 +0200 Subject: [PATCH 31/38] refactor(map-demo): use backend observation retention Remove scan-side obstacle memory and per-update source resets. Keep self-filtering stateless and configure the replanning scenario with a 60 second observation TTL. Signed-off-by: SamuelFoo --- .../rmf_layered_map_server_demo/README.md | 3 +- .../launch/nav2_observations.launch.py | 9 -- .../replan_obstacle_launch.py | 15 +-- .../scan_region_publisher.py | 121 ++---------------- .../test/test_scan_region_publisher.py | 71 +--------- 5 files changed, 22 insertions(+), 197 deletions(-) diff --git a/map_server/rmf_layered_map_server_demo/README.md b/map_server/rmf_layered_map_server_demo/README.md index 7ea5acb..df7e38c 100644 --- a/map_server/rmf_layered_map_server_demo/README.md +++ b/map_server/rmf_layered_map_server_demo/README.md @@ -27,7 +27,6 @@ By default, the demo opens three robot-local RViz windows and one combined-map v * Set `use_nav2_rviz:=False` or `use_global_rviz:=False` to disable either RViz view. * Set `move_robots:=False` to disable robot movement. * Set `spawn_clutter:=False` to use the unmodified warehouse world. -* Set `reset_source:=True` to show only the latest scan from each robot. * Use `map` and `params_file` to override the warehouse map and shared Nav2 parameters. * `beam_stride:=1` and `publish_period_sec:=0.5` control scan sampling. * `max_observation_range:=2.5` and `ttl_sec:=10.0` control range and retention. @@ -72,5 +71,5 @@ Useful launch arguments: * `use_global_rviz:=False` runs without the combined RViz view. * `spawn_delay_sec:=1.0` controls the delay after initial plans are received. * `scenario_timeout_sec:=180.0` controls how long the demo waits for replanning. -* `obstacle_memory_sec:=-1.0` retains obstacle endpoints for the scanner's lifetime. Use `0.0` to disable memory or a positive value for finite retention. +* `ttl_sec:=60.0` controls how long observations outside the current scan remain in the layered map. New clear rays remove stale same-source obstacles before the TTL expires. * `self_filter_radius:=0.22` excludes scan returns inside the robot body. diff --git a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py index 5f18d63..49cfef5 100644 --- a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py +++ b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py @@ -65,7 +65,6 @@ def generate_launch_description(): beam_stride = LaunchConfiguration('beam_stride') publish_period_sec = LaunchConfiguration('publish_period_sec') ttl_sec = LaunchConfiguration('ttl_sec') - reset_source = LaunchConfiguration('reset_source') max_observation_range = LaunchConfiguration('max_observation_range') declarations = [ @@ -116,11 +115,6 @@ def generate_launch_description(): default_value='10.0', description='Lifetime of each robot observation snapshot.', ), - DeclareLaunchArgument( - 'reset_source', - default_value='False', - description='Whether each snapshot replaces the source history.', - ), DeclareLaunchArgument( 'max_observation_range', default_value='2.5', @@ -178,9 +172,6 @@ def generate_launch_description(): publish_period_sec, value_type=float ), 'ttl_sec': ParameterValue(ttl_sec, value_type=float), - 'reset_source': ParameterValue( - reset_source, value_type=bool - ), 'max_observation_range': ParameterValue( max_observation_range, value_type=float ), diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py index 3d1d8b0..3d728e6 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py @@ -68,7 +68,7 @@ def generate_replan_launch_description(scenario_name): use_global_rviz = LaunchConfiguration('use_global_rviz') spawn_delay_sec = LaunchConfiguration('spawn_delay_sec') scenario_timeout_sec = LaunchConfiguration('scenario_timeout_sec') - obstacle_memory_sec = LaunchConfiguration('obstacle_memory_sec') + ttl_sec = LaunchConfiguration('ttl_sec') self_filter_radius = LaunchConfiguration('self_filter_radius') demo_params_file = RewrittenYaml( source_file=params_file, @@ -133,9 +133,9 @@ def generate_replan_launch_description(scenario_name): description='Timeout for observing a replan.', ), DeclareLaunchArgument( - 'obstacle_memory_sec', - default_value='-1.0', - description='Obstacle retention in seconds; negative keeps points.', + 'ttl_sec', + default_value='60.0', + description='Lifetime of observations that are not re-observed.', ), DeclareLaunchArgument( 'self_filter_radius', @@ -186,12 +186,7 @@ def generate_replan_launch_description(scenario_name): 'scan_topic': f'/{robot.name}/inner/scan', 'beam_stride': 1, 'publish_period_sec': 0.5, - 'ttl_sec': 10.0, - 'reset_source': True, - 'obstacle_memory_sec': ParameterValue( - obstacle_memory_sec, value_type=float - ), - 'obstacle_memory_resolution': 0.1, + 'ttl_sec': ParameterValue(ttl_sec, value_type=float), 'self_filter_radius': ParameterValue( self_filter_radius, value_type=float ), diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py index 58843aa..4004f1b 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from math import atan2, cos, sin - import rclpy from rclpy.node import Node from rclpy.qos import ( @@ -34,93 +32,17 @@ from .scan_conversion import scan_regions -def _yaw_from_quaternion(quaternion): - return atan2( - 2.0 * ( - quaternion.w * quaternion.z - + quaternion.x * quaternion.y - ), - 1.0 - 2.0 * ( - quaternion.y * quaternion.y - + quaternion.z * quaternion.z - ), - ) - - -class ObstacleMemory: - """Retain obstacle points across scan updates.""" - - def __init__( - self, - retention_sec=0.0, - resolution=0.1, - self_filter_radius=0.0, - ): - self.retention_sec = float(retention_sec) - self.resolution = float(resolution) - self.self_filter_radius = float(self_filter_radius) - self._points = {} - - @property - def enabled(self): - return self.retention_sec != 0.0 +def filter_obstacle_points(obstacle_points, self_filter_radius): + """Remove current scan returns that fall inside the robot body.""" + if self_filter_radius <= 0.0: + return list(obstacle_points) - def remember(self, obstacle_points, transform, now_sec): - filter_radius_squared = self.self_filter_radius ** 2 - if self.self_filter_radius > 0.0: - obstacle_points = [ - (x, y) - for x, y in obstacle_points - if x * x + y * y > filter_radius_squared - ] - - if not self.enabled: - return list(obstacle_points) - - yaw = _yaw_from_quaternion(transform.rotation) - cosine = cos(yaw) - sine = sin(yaw) - tx = transform.translation.x - ty = transform.translation.y - - if self.retention_sec > 0.0: - oldest = now_sec - self.retention_sec - self._points = { - key: value - for key, value in self._points.items() - if value[2] > oldest - } - - for local_x, local_y in obstacle_points: - global_x = tx + cosine * local_x - sine * local_y - global_y = ty + sine * local_x + cosine * local_y - key = ( - round(global_x / self.resolution), - round(global_y / self.resolution), - ) - self._points[key] = (global_x, global_y, now_sec) - - if self.self_filter_radius > 0.0: - self._points = { - key: value - for key, value in self._points.items() - if ( - (value[0] - tx) ** 2 - + (value[1] - ty) ** 2 - > filter_radius_squared - ) - } - - # Region points are relative to the current robot pose. - remembered = [] - for global_x, global_y, _ in self._points.values(): - dx = global_x - tx - dy = global_y - ty - remembered.append(( - cosine * dx + sine * dy, - -sine * dx + cosine * dy, - )) - return remembered + radius_squared = self_filter_radius ** 2 + return [ + (x, y) + for x, y in obstacle_points + if x * x + y * y > radius_squared + ] def make_scan_patches(clear_polygons, obstacle_points, ttl_sec): @@ -164,15 +86,6 @@ def __init__(self): self.map_name = self.declare_parameter('map_name', 'warehouse').value self.scan_topic = self.declare_parameter('scan_topic', 'scan').value self.ttl_sec = self.declare_parameter('ttl_sec', 10.0).value - self.reset_source = self.declare_parameter( - 'reset_source', False - ).value - self.obstacle_memory_sec = self.declare_parameter( - 'obstacle_memory_sec', 0.0 - ).value - self.obstacle_memory_resolution = self.declare_parameter( - 'obstacle_memory_resolution', 0.1 - ).value self.self_filter_radius = self.declare_parameter( 'self_filter_radius', 0.0 ).value @@ -192,17 +105,10 @@ def __init__(self): raise ValueError('publish_period_sec must not be negative') if self.beam_stride < 1: raise ValueError('beam_stride must be at least one') - if self.obstacle_memory_resolution <= 0.0: - raise ValueError('obstacle_memory_resolution must be positive') if self.self_filter_radius < 0.0: raise ValueError('self_filter_radius must not be negative') self.source_id = f'{self.robot_name}/scan' - self.obstacle_memory = ObstacleMemory( - self.obstacle_memory_sec, - self.obstacle_memory_resolution, - self.self_filter_radius, - ) self.last_publish_stamp_sec = None self.pending_scan = None self.publish_count = 0 @@ -231,7 +137,6 @@ def __init__(self): f'Converting {self.scan_topic} into region updates for ' f'{self.robot_name} (stride={self.beam_stride}, ' f'period={self.publish_period_sec:.2f}s, ' - f'obstacle_memory={self.obstacle_memory_sec:.1f}s, ' f'self_filter_radius={self.self_filter_radius:.2f}m)' ) @@ -283,10 +188,9 @@ def publish_pending_scan(self): self.max_observation_range, self.beam_stride, ) - obstacle_points = self.obstacle_memory.remember( + obstacle_points = filter_obstacle_points( obstacle_points, - transform.transform, - self.get_clock().now().nanoseconds / 1e9, + self.self_filter_radius, ) update = self.make_update(transform, clear_polygons, obstacle_points) self.region_update_publisher.publish(update) @@ -302,7 +206,6 @@ def publish_pending_scan(self): def make_update(self, transform, clear_polygons, obstacle_points): update = MapRegionUpdate() - update.reset_source = self.reset_source update.source = MapObservationSource() # The Rust map server currently uses a system-time clock. # Keep TTL and update ordering in that clock while using the scan stamp for TF. diff --git a/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py b/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py index 81aac18..e725cbd 100644 --- a/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py +++ b/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.py @@ -12,23 +12,14 @@ # See the License for the specific language governing permissions and # limitations under the License. -from geometry_msgs.msg import Transform from rmf_layered_map_msgs.msg import MapRegionPatch from rmf_layered_map_server_demo.scan_region_publisher import ( + filter_obstacle_points, make_scan_patches, - ObstacleMemory, ) from rmf_prototype_msgs.msg import Region -def _transform(x=0.0, y=0.0): - transform = Transform() - transform.translation.x = x - transform.translation.y = y - transform.rotation.w = 1.0 - return transform - - def test_scan_patches_clear_rays_before_marking_obstacle_endpoints(): patches = make_scan_patches( [(0.0, 0.0, 1.0, -0.1, 1.0, 0.1)], @@ -46,64 +37,10 @@ def test_scan_patches_clear_rays_before_marking_obstacle_endpoints(): assert patches[1].regions[0].hint == Region.HINT_POINT -def test_obstacle_memory_filters_points_inside_robot_body(): - memory = ObstacleMemory( - retention_sec=0.0, - self_filter_radius=0.22, - ) - - points = memory.remember( +def test_filters_current_scan_points_inside_robot_body(): + points = filter_obstacle_points( [(0.1, 0.0), (0.3, 0.0)], - _transform(), - now_sec=1.0, - ) - - assert points == [(0.3, 0.0)] - - -def test_obstacle_memory_prunes_points_the_robot_moves_over(): - memory = ObstacleMemory( - retention_sec=-1.0, self_filter_radius=0.22, ) - assert memory.remember( - [(1.0, 0.0)], - _transform(), - now_sec=1.0, - ) == [(1.0, 0.0)] - - assert memory.remember( - [], - _transform(x=1.0), - now_sec=2.0, - ) == [] - -def test_obstacle_memory_keeps_points_when_the_scan_window_moves(): - memory = ObstacleMemory(retention_sec=-1.0, resolution=0.1) - first = memory.remember( - [(1.0, 0.0), (2.0, 0.0)], - _transform(), - now_sec=1.0, - ) - moved = memory.remember( - [(1.0, 0.0)], - _transform(x=5.0), - now_sec=2.0, - ) - - assert sorted(first) == [(1.0, 0.0), (2.0, 0.0)] - assert sorted(moved) == [(-4.0, 0.0), (-3.0, 0.0), (1.0, 0.0)] - - -def test_finite_obstacle_memory_expires_old_scan_windows(): - memory = ObstacleMemory(retention_sec=5.0, resolution=0.1) - memory.remember([(1.0, 0.0)], _transform(), now_sec=1.0) - - remembered = memory.remember( - [(2.0, 0.0)], - _transform(), - now_sec=7.0, - ) - - assert remembered == [(2.0, 0.0)] + assert points == [(0.3, 0.0)] From 14ba7d5b9dd0ee44ef89365797a9cc9a3a2f3a77 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Tue, 28 Jul 2026 21:02:13 +0200 Subject: [PATCH 32/38] feat(map): publish per-source cell snapshots Publish transient-local snapshots of active rasterized cells grouped by observation source. Include map geometry and source metadata so consumers can attribute composed-map contributions without replaying region updates. Signed-off-by: SamuelFoo --- map_server/rmf_layered_map_server/README.md | 3 + map_server/rmf_layered_map_server/src/lib.rs | 142 +++++++++++++++++- .../test/test_layered_map_server.py | 42 +++++- rmf_layered_map_msgs/CMakeLists.txt | 5 +- .../msg/MapSourceContribution.msg | 9 ++ .../msg/MapSourceSnapshot.msg | 10 ++ rmf_layered_map_msgs/package.xml | 2 + 7 files changed, 205 insertions(+), 8 deletions(-) create mode 100644 rmf_layered_map_msgs/msg/MapSourceContribution.msg create mode 100644 rmf_layered_map_msgs/msg/MapSourceSnapshot.msg diff --git a/map_server/rmf_layered_map_server/README.md b/map_server/rmf_layered_map_server/README.md index 4c1b5bb..039b7ee 100644 --- a/map_server/rmf_layered_map_server/README.md +++ b/map_server/rmf_layered_map_server/README.md @@ -9,9 +9,12 @@ Default topics: - `/map/static`: static `nav_msgs/OccupancyGrid` - `/map/region_updates`: `rmf_layered_map_msgs/MapRegionUpdate` - `/map`: composed `nav_msgs/OccupancyGrid` +- `/map/source_contributions`: active cells grouped by source Dynamic observations expire according to their TTL, so transient obstacles do not remain in the global planning map forever. The server transforms and rasterizes each region on arrival, then stores cell contributions by source, map, update type, and cell. Repeated observations refresh matching cells instead of stacking region snapshots. Older evidence is retained only when it outlives a newer contribution. +`/map/source_contributions` publishes the active rasterized cells for each source whenever the composed map changes. + `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. Clear cells remove older obstacles from the same source, while current obstacle endpoints and obstacles reported by other sources remain occupied. Observations outside the clear regions remain active until their TTL expires. If the snapshot replaces the source's entire previous observation state, set `reset_source` so the server removes all 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. Patch regions are expressed in the robot-local observation frame. The server diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs index 81990f0..debc81f 100644 --- a/map_server/rmf_layered_map_server/src/lib.rs +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -16,7 +16,10 @@ use rclrs::{IntoPrimitiveOptions, Node, PrimitiveOptions}; use ros_env::{ geometry_msgs::msg::Pose, nav_msgs::msg::OccupancyGrid, - rmf_layered_map_msgs::msg::{MapRegionPatch, MapRegionUpdate}, + rmf_layered_map_msgs::msg::{ + MapObservationSource, MapRegionPatch, MapRegionUpdate, MapSourceContribution, + MapSourceSnapshot, + }, rmf_prototype_msgs::msg::Region, }; use std::{ @@ -57,6 +60,7 @@ struct ObservationSourceKey { pub struct LayeredMap { static_grid: Option, dynamic_cells: HashMap>>, + source_metadata: HashMap, latest_source_stamps: HashMap, default_ttl_nsec: i128, next_sequence: u64, @@ -68,6 +72,7 @@ impl LayeredMap { Self { static_grid: None, dynamic_cells: HashMap::new(), + source_metadata: HashMap::new(), latest_source_stamps: HashMap::new(), default_ttl_nsec: duration_to_nsec(default_ttl), next_sequence: 0, @@ -105,6 +110,7 @@ impl LayeredMap { }; let reset_source = update.reset_source; + let source_metadata = update.source.clone(); let key = ObservationSourceKey { source_id: update.source.source_id.clone(), map_name: update.source.map_name.clone(), @@ -202,6 +208,7 @@ impl LayeredMap { } if changed || reset_source { + self.source_metadata.insert(key.clone(), source_metadata); self.latest_source_stamps.insert(key, stamp_nsec); } @@ -258,6 +265,58 @@ impl LayeredMap { Some(composed) } + pub fn source_snapshot(&self) -> Option { + let static_grid = self.static_grid.as_ref()?; + let mut source_entries = self.dynamic_cells.iter().collect::>(); + source_entries.sort_by(|(left, _), (right, _)| { + (&left.source_id, &left.map_name).cmp(&(&right.source_id, &right.map_name)) + }); + + let mut sources = Vec::new(); + for (source_key, source_cells) in source_entries { + let mut final_cells = HashMap::new(); + for update_type in [DynamicUpdateType::Clear, DynamicUpdateType::Obstacle] { + for (cell_key, history) in source_cells { + if cell_key.update_type != update_type { + continue; + } + if let Some(cell) = history.last() { + final_cells.insert(cell_key.cell_index, cell.occupancy_value); + } + } + } + if final_cells.is_empty() { + continue; + } + + let mut cells = final_cells.into_iter().collect::>(); + cells.sort_by_key(|(cell_index, _)| *cell_index); + let mut contribution = MapSourceContribution { + source: self + .source_metadata + .get(source_key) + .cloned() + .unwrap_or_default(), + ..Default::default() + }; + for (cell_index, occupancy_value) in cells { + if let Ok(cell_index) = u64::try_from(cell_index) { + contribution.cell_indices.push(cell_index); + contribution.occupancy_values.push(occupancy_value); + } + } + if !contribution.cell_indices.is_empty() { + sources.push(contribution); + } + } + + Some(MapSourceSnapshot { + header: static_grid.header.clone(), + info: static_grid.info.clone(), + sources, + }) + } + fn static_frame_id(&self) -> Option<&str> { self.static_grid .as_ref() @@ -276,6 +335,7 @@ pub struct LayeredMapServerConfig { pub static_map_topic: String, pub region_updates_topic: String, pub composed_map_topic: String, + pub source_contributions_topic: String, pub default_ttl: Duration, pub publish_period: Duration, } @@ -286,6 +346,7 @@ impl Default for LayeredMapServerConfig { static_map_topic: "/map/static".to_string(), region_updates_topic: "/map/region_updates".to_string(), composed_map_topic: "/map".to_string(), + source_contributions_topic: "/map/source_contributions".to_string(), default_ttl: Duration::from_secs(30), publish_period: Duration::from_millis(250), } @@ -310,6 +371,7 @@ pub struct LayeredMapServer { map: LayeredMap, pending_region_updates: VecDeque, map_publisher: rclrs::Publisher, + source_contributions_publisher: rclrs::Publisher, last_published_revision: Option, } @@ -320,12 +382,16 @@ impl LayeredMapServer { ) -> Result> { let map_publisher = node.create_publisher::(composed_map_qos(&config.composed_map_topic))?; + let source_contributions_publisher = node.create_publisher::( + source_contributions_qos(&config.source_contributions_topic), + )?; Ok(Self { node, map: LayeredMap::new(config.default_ttl), pending_region_updates: VecDeque::new(), map_publisher, + source_contributions_publisher, last_published_revision: None, }) } @@ -418,9 +484,23 @@ impl LayeredMapServer { let Some(composed) = self.map.compose() else { return; }; + let source_snapshot = self.map.source_snapshot(); match self.map_publisher.publish(&composed) { Ok(()) => { + if let Some(source_snapshot) = source_snapshot { + if let Err(err) = self + .source_contributions_publisher + .publish(&source_snapshot) + { + rclrs::log_error!( + self.node.logger(), + "Failed to publish source contributions: {:?}", + err + ); + return; + } + } self.last_published_revision = Some(self.map.revision()); rclrs::log!( self.node.logger(), @@ -497,6 +577,10 @@ fn composed_map_qos(topic: &str) -> PrimitiveOptions<'_> { topic.keep_last(MAP_QOS_DEPTH).transient_local().reliable() } +fn source_contributions_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() } @@ -1230,6 +1314,62 @@ mod tests { assert_eq!(composed.data[1 * 5 + 1], 100); } + #[test] + fn source_snapshot_reports_authoritative_rasterized_contributions() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(5, 5, 0)); + + assert!(map.ingest_region_update( + update( + MapRegionPatch::UPDATE_OBSTACLE, + vec![point(1.0, 1.0), point(3.0, 3.0)] + ), + 0, + )); + assert!(map.ingest_region_update( + update_from( + "robot_2/local_costmap", + MapRegionPatch::UPDATE_OBSTACLE, + vec![point(2.0, 2.0)] + ), + 0, + )); + + let mut clear = update( + MapRegionPatch::UPDATE_CLEAR, + vec![rectangle(0.0, 0.0, 2.0, 2.0)], + ); + clear.source.header.stamp.sec = 2; + assert!(map.ingest_region_update(clear, NANOS_PER_SECOND)); + + let snapshot = map.source_snapshot().unwrap(); + assert_eq!(snapshot.info.width, 5); + assert_eq!(snapshot.info.height, 5); + assert_eq!(snapshot.sources.len(), 2); + + let robot_1 = snapshot + .sources + .iter() + .find(|source| source.source.source_id == "robot_1/local_costmap") + .unwrap(); + let robot_1_cells = robot_1 + .cell_indices + .iter() + .copied() + .zip(robot_1.occupancy_values.iter().copied()) + .collect::>(); + assert_eq!(robot_1_cells.get(&6), Some(&0)); + assert_eq!(robot_1_cells.get(&18), Some(&100)); + + let robot_2 = snapshot + .sources + .iter() + .find(|source| source.source.source_id == "robot_2/local_costmap") + .unwrap(); + assert_eq!(robot_2.cell_indices, vec![12]); + assert_eq!(robot_2.occupancy_values, vec![100]); + } + #[test] fn newer_obstacles_survive_older_clear_updates_received_late() { let mut map = LayeredMap::default(); 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 235f0ac..918d509 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 @@ -26,6 +26,7 @@ MapObservationSource, MapRegionPatch, MapRegionUpdate, + MapSourceSnapshot, ) from rmf_prototype_msgs.msg import Region @@ -58,6 +59,7 @@ def tearDownClass(cls): def setUp(self): self.node = rclpy.create_node('test_layered_map_server_node') self.maps = [] + self.source_snapshots = [] transient_qos = QoSProfile( depth=10, @@ -87,6 +89,12 @@ def setUp(self): self.maps.append, qos_profile=transient_qos, ) + self.source_subscription = self.node.create_subscription( + MapSourceSnapshot, + '/map/source_contributions', + self.source_snapshots.append, + qos_profile=transient_qos, + ) def tearDown(self): self.node.destroy_node() @@ -114,6 +122,16 @@ def last_cell(self, x, y): latest = self.maps[-1] return latest.data[y * latest.info.width + x] + def last_source_cell(self, source_id, cell_index): + if not self.source_snapshots: + return None + for source in self.source_snapshots[-1].sources: + if source.source.source_id != source_id: + continue + values = dict(zip(source.cell_indices, source.occupancy_values)) + return values.get(cell_index) + return None + def test_region_update_is_composed_reset_and_expires(self): time.sleep(2.0) @@ -136,9 +154,12 @@ def test_region_update_is_composed_reset_and_expires(self): self.publish_until( self.region_update_publisher, update, - lambda: self.last_cell(2, 2) == 100, + lambda: ( + self.last_cell(2, 2) == 100 + and self.last_source_cell('test/obstacle', 12) == 100 + ), ), - 'layered map server did not compose the obstacle region', + 'layered map server did not publish the obstacle contribution', ) reset = reset_update() @@ -147,9 +168,12 @@ def test_region_update_is_composed_reset_and_expires(self): self.publish_until( self.region_update_publisher, reset, - lambda: self.last_cell(2, 2) == 0, + lambda: ( + self.last_cell(2, 2) == 0 + and self.last_source_cell('test/obstacle', 12) is None + ), ), - 'layered map server did not reset the obstacle region', + 'layered map server did not reset the obstacle source', ) update = obstacle_update() @@ -163,8 +187,14 @@ def test_region_update_is_composed_reset_and_expires(self): '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', + self.wait_for( + lambda: ( + self.last_cell(2, 2) == 0 + and self.last_source_cell('test/obstacle', 12) is None + ), + timeout=4.0, + ), + 'layered map server did not prune the expired obstacle source', ) diff --git a/rmf_layered_map_msgs/CMakeLists.txt b/rmf_layered_map_msgs/CMakeLists.txt index 3bdd01f..dc50aae 100644 --- a/rmf_layered_map_msgs/CMakeLists.txt +++ b/rmf_layered_map_msgs/CMakeLists.txt @@ -13,6 +13,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(rmf_prototype_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) @@ -21,11 +22,13 @@ set(msg_files "msg/MapObservationSource.msg" "msg/MapRegionPatch.msg" "msg/MapRegionUpdate.msg" + "msg/MapSourceContribution.msg" + "msg/MapSourceSnapshot.msg" ) rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} - DEPENDENCIES geometry_msgs rmf_prototype_msgs std_msgs + DEPENDENCIES geometry_msgs nav_msgs rmf_prototype_msgs std_msgs ADD_LINTER_TESTS ) diff --git a/rmf_layered_map_msgs/msg/MapSourceContribution.msg b/rmf_layered_map_msgs/msg/MapSourceContribution.msg new file mode 100644 index 0000000..f618773 --- /dev/null +++ b/rmf_layered_map_msgs/msg/MapSourceContribution.msg @@ -0,0 +1,9 @@ +# MapSourceContribution.msg + +# Source that owns these rasterized cell contributions. +MapObservationSource source + +# Row-major indices into MapSourceSnapshot.info. Each index has a matching +# OccupancyGrid-compatible value in occupancy_values. +uint64[] cell_indices +int8[] occupancy_values diff --git a/rmf_layered_map_msgs/msg/MapSourceSnapshot.msg b/rmf_layered_map_msgs/msg/MapSourceSnapshot.msg new file mode 100644 index 0000000..334c2dc --- /dev/null +++ b/rmf_layered_map_msgs/msg/MapSourceSnapshot.msg @@ -0,0 +1,10 @@ +# MapSourceSnapshot.msg + +# Global frame for the rasterized source contributions. +std_msgs/Header header + +# Geometry shared by every source contribution in this snapshot. +nav_msgs/MapMetaData info + +# Complete active contribution state, grouped by observation source. +MapSourceContribution[] sources diff --git a/rmf_layered_map_msgs/package.xml b/rmf_layered_map_msgs/package.xml index 2d8c458..7614a83 100644 --- a/rmf_layered_map_msgs/package.xml +++ b/rmf_layered_map_msgs/package.xml @@ -11,10 +11,12 @@ rosidl_default_generators geometry_msgs + nav_msgs rmf_prototype_msgs std_msgs geometry_msgs + nav_msgs rmf_prototype_msgs std_msgs rosidl_default_runtime From ccea5f46e08ac9679df33cd4d20efbe365f03ea1 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Tue, 28 Jul 2026 21:02:58 +0200 Subject: [PATCH 33/38] feat(map-demo): show retained source contributions Render backend source snapshots as robot-colored obstacle markers. Reconcile each complete snapshot with RViz and launch the transient-local display in both map demos. Signed-off-by: SamuelFoo --- .../launch/nav2_observations.launch.py | 7 + .../replan_obstacle_launch.py | 7 + .../source_contribution_visualizer.py | 199 ++++++++++++++++++ .../rviz/nav2_observations.rviz | 13 ++ .../rmf_layered_map_server_demo/setup.py | 2 + .../test_source_contribution_visualizer.py | 83 ++++++++ 6 files changed, 311 insertions(+) create mode 100644 map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/source_contribution_visualizer.py create mode 100644 map_server/rmf_layered_map_server_demo/test/test_source_contribution_visualizer.py diff --git a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py index 49cfef5..1394ad6 100644 --- a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py +++ b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py @@ -152,6 +152,12 @@ def generate_launch_description(): executable='region_update_visualizer', output='screen', ) + source_contribution_visualizer = Node( + condition=IfCondition(use_global_rviz), + package='rmf_layered_map_server_demo', + executable='source_contribution_visualizer', + output='screen', + ) observation_nodes = [] goal_nodes = [] @@ -234,6 +240,7 @@ def generate_launch_description(): nav2_simulation, layered_map_server, region_visualizer, + source_contribution_visualizer, *observation_nodes, *goal_nodes, clutter_spawner, diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py index 3d728e6..e80e2b4 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py @@ -173,6 +173,12 @@ def generate_replan_launch_description(scenario_name): executable='region_update_visualizer', output='screen', ) + source_contribution_visualizer = Node( + condition=IfCondition(use_global_rviz), + package='rmf_layered_map_server_demo', + executable='source_contribution_visualizer', + output='screen', + ) observation_nodes = [ Node( package='rmf_layered_map_server_demo', @@ -275,6 +281,7 @@ def generate_replan_launch_description(scenario_name): nav2_simulation, layered_map_server, region_visualizer, + source_contribution_visualizer, *observation_nodes, destination_server, path_server, diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/source_contribution_visualizer.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/source_contribution_visualizer.py new file mode 100644 index 0000000..3767477 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/source_contribution_visualizer.py @@ -0,0 +1,199 @@ +# 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 math import atan2, cos, sin + +from geometry_msgs.msg import Point +import rclpy +from rclpy.node import Node +from rclpy.qos import ( + DurabilityPolicy, + QoSProfile, + ReliabilityPolicy, +) +from rmf_layered_map_msgs.msg import MapSourceSnapshot +from visualization_msgs.msg import Marker, MarkerArray + +from .region_update_visualizer import OBSTACLE_MARKER_Z, SOURCE_COLORS +from .replan_scenarios import ROBOT_COLORS + + +def _yaw_from_quaternion(quaternion): + return atan2( + 2.0 * ( + quaternion.w * quaternion.z + + quaternion.x * quaternion.y + ), + 1.0 - 2.0 * ( + quaternion.y * quaternion.y + + quaternion.z * quaternion.z + ), + ) + + +def _cell_point(info, cell_index): + width = int(info.width) + height = int(info.height) + if width <= 0 or height <= 0 or cell_index >= width * height: + return None + + cell_x = cell_index % width + cell_y = cell_index // width + local_x = (cell_x + 0.5) * info.resolution + local_y = (cell_y + 0.5) * info.resolution + yaw = _yaw_from_quaternion(info.origin.orientation) + cosine = cos(yaw) + sine = sin(yaw) + + point = Point() + point.x = info.origin.position.x + cosine * local_x - sine * local_y + point.y = info.origin.position.y + sine * local_x + cosine * local_y + point.z = OBSTACLE_MARKER_Z + return point + + +class SourceContributionMarkerState: + """Convert backend source snapshots into colored marker actions.""" + + def __init__(self): + self.source_colors = {} + + def _source_color(self, source, key): + color = self.source_colors.get(key) + if color is not None: + return color + + robot_color = ROBOT_COLORS.get(source.robot_name) + color = ( + robot_color[:3] + if robot_color is not None + else SOURCE_COLORS[len(self.source_colors) % len(SOURCE_COLORS)] + ) + self.source_colors[key] = color + return color + + def _new_points_marker(self, snapshot, namespace, points, color): + marker = Marker() + marker.header = snapshot.header + marker.ns = namespace + marker.id = 0 + marker.type = Marker.POINTS + marker.action = Marker.ADD + marker.pose.orientation.w = 1.0 + marker.scale.x = max(0.03, snapshot.info.resolution * 0.8) + marker.scale.y = marker.scale.x + marker.points = points + marker.color.r = color[0] + marker.color.g = color[1] + marker.color.b = color[2] + marker.color.a = 0.85 + return marker + + def apply_snapshot(self, snapshot): + """Replace the displayed contributions with the latest snapshot.""" + marker_array = MarkerArray() + reset = Marker() + reset.action = Marker.DELETEALL + marker_array.markers.append(reset) + + if ( + not snapshot.header.frame_id + or snapshot.info.width == 0 + or snapshot.info.height == 0 + or snapshot.info.resolution <= 0.0 + ): + return marker_array + + for contribution in snapshot.sources: + if ( + not contribution.source.source_id + or len(contribution.cell_indices) + != len(contribution.occupancy_values) + ): + continue + + source_id = contribution.source.source_id + key = (source_id, contribution.source.map_name) + namespace = f'{source_id}/backend_contributions' + color = self._source_color(contribution.source, key) + points = [] + for cell_index, occupancy_value in zip( + contribution.cell_indices, + contribution.occupancy_values, + ): + if occupancy_value <= 0: + continue + point = _cell_point(snapshot.info, cell_index) + if point is not None: + points.append(point) + + if points: + marker_array.markers.append( + self._new_points_marker( + snapshot, + namespace, + points, + color, + ) + ) + + return marker_array + + +class SourceContributionVisualizer(Node): + """Visualize map contributions by robot source.""" + + def __init__(self): + super().__init__('source_contribution_visualizer') + input_topic = self.declare_parameter( + 'input_topic', '/map/source_contributions' + ).value + output_topic = self.declare_parameter( + 'output_topic', '/map/source_contribution_markers' + ).value + + transient_qos = QoSProfile( + depth=10, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, + ) + self.state = SourceContributionMarkerState() + self.publisher = self.create_publisher( + MarkerArray, + output_topic, + transient_qos, + ) + self.subscription = self.create_subscription( + MapSourceSnapshot, + input_topic, + self.visualize_snapshot, + transient_qos, + ) + self.get_logger().info(f'Visualizing {input_topic} as {output_topic}') + + def visualize_snapshot(self, snapshot): + """Publish markers for a source snapshot.""" + self.publisher.publish(self.state.apply_snapshot(snapshot)) + + +def main(): + rclpy.init() + node = SourceContributionVisualizer() + 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/nav2_observations.rviz b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz index 607eae0..4f7c30e 100644 --- a/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz +++ b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz @@ -7,6 +7,7 @@ Panels: - /Global Options1 - /Combined Global Map1 - /Region Contributions1 + - /Retained Robot Contributions1 - /Scenario Robots, Goals, and Obstacle1 - /Robot 0 Nav2 Path1 - /Robot 1 Nav2 Path1 @@ -72,6 +73,18 @@ Visualization Manager: Reliability Policy: Reliable Value: /map/region_markers Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Retained Robot Contributions + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/source_contribution_markers + Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true Name: Scenario Robots, Goals, and Obstacle diff --git a/map_server/rmf_layered_map_server_demo/setup.py b/map_server/rmf_layered_map_server_demo/setup.py index 003c2fa..0ba015d 100644 --- a/map_server/rmf_layered_map_server_demo/setup.py +++ b/map_server/rmf_layered_map_server_demo/setup.py @@ -52,6 +52,8 @@ 'rmf_layered_map_server_demo.region_update_visualizer:main', 'scan_region_publisher = ' 'rmf_layered_map_server_demo.scan_region_publisher:main', + 'source_contribution_visualizer = ' + 'rmf_layered_map_server_demo.source_contribution_visualizer:main', ], }, ) diff --git a/map_server/rmf_layered_map_server_demo/test/test_source_contribution_visualizer.py b/map_server/rmf_layered_map_server_demo/test/test_source_contribution_visualizer.py new file mode 100644 index 0000000..6ce214a --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_source_contribution_visualizer.py @@ -0,0 +1,83 @@ +# 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 math import sqrt + +import pytest +from rmf_layered_map_msgs.msg import MapSourceContribution, MapSourceSnapshot +from rmf_layered_map_server_demo.replan_scenarios import ROBOT_COLORS +from rmf_layered_map_server_demo.source_contribution_visualizer import ( + _cell_point, + SourceContributionMarkerState, +) +from visualization_msgs.msg import Marker + + +def _snapshot(): + snapshot = MapSourceSnapshot() + snapshot.header.frame_id = 'map' + snapshot.info.width = 3 + snapshot.info.height = 2 + snapshot.info.resolution = 1.0 + snapshot.info.origin.orientation.w = 1.0 + + contribution = MapSourceContribution() + contribution.source.source_id = 'robot0/scan' + contribution.source.robot_name = 'robot0' + contribution.source.map_name = 'warehouse' + contribution.cell_indices = [1, 5] + contribution.occupancy_values = [100, 0] + snapshot.sources = [contribution] + return snapshot + + +def test_visualizes_backend_obstacles_in_the_observing_robot_color(): + markers = SourceContributionMarkerState().apply_snapshot(_snapshot()).markers + + assert markers[0].action == Marker.DELETEALL + marker = markers[1] + assert marker.action == Marker.ADD + assert marker.type == Marker.POINTS + assert marker.ns == 'robot0/scan/backend_contributions' + assert [(point.x, point.y) for point in marker.points] == [(1.5, 0.5)] + assert ( + marker.color.r, + marker.color.g, + marker.color.b, + ) == pytest.approx(ROBOT_COLORS['robot0'][:3]) + + +def test_empty_snapshot_clears_markers(): + empty_snapshot = _snapshot() + empty_snapshot.sources = [] + + markers = SourceContributionMarkerState().apply_snapshot( + empty_snapshot + ).markers + + assert len(markers) == 1 + assert markers[0].action == Marker.DELETEALL + + +def test_cell_centers_respect_rotated_map_origins(): + snapshot = _snapshot() + snapshot.info.origin.position.x = 2.0 + snapshot.info.origin.position.y = 3.0 + snapshot.info.origin.orientation.z = sqrt(0.5) + snapshot.info.origin.orientation.w = sqrt(0.5) + + point = _cell_point(snapshot.info, 0) + + assert point.x == pytest.approx(1.5) + assert point.y == pytest.approx(3.5) From fe56756ca05e44a3ef495420fe9039aa3776ef8e Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Tue, 28 Jul 2026 21:03:22 +0200 Subject: [PATCH 34/38] fix(map-demo): show only the latest scan regions Delete superseded scan markers immediately instead of waiting for marker TTL. Keep retained obstacle contributions on the separate backend-driven display and hide duplicate raw endpoints. Signed-off-by: SamuelFoo --- .../rmf_layered_map_server_demo/README.md | 2 +- .../launch/nav2_observations.launch.py | 4 ++ .../region_update_visualizer.py | 65 ++++++++++--------- .../replan_obstacle_launch.py | 4 ++ .../rviz/nav2_observations.rviz | 6 +- .../test/test_region_update_visualizer.py | 49 ++++++-------- .../test/test_replan_obstacle_demo.py | 1 - 7 files changed, 65 insertions(+), 66 deletions(-) diff --git a/map_server/rmf_layered_map_server_demo/README.md b/map_server/rmf_layered_map_server_demo/README.md index df7e38c..2ac89f8 100644 --- a/map_server/rmf_layered_map_server_demo/README.md +++ b/map_server/rmf_layered_map_server_demo/README.md @@ -43,7 +43,7 @@ ros2 launch rmf_layered_map_server_demo replan_obstacle.launch.py Two robots start with non-overlapping scan regions and separate goals. Once their initial plans are active, the demo spawns a red bar across both scan regions. The bar meets the right wall to form a dead end and leaves a detour around its left end. -The global RViz view shows robot-colored poses, goals, region contributions, and Nav2 paths alongside the green RMF plans. +The global RViz view shows the composed map, robot poses and goals, and Nav2 paths alongside the green RMF plans. RViz draws active per-robot obstacle cells from `/map/source_contributions` and separately shows each robot's latest free-space scan sectors. Use the warehouse scenario to reproduce the original two-aisle case: diff --git a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py index 1394ad6..301e880 100644 --- a/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py +++ b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py @@ -151,6 +151,10 @@ def generate_launch_description(): package='rmf_layered_map_server_demo', executable='region_update_visualizer', output='screen', + parameters=[{ + 'output_topic': '/map/scan_region_markers', + 'show_obstacles': False, + }], ) source_contribution_visualizer = Node( condition=IfCondition(use_global_rviz), diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py index c0f5575..221aae1 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py @@ -162,12 +162,16 @@ def _markers_from_patch(update, patch, first_id, color, default_ttl_sec): class RegionMarkerState: - """Convert region updates into persistent per-source RViz marker actions.""" + """Convert region updates into per-source RViz marker actions.""" - def __init__(self, default_ttl_sec=30.0): + def __init__( + self, + default_ttl_sec=30.0, + show_obstacles=True, + ): self.default_ttl_sec = default_ttl_sec + self.show_obstacles = show_obstacles self.markers_by_source = {} - self.next_ids = {} self.latest_stamps = {} self.source_colors = {} @@ -195,24 +199,17 @@ def apply_update(self, update): ) self.source_colors[key] = color marker_array = MarkerArray() - self.markers_by_source[key] = [ - marker - for marker in self.markers_by_source.get(key, ()) - if marker[3] is None or marker[3] > stamp_nsec - ] - if update.reset_source: - for namespace, marker_id, frame_id, _ in self.markers_by_source[key]: - marker = Marker() - marker.header.frame_id = frame_id - marker.ns = namespace - marker.id = marker_id - marker.action = Marker.DELETE - marker_array.markers.append(marker) - self.markers_by_source[key] = [] - self.next_ids[key] = 0 - - next_id = self.next_ids.get(key, 0) + # Marker TTL cannot hide a superseded scan immediately. + for namespace, marker_id, frame_id in self.markers_by_source.get(key, ()): + marker = Marker() + marker.header.frame_id = frame_id + marker.ns = namespace + marker.id = marker_id + marker.action = Marker.DELETE + marker_array.markers.append(marker) + + next_id = 0 new_markers = [] for patch in update.patches: if patch.update_type not in ( @@ -220,6 +217,11 @@ def apply_update(self, update): MapRegionPatch.UPDATE_OBSTACLE, ): continue + if ( + patch.update_type == MapRegionPatch.UPDATE_OBSTACLE + and not self.show_obstacles + ): + continue patch_markers = _markers_from_patch( update, patch, @@ -231,28 +233,21 @@ def apply_update(self, update): next_id += len(patch_markers) marker_array.markers.extend(new_markers) - self.next_ids[key] = next_id - self.markers_by_source[key].extend( + self.markers_by_source[key] = [ ( marker.ns, marker.id, marker.header.frame_id, - stamp_nsec + marker.lifetime.sec * 1_000_000_000 - + marker.lifetime.nanosec - if marker.lifetime.sec > 0 or marker.lifetime.nanosec > 0 - else None, ) for marker in new_markers - ) - - if new_markers or update.reset_source: - self.latest_stamps[key] = stamp_nsec + ] + self.latest_stamps[key] = stamp_nsec return marker_array class RegionUpdateVisualizer(Node): - """Visualize active map-region contributions as colored RViz markers.""" + """Visualize map-region updates as colored RViz markers.""" def __init__(self): super().__init__('region_update_visualizer') @@ -265,12 +260,18 @@ def __init__(self): default_ttl_sec = self.declare_parameter( 'default_ttl_sec', 30.0 ).value + show_obstacles = self.declare_parameter( + 'show_obstacles', True + ).value reliable_qos = QoSProfile( depth=10, reliability=ReliabilityPolicy.RELIABLE, ) - self.state = RegionMarkerState(default_ttl_sec) + self.state = RegionMarkerState( + default_ttl_sec, + show_obstacles, + ) self.publisher = self.create_publisher( MarkerArray, output_topic, diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py index e80e2b4..2204445 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py @@ -172,6 +172,10 @@ def generate_replan_launch_description(scenario_name): package='rmf_layered_map_server_demo', executable='region_update_visualizer', output='screen', + parameters=[{ + 'output_topic': '/map/scan_region_markers', + 'show_obstacles': False, + }], ) source_contribution_visualizer = Node( condition=IfCondition(use_global_rviz), diff --git a/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz index 4f7c30e..247b243 100644 --- a/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz +++ b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz @@ -6,8 +6,8 @@ Panels: Expanded: - /Global Options1 - /Combined Global Map1 - - /Region Contributions1 - /Retained Robot Contributions1 + - /Latest Robot Scan Regions1 - /Scenario Robots, Goals, and Obstacle1 - /Robot 0 Nav2 Path1 - /Robot 1 Nav2 Path1 @@ -63,7 +63,7 @@ Visualization Manager: Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true - Name: Region Contributions + Name: Latest Robot Scan Regions Namespaces: {} Topic: @@ -71,7 +71,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /map/region_markers + Value: /map/scan_region_markers Value: true - Class: rviz_default_plugins/MarkerArray Enabled: true diff --git a/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py b/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py index fe8df1f..7339870 100644 --- a/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py +++ b/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py @@ -23,7 +23,7 @@ from visualization_msgs.msg import Marker -def _update(stamp_sec=1, reset_source=True): +def _update(stamp_sec=1): update = MapRegionUpdate() update.source.header.stamp.sec = stamp_sec update.source.header.frame_id = 'map' @@ -34,7 +34,6 @@ def _update(stamp_sec=1, reset_source=True): update.source.robot_pose.position.y = 3.0 update.source.robot_pose.orientation.w = 1.0 update.source.default_ttl_sec = 4.0 - update.reset_source = reset_source return update @@ -115,7 +114,7 @@ def test_visualizes_clear_regions_with_a_lighter_source_color(): assert clear_marker.points[0].z < obstacle_marker.points[0].z -def test_reset_deletes_the_previous_source_markers_before_replacing_them(): +def test_new_update_replaces_previous_markers(): state = RegionMarkerState() first = _update(stamp_sec=1) first_patch = MapRegionPatch() @@ -133,40 +132,32 @@ def test_reset_deletes_the_previous_source_markers_before_replacing_them(): markers = state.apply_update(replacement).markers assert [marker.action for marker in markers] == [Marker.DELETE, Marker.ADD] - assert markers[0].ns == markers[1].ns == 'robot0/scan' assert markers[0].id == markers[1].id == 0 assert [(point.x, point.y) for point in markers[1].points] == [(3.0, 4.0)] -def test_older_update_does_not_replace_newer_visualization(): - state = RegionMarkerState() - state.apply_update(_update(stamp_sec=2)) +def test_scan_region_mode_can_hide_raw_obstacles(): + state = RegionMarkerState(show_obstacles=False) + update = _update(stamp_sec=1) + clear_patch = MapRegionPatch() + clear_patch.update_type = MapRegionPatch.UPDATE_CLEAR + clear_patch.regions = [ + _region(Region.HINT_CONVEX_POLYGON, [0.0, 0.0, 1.0, -0.1, 1.0, 0.1]), + ] + obstacle_patch = MapRegionPatch() + obstacle_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE + obstacle_patch.regions = [_region(Region.HINT_POINT, [1.0, 0.0])] + update.patches = [clear_patch, obstacle_patch] - markers = state.apply_update(_update(stamp_sec=1)).markers + markers = state.apply_update(update).markers - assert markers == [] + assert [marker.type for marker in markers] == [Marker.LINE_LIST] -def test_non_reset_updates_retain_only_unexpired_marker_bookkeeping(): +def test_older_update_does_not_replace_newer_visualization(): state = RegionMarkerState() - first = _update(stamp_sec=1, reset_source=False) - first_patch = MapRegionPatch() - first_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE - first_patch.regions = [_region(Region.HINT_POINT, [1.0, 2.0])] - first.patches = [first_patch] - state.apply_update(first) - - second = _update(stamp_sec=2, reset_source=False) - second_patch = MapRegionPatch() - second_patch.update_type = MapRegionPatch.UPDATE_OBSTACLE - second_patch.regions = [_region(Region.HINT_POINT, [3.0, 4.0])] - second.patches = [second_patch] - markers = state.apply_update(second).markers - - assert [marker.action for marker in markers] == [Marker.ADD] - assert markers[0].id == 1 - assert len(state.markers_by_source[('robot0/scan', 'warehouse')]) == 2 + state.apply_update(_update(stamp_sec=2)) - state.apply_update(_update(stamp_sec=6, reset_source=False)) + markers = state.apply_update(_update(stamp_sec=1)).markers - assert state.markers_by_source[('robot0/scan', 'warehouse')] == [] + assert markers == [] diff --git a/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py b/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py index 3456ce9..3c13fe7 100644 --- a/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py +++ b/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py @@ -90,7 +90,6 @@ def _region_update(robot_name): update.source.robot_name = robot_name update.source.map_name = 'single_room' update.source.robot_pose.orientation.w = 1.0 - update.reset_source = True patch = MapRegionPatch() patch.update_type = MapRegionPatch.UPDATE_OBSTACLE region = Region() From 6c5e85ee574363cb22fa1bd761d9c524147bbd39 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Tue, 28 Jul 2026 22:56:54 +0200 Subject: [PATCH 35/38] fix(map): omit clear cells from source snapshots Publish only active obstacle cells in per-source snapshots. Clear regions already remove stale obstacles from backend state, so omitting clear cells keeps visualization authoritative without carrying unused free-space history. Signed-off-by: SamuelFoo --- map_server/rmf_layered_map_server/README.md | 4 +- map_server/rmf_layered_map_server/src/lib.rs | 38 +++++++------------ .../msg/MapSourceContribution.msg | 2 +- .../msg/MapSourceSnapshot.msg | 4 +- 4 files changed, 19 insertions(+), 29 deletions(-) diff --git a/map_server/rmf_layered_map_server/README.md b/map_server/rmf_layered_map_server/README.md index 039b7ee..5c77654 100644 --- a/map_server/rmf_layered_map_server/README.md +++ b/map_server/rmf_layered_map_server/README.md @@ -9,11 +9,11 @@ Default topics: - `/map/static`: static `nav_msgs/OccupancyGrid` - `/map/region_updates`: `rmf_layered_map_msgs/MapRegionUpdate` - `/map`: composed `nav_msgs/OccupancyGrid` -- `/map/source_contributions`: active cells grouped by source +- `/map/source_contributions`: active obstacle cells grouped by source Dynamic observations expire according to their TTL, so transient obstacles do not remain in the global planning map forever. The server transforms and rasterizes each region on arrival, then stores cell contributions by source, map, update type, and cell. Repeated observations refresh matching cells instead of stacking region snapshots. Older evidence is retained only when it outlives a newer contribution. -`/map/source_contributions` publishes the active rasterized cells for each source whenever the composed map changes. +`/map/source_contributions` publishes the active rasterized obstacle cells for each source whenever the composed map changes. `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. Clear cells remove older obstacles from the same source, while current obstacle endpoints and obstacles reported by other sources remain occupied. Observations outside the clear regions remain active until their TTL expires. If the snapshot replaces the source's entire previous observation state, set `reset_source` so the server removes all 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. diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs index debc81f..f4f31db 100644 --- a/map_server/rmf_layered_map_server/src/lib.rs +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -274,22 +274,18 @@ impl LayeredMap { let mut sources = Vec::new(); for (source_key, source_cells) in source_entries { - let mut final_cells = HashMap::new(); - for update_type in [DynamicUpdateType::Clear, DynamicUpdateType::Obstacle] { - for (cell_key, history) in source_cells { - if cell_key.update_type != update_type { - continue; - } - if let Some(cell) = history.last() { - final_cells.insert(cell_key.cell_index, cell.occupancy_value); + // Clear cells can be omitted because each snapshot replaces the previous one. + let mut cells = source_cells + .iter() + .filter_map(|(cell_key, history)| { + if cell_key.update_type != DynamicUpdateType::Obstacle { + return None; } - } - } - if final_cells.is_empty() { - continue; - } - - let mut cells = final_cells.into_iter().collect::>(); + history + .last() + .map(|cell| (cell_key.cell_index, cell.occupancy_value)) + }) + .collect::>(); cells.sort_by_key(|(cell_index, _)| *cell_index); let mut contribution = MapSourceContribution { source: self @@ -1315,7 +1311,7 @@ mod tests { } #[test] - fn source_snapshot_reports_authoritative_rasterized_contributions() { + fn source_snapshot_reports_active_obstacle_contributions() { let mut map = LayeredMap::default(); map.set_static_map(static_grid(5, 5, 0)); @@ -1352,14 +1348,8 @@ mod tests { .iter() .find(|source| source.source.source_id == "robot_1/local_costmap") .unwrap(); - let robot_1_cells = robot_1 - .cell_indices - .iter() - .copied() - .zip(robot_1.occupancy_values.iter().copied()) - .collect::>(); - assert_eq!(robot_1_cells.get(&6), Some(&0)); - assert_eq!(robot_1_cells.get(&18), Some(&100)); + assert_eq!(robot_1.cell_indices, vec![18]); + assert_eq!(robot_1.occupancy_values, vec![100]); let robot_2 = snapshot .sources diff --git a/rmf_layered_map_msgs/msg/MapSourceContribution.msg b/rmf_layered_map_msgs/msg/MapSourceContribution.msg index f618773..d8fbcb1 100644 --- a/rmf_layered_map_msgs/msg/MapSourceContribution.msg +++ b/rmf_layered_map_msgs/msg/MapSourceContribution.msg @@ -1,6 +1,6 @@ # MapSourceContribution.msg -# Source that owns these rasterized cell contributions. +# Source that owns these rasterized obstacle cells. MapObservationSource source # Row-major indices into MapSourceSnapshot.info. Each index has a matching diff --git a/rmf_layered_map_msgs/msg/MapSourceSnapshot.msg b/rmf_layered_map_msgs/msg/MapSourceSnapshot.msg index 334c2dc..2f980fd 100644 --- a/rmf_layered_map_msgs/msg/MapSourceSnapshot.msg +++ b/rmf_layered_map_msgs/msg/MapSourceSnapshot.msg @@ -1,10 +1,10 @@ # MapSourceSnapshot.msg -# Global frame for the rasterized source contributions. +# Global frame for the rasterized obstacle contributions. std_msgs/Header header # Geometry shared by every source contribution in this snapshot. nav_msgs/MapMetaData info -# Complete active contribution state, grouped by observation source. +# Complete active obstacle state, grouped by observation source. MapSourceContribution[] sources From 9ead92c0adaeed9c3abe0cebe830dede71148e47 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Wed, 29 Jul 2026 05:40:56 +0200 Subject: [PATCH 36/38] fix(nav2-traffic): wait for a localized pose Do not publish default odometry before the first AMCL update. Once localized, continue republishing the latest pose so downstream freshness checks retain their existing behavior. Signed-off-by: SamuelFoo --- nav2_integration/rmf_nav2_traffic/src/agent.rs | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/nav2_integration/rmf_nav2_traffic/src/agent.rs b/nav2_integration/rmf_nav2_traffic/src/agent.rs index 72b9993..41e1e62 100644 --- a/nav2_integration/rmf_nav2_traffic/src/agent.rs +++ b/nav2_integration/rmf_nav2_traffic/src/agent.rs @@ -114,10 +114,21 @@ fn create_amcl_pose_subscriber( )); } -fn update_amcl_pose(mut agents: Query<(&mut AmclPose, &AmclPoseSubscription, &OdomPublisher)>) { - for (mut amcl_pose, amcl_pose_sub, odom_pub) in agents.iter_mut() { +fn update_amcl_pose( + mut agents: Query<( + &mut Nav2Agent, + &mut AmclPose, + &AmclPoseSubscription, + &OdomPublisher, + )>, +) { + for (mut agent, mut amcl_pose, amcl_pose_sub, odom_pub) in agents.iter_mut() { if let Some(amcl_pose_msg) = amcl_pose_sub.subscriber.data_callback() { amcl_pose.0 = amcl_pose_msg.clone(); + agent.localized = true; + } + if !agent.localized { + continue; } let mut odom = Odometry::default(); From 2fd48081ea0f1f7ea1617b9ed73be5b7a253f6ed Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Wed, 29 Jul 2026 05:45:27 +0200 Subject: [PATCH 37/38] refactor(nav2): start staged robots independently Start each robot stack without waiting for another robot's coordinator to exit. Each stack still relies on its own coordinator to activate Nav2 after its transforms are ready. Signed-off-by: SamuelFoo --- .../cloned_multi_tb3_simulation_launch.py | 21 +++---------------- 1 file changed, 3 insertions(+), 18 deletions(-) diff --git a/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py b/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py index f655f04..65a7598 100644 --- a/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py +++ b/nav2_integration/sp_demo_nav2_bringup/launch/cloned_multi_tb3_simulation_launch.py @@ -31,7 +31,7 @@ RegisterEventHandler, ) from launch.conditions import IfCondition, UnlessCondition -from launch.event_handlers import OnProcessExit, OnShutdown +from launch.event_handlers import OnShutdown from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, TextSubstitution from launch_ros.actions import Node @@ -141,7 +141,6 @@ def generate_launch_description(): # Define commands for launching the navigation instances bringup_cmd_group = [] staged_groups = [] - staged_init_nodes = [] for robot_index, robot_name in enumerate(robots_list): init_pose = robots_list[robot_name] namespace = robot_name + '/inner' @@ -220,7 +219,6 @@ def make_group(robot_autostart, condition, ready_node=None): output='screen', remappings=[('/tf', 'tf'), ('/tf_static', 'tf_static')], ) - staged_init_nodes.append(staged_init) staged_groups.append( make_group('False', IfCondition(staged_startup), staged_init) ) @@ -291,20 +289,7 @@ def make_group(robot_autostart, condition, ready_node=None): for cmd in bringup_cmd_group: ld.add_action(cmd) - for staged_init, next_group in zip( - staged_init_nodes, - staged_groups[1:], - ): - ld.add_action( - RegisterEventHandler( - condition=IfCondition(staged_startup), - event_handler=OnProcessExit( - target_action=staged_init, - on_exit=[next_group], - ), - ) - ) - if staged_groups: - ld.add_action(staged_groups[0]) + for group in staged_groups: + ld.add_action(group) return ld From a62ff42836aa229c30e991e5a4ba64dfc6cee558 Mon Sep 17 00:00:00 2001 From: SamuelFoo Date: Wed, 29 Jul 2026 05:52:00 +0200 Subject: [PATCH 38/38] fix(map-demo): extend warehouse observation TTL Keep warehouse observations for 210 seconds so they remain available throughout the longer scenario. Preserve the 60-second simple-demo default and the ttl_sec launch override. Signed-off-by: SamuelFoo --- map_server/rmf_layered_map_server_demo/README.md | 2 +- .../rmf_layered_map_server_demo/replan_obstacle_launch.py | 2 +- .../rmf_layered_map_server_demo/replan_scenarios.py | 3 +++ .../test/test_replan_obstacle_demo.py | 1 + 4 files changed, 6 insertions(+), 2 deletions(-) diff --git a/map_server/rmf_layered_map_server_demo/README.md b/map_server/rmf_layered_map_server_demo/README.md index 2ac89f8..e0d914d 100644 --- a/map_server/rmf_layered_map_server_demo/README.md +++ b/map_server/rmf_layered_map_server_demo/README.md @@ -71,5 +71,5 @@ Useful launch arguments: * `use_global_rviz:=False` runs without the combined RViz view. * `spawn_delay_sec:=1.0` controls the delay after initial plans are received. * `scenario_timeout_sec:=180.0` controls how long the demo waits for replanning. -* `ttl_sec:=60.0` controls how long observations outside the current scan remain in the layered map. New clear rays remove stale same-source obstacles before the TTL expires. +* `ttl_sec` controls how long observations outside the current scan remain in the layered map. It defaults to 60 seconds for the simple scenario and 210 seconds for the warehouse scenario. New clear rays remove stale same-source obstacles before the TTL expires. * `self_filter_radius:=0.22` excludes scan returns inside the robot body. diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py index 2204445..b8f2576 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py @@ -134,7 +134,7 @@ def generate_replan_launch_description(scenario_name): ), DeclareLaunchArgument( 'ttl_sec', - default_value='60.0', + default_value=str(scenario_config.ttl_sec), description='Lifetime of observations that are not re-observed.', ), DeclareLaunchArgument( diff --git a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_scenarios.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_scenarios.py index 276f68b..46d536a 100644 --- a/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_scenarios.py +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_scenarios.py @@ -40,6 +40,7 @@ class ReplanScenario: world_name: str robots: tuple[RobotLayout, ...] scan_radius: float + ttl_sec: float bar_name: str bar_center: tuple[float, float] bar_size: tuple[float, float, float] @@ -54,6 +55,7 @@ class ReplanScenario: RobotLayout('robot1', (5.5, -4.5, pi / 2.0), (6.0, 4.0)), ), scan_radius=4.0, + ttl_sec=60.0, bar_name='simple_replan_bar', bar_center=(1.75, -1.5), bar_size=(15.5, 0.5, 1.0), @@ -68,6 +70,7 @@ class ReplanScenario: RobotLayout('robot1', (3.5, -21.0, pi / 2.0), (3.5, -15.0)), ), scan_radius=3.5, + ttl_sec=210.0, bar_name='warehouse_replan_bar', bar_center=(-2.5, -18.5), bar_size=(15.0, 0.5, 1.0), diff --git a/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py b/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py index 3c13fe7..85fddb5 100644 --- a/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py +++ b/map_server/rmf_layered_map_server_demo/test/test_replan_obstacle_demo.py @@ -192,3 +192,4 @@ def test_warehouse_scenario_layout(): assert WAREHOUSE_SCENARIO.robots[1].pose[:2] == (3.5, -21.0) assert WAREHOUSE_SCENARIO.bar_center == (-2.5, -18.5) assert WAREHOUSE_SCENARIO.bar_size == (15.0, 0.5, 1.0) + assert WAREHOUSE_SCENARIO.ttl_sec == 210.0