diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md new file mode 100644 index 0000000..2985994 --- /dev/null +++ b/discourse/3-layered-global-occupancy-map.md @@ -0,0 +1,139 @@ +[Draft for Discourse] + +# Layered Global Map Observations + +This post describes the observation interface 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 +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 + +* 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 + +# Observation Topic + +Observation sources publish dynamic map observations on: + +* `/map/region_updates` - [`MapRegionUpdate.msg`](../rmf_layered_map_msgs/msg/MapRegionUpdate.msg) + +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. + +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. + +Important fields: + +* `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`: 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` + +[`MapRegionUpdate.msg`](../rmf_layered_map_msgs/msg/MapRegionUpdate.msg) +describes one observation snapshot from a source. + +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`: 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 +does not have a TTL because it is a bookkeeping operation, not an active map +observation. + +## `MapRegionPatch` + +[`MapRegionPatch.msg`](../rmf_layered_map_msgs/msg/MapRegionPatch.msg) +describes one group of regions with the same action and TTL. + +Patch types: + +* `UPDATE_CLEAR`: regions are temporary free space +* `UPDATE_OBSTACLE`: regions are temporary occupied space + +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. + +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. + +The first prototype uses `rmf_prototype_msgs/Region` for sparse 2D geometry so +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 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. +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 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 first server 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 +* 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 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/.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/README.md b/map_server/rmf_layered_map_server/README.md new file mode 100644 index 0000000..3b9c7bc --- /dev/null +++ b/map_server/rmf_layered_map_server/README.md @@ -0,0 +1,45 @@ +# 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. + +`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. + +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 + +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/package.xml b/map_server/rmf_layered_map_server/package.xml new file mode 100644 index 0000000..0f54ca1 --- /dev/null +++ b/map_server/rmf_layered_map_server/package.xml @@ -0,0 +1,22 @@ + + + + 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 + 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..f5529ce --- /dev/null +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -0,0 +1,1140 @@ +// 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::{ + geometry_msgs::msg::Pose, + nav_msgs::msg::OccupancyGrid, + rmf_layered_map_msgs::msg::{MapRegionPatch, MapRegionUpdate}, + rmf_prototype_msgs::msg::Region, +}; +use std::{collections::HashMap, 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, + frame_id: String, + update_type: DynamicUpdateType, + occupancy_value: i8, + regions: Vec, + 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, +} + +impl LayeredMap { + pub fn new(default_ttl: Duration) -> Self { + Self { + static_grid: None, + dynamic_observations: Vec::new(), + latest_source_stamps: HashMap::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 source_validation_error(&update, self.static_frame_id()).is_some() { + 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 = i128::from(update.source.header.stamp.sec) * NANOS_PER_SECOND + + i128::from(update.source.header.stamp.nanosec); + + if self + .latest_source_stamps + .get(&key) + .is_some_and(|latest_stamp| stamp_nsec < *latest_stamp) + { + 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 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, + 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, + }; + + 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); + 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(), + frame_id: frame_id.clone(), + update_type, + occupancy_value, + regions, + expires_at_nsec, + }); + changed = true; + } + + if changed || reset_source { + self.latest_source_stamps.insert(key, stamp_nsec); + } + + if changed { + self.revision = self.revision.wrapping_add(1); + } + changed + } + + 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); + 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); + } + + 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 { + 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) { + 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 {}", + 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: rclrs::WorkerTimer, +} + +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, + }) +} + +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 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, + 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 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()); + } + + if region.points.len() % 2 != 0 { + return Some("region points must contain complete x/y pairs".to_string()); + } + + 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 + )), + } +} + +fn is_rasterizable_region_hint(hint: u8) -> bool { + matches!( + hint, + 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(); + 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 !is_rasterizable_region_hint(region.hint) { + 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 duration_to_nsec(duration: Duration) -> i128 { + i128::from(duration.as_secs()) * NANOS_PER_SECOND + i128::from(duration.subsec_nanos()) +} + +fn positive_seconds_to_nsec(seconds: f64) -> Option { + if !seconds.is_finite() || seconds <= 0.0 { + return None; + } + + Some((seconds * NANOS_PER_SECOND as f64).round() as i128) +} + +#[cfg(test)] +mod tests { + use super::*; + use ros_env::{ + geometry_msgs::msg::{Point, Pose, Quaternion}, + nav_msgs::msg::MapMetaData, + 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 { + OccupancyGrid { + header: Header { + frame_id: "map".to_string(), + ..Default::default() + }, + 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 { + let mut source = 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(), + default_ttl_sec: 10.0, + ..Default::default() + }; + source.header.stamp.sec = 1; + source.robot_pose.orientation.w = 1.0; + source + } + + 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 patch(update_type: u8, regions: Vec) -> MapRegionPatch { + MapRegionPatch { + update_type, + regions, + ..Default::default() + } + } + + fn update(update_type: u8, regions: Vec) -> MapRegionUpdate { + MapRegionUpdate { + source: source(), + patches: vec![patch(update_type, regions)], + ..Default::default() + } + } + + fn update_from(source_id: &str, update_type: u8, regions: Vec) -> MapRegionUpdate { + MapRegionUpdate { + source: source_with_id(source_id), + patches: vec![patch(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( + MapRegionPatch::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(MapRegionPatch::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 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(); + map.set_static_map(static_grid(3, 3, 0)); + + assert!(map.ingest_region_update( + update( + MapRegionPatch::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( + MapRegionPatch::UPDATE_OBSTACLE, + vec![Region { + hint: Region::HINT_POINT, + points: vec![1.0, 1.0, 2.0, 2.0], + }] + ), + 0, + )); + + let composed = map.compose().unwrap(); + assert!(composed.data.iter().all(|cell| *cell == 0)); + } + + #[test] + 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( + update( + MapRegionPatch::UPDATE_OBSTACLE, + vec![Region { + hint: Region::HINT_POLYGON, + points: vec![0.0, 0.0, 2.0, 0.0, 1.0, 2.0], + }] + ), + 0, + )); + assert_eq!(map.dynamic_observation_count(), 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 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(); + map.set_static_map(static_grid(3, 3, 0)); + + assert!(map.ingest_region_update( + update( + MapRegionPatch::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( + 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, + )); + + 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 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(); + map.set_static_map(static_grid(3, 3, 0)); + + assert!(map.ingest_region_update( + update( + MapRegionPatch::UPDATE_OBSTACLE, + vec![rectangle(1.0, 1.0, 2.0, 2.0)] + ), + 0, + )); + + assert!(map.ingest_region_update( + MapRegionUpdate { + source: source(), + reset_source: true, + ..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", + MapRegionPatch::UPDATE_OBSTACLE, + vec![rectangle(1.0, 1.0, 2.0, 2.0)] + ), + 0, + )); + assert!(map.ingest_region_update( + update_from( + "robot_2/local_costmap", + MapRegionPatch::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"), + reset_source: true, + ..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(()) +} 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..3fdf855 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/demo_publisher.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 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, + MapRegionPatch, + 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()) + 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') + + +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.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' + 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] + patch.regions.append(region) + msg.patches.append(patch) + 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', + ], + }, +) 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..235f0ac --- /dev/null +++ b/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py @@ -0,0 +1,215 @@ +# 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, + MapRegionPatch, + 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) + update.source.header.stamp = self.node.get_clock().now().to_msg() + 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() + reset.source.header.stamp = self.node.get_clock().now().to_msg() + 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() + update.source.header.stamp = self.node.get_clock().now().to_msg() + 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.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' + 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 = 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] + patch.regions.append(region) + msg.patches.append(patch) + return msg + + +def reset_update(): + msg = MapRegionUpdate() + msg.source = map_source() + msg.reset_source = True + 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 diff --git a/rmf_layered_map_msgs/CMakeLists.txt b/rmf_layered_map_msgs/CMakeLists.txt new file mode 100644 index 0000000..3bdd01f --- /dev/null +++ b/rmf_layered_map_msgs/CMakeLists.txt @@ -0,0 +1,34 @@ +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(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 geometry_msgs rmf_prototype_msgs std_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..a35cd00 --- /dev/null +++ b/rmf_layered_map_msgs/msg/MapObservationSource.msg @@ -0,0 +1,24 @@ +# MapObservationSource.msg + +# Global 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 +# 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 + +# 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 new file mode 100644 index 0000000..14dc00b --- /dev/null +++ b/rmf_layered_map_msgs/msg/MapRegionPatch.msg @@ -0,0 +1,29 @@ +# 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 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 diff --git a/rmf_layered_map_msgs/msg/MapRegionUpdate.msg b/rmf_layered_map_msgs/msg/MapRegionUpdate.msg new file mode 100644 index 0000000..e067666 --- /dev/null +++ b/rmf_layered_map_msgs/msg/MapRegionUpdate.msg @@ -0,0 +1,15 @@ +# MapRegionUpdate.msg + +# One observation snapshot from a source. +MapObservationSource source + +# 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 + +# 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 new file mode 100644 index 0000000..2d8c458 --- /dev/null +++ b/rmf_layered_map_msgs/package.xml @@ -0,0 +1,29 @@ + + + + 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 + + geometry_msgs + rmf_prototype_msgs + std_msgs + + geometry_msgs + rmf_prototype_msgs + std_msgs + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + +