diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md new file mode 100644 index 0000000..529f6e4 --- /dev/null +++ b/discourse/3-layered-global-occupancy-map.md @@ -0,0 +1,172 @@ +[Draft for Discourse] + +# Layered Global Map Observations + +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 +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 include the observation-frame pose at observation time +* 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 + `rmf_prototype_msgs` unchanged + +# Observation Topics + +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 +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. + +# 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 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. 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. 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. + +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: + +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 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 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 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 +* 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 +* retaining markers until TTL expiry and replacing them on source reset diff --git a/map_server/README.md b/map_server/README.md new file mode 100644 index 0000000..15978b6 --- /dev/null +++ b/map_server/README.md @@ -0,0 +1,15 @@ +# 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. + +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/.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..5c77654 --- /dev/null +++ b/map_server/rmf_layered_map_server/README.md @@ -0,0 +1,61 @@ +# 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` +- `/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 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. + +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. + +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: + +```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/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 new file mode 100644 index 0000000..f4f31db --- /dev/null +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -0,0 +1,1596 @@ +// 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::{ + MapObservationSource, MapRegionPatch, MapRegionUpdate, MapSourceContribution, + MapSourceSnapshot, + }, + rmf_prototype_msgs::msg::Region, +}; +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, Hash, PartialEq, Eq)] +enum DynamicUpdateType { + Obstacle, + Clear, +} + +#[derive(Clone, Debug, Hash, PartialEq, Eq)] +struct DynamicCellKey { + update_type: DynamicUpdateType, + cell_index: usize, +} + +#[derive(Clone, Debug)] +struct DynamicCell { + occupancy_value: i8, + expires_at_nsec: i128, + sequence: u64, +} + +#[derive(Clone, Debug, Hash, PartialEq, Eq)] +struct ObservationSourceKey { + source_id: String, + map_name: String, +} + +#[derive(Clone, Debug)] +pub struct LayeredMap { + static_grid: Option, + dynamic_cells: HashMap>>, + source_metadata: HashMap, + latest_source_stamps: HashMap, + default_ttl_nsec: i128, + next_sequence: u64, + revision: u64, +} + +impl LayeredMap { + pub fn new(default_ttl: Duration) -> Self { + 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, + revision: 0, + } + } + + pub fn revision(&self) -> u64 { + self.revision + } + + pub fn dynamic_observation_count(&self) -> usize { + 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); + } + + 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 Some(static_grid) = self.static_grid.as_ref() else { + return false; + }; + + 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(), + }; + 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 { + changed |= self.dynamic_cells.remove(&key).is_some(); + } + + let default_ttl_sec = update.source.default_ttl_sec; + 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 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), + }; + + 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 { + 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, + 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; + } + + if changed || reset_source { + self.source_metadata.insert(key.clone(), source_metadata); + 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 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); + } + changed + } + + pub fn compose(&self) -> Option { + let mut composed = self.static_grid.clone()?; + normalize_grid_data(&mut composed); + + 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) + } + + 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 { + // 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; + } + 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 + .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() + .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 source_contributions_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(), + source_contributions_topic: "/map/source_contributions".to_string(), + default_ttl: Duration::from_secs(30), + publish_period: Duration::from_millis(250), + } + } +} + +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, + source_contributions_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))?; + 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, + }) + } + + 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 {}", + self.map.revision() + ); + self.publish_if_changed(); + } + + 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 rasterized cells {}", + 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 rasterized cells {}", + 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; + }; + 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(), + "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 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() +} + +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_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, axis-aligned rectangle, or convex polygon", + 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 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 { + 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 convex_polygon(points: Vec) -> Region { + Region { + hint: Region::HINT_CONVEX_POLYGON, + points, + } + } + + 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 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(); + 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, + )); + assert_eq!(map.dynamic_observation_count(), 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 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 source_snapshot_reports_active_obstacle_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(); + assert_eq!(robot_1.cell_indices, vec![18]); + assert_eq!(robot_1.occupancy_values, vec![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(); + 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); + } + + #[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}") + ); + } +} 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/README.md b/map_server/rmf_layered_map_server_demo/README.md new file mode 100644 index 0000000..e0d914d --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/README.md @@ -0,0 +1,75 @@ +# Layered Map Server Demos + +This package contains three 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 +``` + +Use `use_rviz:=False` for a headless run. + +## Three-Robot Nav2 Observation Demo + +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 +``` + +![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. +* Set `move_robots:=False` to disable robot movement. +* Set `spawn_clutter:=False` to use the unmodified warehouse world. +* 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 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: + +```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. +* `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/docs/images/nav2_observations.png b/map_server/rmf_layered_map_server_demo/docs/images/nav2_observations.png new file mode 100644 index 0000000..0d5aad7 Binary files /dev/null and b/map_server/rmf_layered_map_server_demo/docs/images/nav2_observations.png differ 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/launch/nav2_observations.launch.py b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py new file mode 100644 index 0000000..301e880 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/launch/nav2_observations.launch.py @@ -0,0 +1,253 @@ +# 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), +) + +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.""" + 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') + move_robots = LaunchConfiguration('move_robots') + 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( + 'move_robots', + default_value='True', + description='Whether to cycle the robots through example Nav2 goals.', + ), + DeclareLaunchArgument( + 'spawn_clutter', + default_value='True', + description='Whether to spawn one deterministic obstacle near each robot.', + ), + DeclareLaunchArgument( + 'beam_stride', + default_value='1', + description='Convert every Nth laser beam into map regions.', + ), + DeclareLaunchArgument( + 'publish_period_sec', + default_value='0.5', + description='Minimum period between region snapshots from each robot.', + ), + DeclareLaunchArgument( + 'ttl_sec', + default_value='10.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:=', move_robots], + ], + 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', + parameters=[{ + 'output_topic': '/map/scan_region_markers', + 'show_obstacles': False, + }], + ) + 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 = [] + 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')], + ) + ) + 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, + 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': 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, + source_contribution_visualizer, + *observation_nodes, + *goal_nodes, + clutter_spawner, + global_rviz, + global_rviz_exit_handler, + ]) 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 0000000..06068fb Binary files /dev/null and b/map_server/rmf_layered_map_server_demo/maps/single_room.png differ 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 new file mode 100644 index 0000000..90d38f7 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/package.xml @@ -0,0 +1,44 @@ + + + + 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 + + action_msgs + 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 + sp_demo_nav2_bringup + tf2_ros_py + visualization_msgs + + ament_copyright + ament_pep257 + python3-pytest + + + 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_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/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/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/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..221aae1 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/region_update_visualizer.py @@ -0,0 +1,306 @@ +# 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 + +from .replan_scenarios import ROBOT_COLORS + + +SOURCE_COLORS = ( + ROBOT_COLORS['robot0'][:3], + ROBOT_COLORS['robot1'][:3], + (0.18, 0.80, 0.44), + (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 +OBSTACLE_MARKER_Z = 0.08 + + +def _point(x, y, z): + point = Point() + point.x = float(x) + point.y = float(y) + point.z = z + 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 _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 + 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 = [] + 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], z)) + 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, 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)])) + elif ( + region.hint == Region.HINT_CONVEX_POLYGON + and len(region.points) >= 6 + and len(region.points) % 2 == 0 + ): + corners = tuple( + _point(x, y, z) + 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 + 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 region_lines: + marker = _new_marker( + update, + patch, + marker_id, + Marker.LINE_LIST, + color, + default_ttl_sec, + ) + marker.scale.x = 0.08 + marker.points = region_lines + markers.append(marker) + + return markers + + +class RegionMarkerState: + """Convert region updates into per-source RViz marker actions.""" + + 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.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.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() + + # 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 ( + MapRegionPatch.UPDATE_CLEAR, + MapRegionPatch.UPDATE_OBSTACLE, + ): + continue + if ( + patch.update_type == MapRegionPatch.UPDATE_OBSTACLE + and not self.show_obstacles + ): + 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.markers_by_source[key] = [ + ( + marker.ns, + marker.id, + marker.header.frame_id, + ) + for marker in new_markers + ] + self.latest_stamps[key] = stamp_nsec + + return marker_array + + +class RegionUpdateVisualizer(Node): + """Visualize map-region updates 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 + show_obstacles = self.declare_parameter( + 'show_obstacles', True + ).value + + reliable_qos = QoSProfile( + depth=10, + reliability=ReliabilityPolicy.RELIABLE, + ) + self.state = RegionMarkerState( + default_ttl_sec, + show_obstacles, + ) + 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/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..b8f2576 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_obstacle_launch.py @@ -0,0 +1,298 @@ +# 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') + ttl_sec = LaunchConfiguration('ttl_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( + 'ttl_sec', + default_value=str(scenario_config.ttl_sec), + description='Lifetime of observations that are not re-observed.', + ), + 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', + parameters=[{ + 'output_topic': '/map/scan_region_markers', + 'show_obstacles': False, + }], + ) + 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', + 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': ParameterValue(ttl_sec, value_type=float), + '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, + source_contribution_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..46d536a --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/replan_scenarios.py @@ -0,0 +1,94 @@ +# 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 + ttl_sec: 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, + ttl_sec=60.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, + ttl_sec=210.0, + 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_conversion.py b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_conversion.py new file mode 100644 index 0000000..c069235 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_conversion.py @@ -0,0 +1,72 @@ +# 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_regions( + ranges, + angle_min, + angle_increment, + range_min, + range_max, + max_observation_range, + beam_stride, +): + """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) + + 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] + 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 not isfinite(clear_distance) or clear_distance <= 0.0: + continue + + angle = angle_min + index * angle_increment + 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 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 new file mode 100644 index 0000000..4004f1b --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rmf_layered_map_server_demo/scan_region_publisher.py @@ -0,0 +1,243 @@ +# 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_regions + + +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) + + 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): + """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 scan as clear and obstacle regions.""" + + 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', 10.0).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 + 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') + 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.last_publish_stamp_sec = None + self.pending_scan = 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.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 ' + f'{self.robot_name} (stride={self.beam_stride}, ' + f'period={self.publish_period_sec:.2f}s, ' + f'self_filter_radius={self.self_filter_radius:.2f}m)' + ) + + 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: + transform = self.tf_buffer.lookup_transform( + self.map_frame, + scan.header.frame_id, + Time.from_msg(scan.header.stamp), + ) + 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}' + ) + return + + self.pending_scan = None + clear_polygons, obstacle_points = scan_regions( + scan.ranges, + scan.angle_min, + scan.angle_increment, + scan.range_min, + scan.range_max, + self.max_observation_range, + self.beam_stride, + ) + obstacle_points = filter_obstacle_points( + obstacle_points, + self.self_filter_radius, + ) + 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(clear_polygons)} clear regions and ' + f'{len(obstacle_points)} obstacle regions from ' + f'{len(scan.ranges)} laser beams' + ) + + def make_update(self, transform, clear_polygons, obstacle_points): + update = MapRegionUpdate() + 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 + + update.patches = make_scan_patches( + clear_polygons, + obstacle_points, + self.ttl_sec, + ) + 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/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/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/rviz/nav2_observations.rviz b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz new file mode 100644 index 0000000..247b243 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/rviz/nav2_observations.rviz @@ -0,0 +1,229 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Combined Global Map1 + - /Retained Robot Contributions1 + - /Latest Robot Scan Regions1 + - /Scenario Robots, Goals, and Obstacle1 + - /Robot 0 Nav2 Path1 + - /Robot 1 Nav2 Path1 + 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 + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Latest Robot Scan Regions + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/scan_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 + 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 + 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.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..0ba015d --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/setup.py @@ -0,0 +1,59 @@ +# 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 glob import glob +import os + +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, '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, + 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_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', + '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 = ' + '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_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_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/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_region_update_visualizer.py b/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py new file mode 100644 index 0000000..7339870 --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_region_update_visualizer.py @@ -0,0 +1,163 @@ +# 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 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_prototype_msgs.msg import Region +from visualization_msgs.msg import Marker + + +def _update(stamp_sec=1): + 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 + 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_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_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_new_update_replaces_previous_markers(): + 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].id == markers[1].id == 0 + assert [(point.x, point.y) for point in markers[1].points] == [(3.0, 4.0)] + + +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).markers + + assert [marker.type for marker in markers] == [Marker.LINE_LIST] + + +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 == [] 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..85fddb5 --- /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 + 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) + assert WAREHOUSE_SCENARIO.ttl_sec == 210.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..f2c631a --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_scan_conversion.py @@ -0,0 +1,102 @@ +# 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, inf, nan, pi, sin + +import pytest + +from rmf_layered_map_server_demo.scan_conversion import scan_regions + + +def test_scan_filters_invalid_and_out_of_range_returns(): + clear_polygons, obstacle_points = scan_regions( + [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(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(): + _, obstacle_points = scan_regions( + [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(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_regions( + [], + angle_min=0.0, + angle_increment=0.1, + range_min=0.1, + range_max=5.0, + max_observation_range=2.5, + beam_stride=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 new file mode 100644 index 0000000..e725cbd --- /dev/null +++ b/map_server/rmf_layered_map_server_demo/test/test_scan_region_publisher.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 rmf_layered_map_msgs.msg import MapRegionPatch +from rmf_layered_map_server_demo.scan_region_publisher import ( + filter_obstacle_points, + 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 + + +def test_filters_current_scan_points_inside_robot_body(): + points = filter_obstacle_points( + [(0.1, 0.0), (0.3, 0.0)], + self_filter_radius=0.22, + ) + + assert points == [(0.3, 0.0)] 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) 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/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..918d509 --- /dev/null +++ b/map_server/rmf_layered_map_server_test/test/test_layered_map_server.py @@ -0,0 +1,245 @@ +# 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, + MapSourceSnapshot, +) +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 = [] + self.source_snapshots = [] + + 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, + ) + 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() + + 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 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) + + 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 + and self.last_source_cell('test/obstacle', 12) == 100 + ), + ), + 'layered map server did not publish the obstacle contribution', + ) + + 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 + and self.last_source_cell('test/obstacle', 12) is None + ), + ), + 'layered map server did not reset the obstacle source', + ) + + 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 + and self.last_source_cell('test/obstacle', 12) is None + ), + timeout=4.0, + ), + 'layered map server did not prune the expired obstacle source', + ) + + +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/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/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(); 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/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..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 @@ -30,7 +30,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, TextSubstitution @@ -63,6 +63,8 @@ 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') + staged_startup = LaunchConfiguration('staged_startup') log_settings = LaunchConfiguration('log_settings', default='true') # Declare the launch arguments @@ -108,6 +110,18 @@ 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', + ) + + 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( @@ -126,11 +140,13 @@ def generate_launch_description(): # Define commands for launching the navigation instances bringup_cmd_group = [] - for robot_name in robots_list: + staged_groups = [] + 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=', @@ -161,11 +177,19 @@ 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', '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'])), @@ -176,9 +200,28 @@ 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_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')) @@ -199,12 +242,15 @@ 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) + 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'])], @@ -243,4 +289,7 @@ def generate_launch_description(): for cmd in bringup_cmd_group: ld.add_action(cmd) + for group in staged_groups: + ld.add_action(group) + return ld 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..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,22 +23,24 @@ from launch.actions import ( DeclareLaunchArgument, ExecuteProcess, + GroupAction, IncludeLaunchDescription, 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 -from launch_ros.actions import Node +from launch_ros.actions import Node, SetRemap 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 +53,8 @@ def generate_launch_description(): autostart = LaunchConfiguration('autostart') 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') @@ -122,6 +126,18 @@ 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_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'), @@ -194,6 +210,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 +243,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 @@ -236,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() @@ -272,6 +315,8 @@ 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(declare_clock_topic_cmd) ld.add_action(world_sdf_xacro) ld.add_action(remove_temp_sdf_file) @@ -283,5 +328,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 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 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, diff --git a/rmf_layered_map_msgs/CMakeLists.txt b/rmf_layered_map_msgs/CMakeLists.txt new file mode 100644 index 0000000..dc50aae --- /dev/null +++ b/rmf_layered_map_msgs/CMakeLists.txt @@ -0,0 +1,37 @@ +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(nav_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" + "msg/MapSourceContribution.msg" + "msg/MapSourceSnapshot.msg" +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + DEPENDENCIES geometry_msgs nav_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..4a686d5 --- /dev/null +++ b/rmf_layered_map_msgs/msg/MapRegionPatch.msg @@ -0,0 +1,30 @@ +# MapRegionPatch.msg + +# The regions should be interpreted as temporary occupied space. +uint8 UPDATE_OBSTACLE=1 + +# 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. +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, axis-aligned +# rectangle, and convex polygon 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..57dbc3d --- /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 older same-source obstacles are removed +# along clear rays before current obstacle endpoints are marked. +MapRegionPatch[] patches diff --git a/rmf_layered_map_msgs/msg/MapSourceContribution.msg b/rmf_layered_map_msgs/msg/MapSourceContribution.msg new file mode 100644 index 0000000..d8fbcb1 --- /dev/null +++ b/rmf_layered_map_msgs/msg/MapSourceContribution.msg @@ -0,0 +1,9 @@ +# MapSourceContribution.msg + +# Source that owns these rasterized obstacle cells. +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..2f980fd --- /dev/null +++ b/rmf_layered_map_msgs/msg/MapSourceSnapshot.msg @@ -0,0 +1,10 @@ +# MapSourceSnapshot.msg + +# 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 obstacle state, grouped by observation source. +MapSourceContribution[] sources diff --git a/rmf_layered_map_msgs/package.xml b/rmf_layered_map_msgs/package.xml new file mode 100644 index 0000000..7614a83 --- /dev/null +++ b/rmf_layered_map_msgs/package.xml @@ -0,0 +1,31 @@ + + + + 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 + nav_msgs + rmf_prototype_msgs + std_msgs + + geometry_msgs + nav_msgs + rmf_prototype_msgs + std_msgs + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + +