diff --git a/discourse/3-layered-global-occupancy-map.md b/discourse/3-layered-global-occupancy-map.md index 256685e..529f6e4 100644 --- a/discourse/3-layered-global-occupancy-map.md +++ b/discourse/3-layered-global-occupancy-map.md @@ -21,8 +21,7 @@ observations, can be added later. * Clear-space patches have TTLs, just like obstacle patches * A source can reset its previous observations without using a TTL * Robot-mounted sources include the observation-frame pose at observation time -* The Rust map server composes a static occupancy grid and active observations - into `/map` +* The Rust map server rasterizes incoming observations once and composes their active cell contributions with a static occupancy grid into `/map` * The Nav2 demo converts scans from three robots into clear ray sectors and occupied endpoint regions, then displays the source contributions and map * The replanning demo fuses two robots' scans and replans when an updated map blocks an active route * The observation messages live in `rmf_layered_map_msgs`, leaving @@ -114,21 +113,15 @@ accepts point and axis-aligned rectangle regions. # Layered Map Server -`rmf_layered_map_server` keeps the static occupancy grid separate from dynamic -observations and publishes their composition on `/map`. It validates source -timestamps and frames, ignores updates that are older than the latest accepted -update from the same source, supports source resets, and removes observations -after their TTL expires. +`rmf_layered_map_server` keeps the static occupancy grid separate from dynamic observations and publishes their composition on `/map`. It validates source timestamps and frames, ignores updates older than the latest accepted update from the same source, supports source resets, and removes observations after their TTL expires. Each region is transformed and rasterized on arrival. Repeated observations refresh cell contributions instead of stacking region snapshots. Older evidence is retained only when it outlives a newer contribution. -The server applies clear patches before obstacle patches from the same update. -During composition, obstacle observations win over clear observations so -occupied space is not accidentally erased by another active source. +The server applies clear patches before obstacle patches from the same update. During composition, obstacle observations win over clear observations so occupied space is not accidentally erased by another active source. Updates received before the static grid are held in a bounded FIFO queue and rasterized when the grid arrives. A static grid geometry change discards active cell contributions because their indices refer to the old geometry. # Three-Robot Nav2 Demo The launch file starts three robots in different free corners of the warehouse, cycles them through fixed Nav2 goals, and spawns one deterministic Gazebo box near each robot. Each observation node subscribes to its robot's local `sensor_msgs/LaserScan`, filters invalid or out-of-range returns, and publishes free-space ray sectors as convex polygons and occupied endpoints as point regions. -Each scan adds temporary clear and obstacle patches to a rolling observation history. The map server rasterizes clear sectors before obstacle endpoints so a measured hit remains occupied. Active obstacle evidence still wins over clear evidence until its TTL expires. An update may set `reset_source` so the new scan replaces all active observations from that source instead of being added to its history. The observation-frame pose is recorded in the shared `map` frame so the map server can transform the scan-local regions before rasterizing them. +Each scan adds temporary clear and obstacle patches. The map server rasterizes them on arrival and refreshes matching cell contributions rather than retaining region messages. It applies clear cells before obstacle cells so a measured hit remains occupied. Active obstacle evidence still wins over clear evidence until its TTL expires. An update may set `reset_source` so the new scan replaces all active observations from that source. The observation-frame pose is recorded in the shared `map` frame so the map server can transform scan-local regions before rasterizing them. The demo launches Nav2 localization, planning, and control for the fixed goal loops, but does not launch RMF planning. Robot-local RViz windows show Nav2 state, while another RViz window displays the combined global `/map`. The combined view overlays incoming region updates as colored markers grouped by source, with the same retention behavior as the map contributions. diff --git a/map_server/rmf_layered_map_server/README.md b/map_server/rmf_layered_map_server/README.md index 3b9c7bc..e6e5c34 100644 --- a/map_server/rmf_layered_map_server/README.md +++ b/map_server/rmf_layered_map_server/README.md @@ -10,7 +10,7 @@ Default topics: - `/map/region_updates`: `rmf_layered_map_msgs/MapRegionUpdate` - `/map`: composed `nav_msgs/OccupancyGrid` -Dynamic observations expire according to their TTL, so transient local obstacles do not remain in the global planning map forever. A LiDAR or local costmap observer can keep refreshing an obstacle while it is still seen, then let the server decay it after the TTL. +Dynamic observations expire according to their TTL, so transient obstacles do not remain in the global planning map forever. The server transforms and rasterizes each region on arrival, then stores cell contributions by source, map, update type, and cell. Repeated observations refresh matching cells instead of stacking region snapshots. Older evidence is retained only when it outlives a newer contribution. `MapRegionUpdate` messages contain a list of `MapRegionPatch` entries. A sensor snapshot that clears freespace and marks obstacles can publish clear and obstacle @@ -26,6 +26,25 @@ must match the static occupancy grid frame. Updates need a non-zero source timestamp. The current implementation accepts point and axis-aligned rectangle regions. It logs and ignores other region types. +Region updates received before the static occupancy grid are held in a bounded FIFO queue and rasterized once the grid arrives. Replacing only the static grid data preserves active dynamic cells; changing its frame, dimensions, resolution, or origin discards them because their cell indices no longer describe the new grid. + +## Performance Benchmark + +The deterministic benchmark is installed by the normal colcon build and exercises the map server without ROS transport, visualization, or simulator overhead. It reports ingest, prune, composition, and total latency, plus throughput, peak RSS, and an output-grid checksum. + +Build and source the workspace, then run each supported update pattern: + +```bash +ros2 run rmf_layered_map_server layered_map_benchmark \ + --label candidate --scenario reset +ros2 run rmf_layered_map_server layered_map_benchmark \ + --label candidate --scenario rolling-overlap +ros2 run rmf_layered_map_server layered_map_benchmark \ + --label candidate --scenario rolling-moving +``` + +For an A/B comparison, run the same benchmark source and arguments against both revisions. Compare performance only when the checksums match. + ## Visualization Smoke Test Build and source the workspace: diff --git a/map_server/rmf_layered_map_server/src/bin/layered_map_benchmark.rs b/map_server/rmf_layered_map_server/src/bin/layered_map_benchmark.rs new file mode 100644 index 0000000..182bed7 --- /dev/null +++ b/map_server/rmf_layered_map_server/src/bin/layered_map_benchmark.rs @@ -0,0 +1,365 @@ +// Copyright 2026 Open Source Robotics Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +use rmf_layered_map_server::LayeredMap; +use ros_env::{ + geometry_msgs::msg::{Point, Pose, Quaternion}, + nav_msgs::msg::{MapMetaData, OccupancyGrid}, + rmf_layered_map_msgs::msg::{MapObservationSource, MapRegionPatch, MapRegionUpdate}, + rmf_prototype_msgs::msg::Region, + std_msgs::msg::Header, +}; +use std::{ + collections::hash_map::DefaultHasher, + env, fs, + hash::{Hash, Hasher}, + hint::black_box, + io::{self, Write}, + time::{Duration, Instant}, +}; + +const NANOS_PER_SECOND: i128 = 1_000_000_000; +const UPDATES: usize = 240; +const ROUNDS: usize = 8; +const WARMUP_ROUNDS: usize = 2; +const BEAMS: usize = 90; +const SOURCES: usize = 2; +const EVENT_PERIOD_NSEC: i128 = 250_000_000; +const TTL_SEC: f64 = 10.0; +const MAP_WIDTH: u32 = 240; +const MAP_HEIGHT: u32 = 180; +const MAP_RESOLUTION: f32 = 0.1; +const SCAN_RADIUS: f64 = 4.0; + +#[derive(Clone, Copy)] +enum Scenario { + Reset, + RollingOverlap, + RollingMoving, +} + +impl Scenario { + fn parse(value: &str) -> Result { + match value { + "reset" => Ok(Self::Reset), + "rolling-overlap" => Ok(Self::RollingOverlap), + "rolling-moving" => Ok(Self::RollingMoving), + _ => Err(format!( + "unsupported scenario '{value}'; expected reset, rolling-overlap, or rolling-moving" + )), + } + } + + fn name(self) -> &'static str { + match self { + Self::Reset => "reset", + Self::RollingOverlap => "rolling-overlap", + Self::RollingMoving => "rolling-moving", + } + } +} + +struct Config { + label: String, + scenario: Scenario, +} + +#[derive(Clone)] +struct TraceEvent { + update: MapRegionUpdate, + now_nsec: i128, +} + +#[derive(Default)] +struct Samples { + ingest: Vec, + prune: Vec, + compose: Vec, + total: Vec, +} + +struct ReplayResult { + samples: Samples, + checksum: u64, +} + +fn main() -> Result<(), String> { + let config = parse_args()?; + println!( + "Running layered map benchmark ({})...", + config.scenario.name() + ); + io::stdout() + .flush() + .map_err(|error| format!("failed to flush benchmark output: {error}"))?; + + let trace = make_trace(&config); + + for _ in 0..WARMUP_ROUNDS { + black_box(replay(&trace, false).checksum); + } + + let mut samples = Samples::default(); + let mut expected_checksum = None; + for _ in 0..ROUNDS { + let result = replay(&trace, true); + if let Some(expected) = expected_checksum { + if result.checksum != expected { + return Err(format!( + "non-deterministic output checksum: expected {expected}, got {}", + result.checksum + )); + } + } else { + expected_checksum = Some(result.checksum); + } + samples.ingest.extend(result.samples.ingest); + samples.prune.extend(result.samples.prune); + samples.compose.extend(result.samples.compose); + samples.total.extend(result.samples.total); + } + + let measured_updates = UPDATES * ROUNDS; + let measured_seconds = samples.total.iter().map(Duration::as_secs_f64).sum::(); + let throughput = measured_updates as f64 / measured_seconds; + let peak_rss_kib = peak_rss_kib().unwrap_or(0); + + println!(); + println!("Results"); + println!(" Label {}", config.label); + println!(" Scenario {}", config.scenario.name()); + println!(" Workload {UPDATES} updates x {ROUNDS} rounds ({WARMUP_ROUNDS} warm-up)"); + println!(" Scan {BEAMS} beams x {SOURCES} sources"); + println!(" Throughput {throughput:.1} updates/s"); + println!(" Peak RSS {:.2} MiB", peak_rss_kib as f64 / 1024.0); + println!(" Checksum {}", expected_checksum.unwrap_or(0)); + println!(); + println!("Latency (ms)"); + println!( + " {:<10} {:>10} {:>10} {:>10} {:>10}", + "Phase", "Mean", "P50", "P95", "P99" + ); + print_stats("ingest", samples.ingest); + print_stats("prune", samples.prune); + print_stats("compose", samples.compose); + print_stats("total", samples.total); + + Ok(()) +} + +fn parse_args() -> Result { + let mut config = Config { + label: "unlabeled".to_string(), + scenario: Scenario::Reset, + }; + + let mut args = env::args().skip(1); + while let Some(flag) = args.next() { + let value = args + .next() + .ok_or_else(|| format!("missing value for argument '{flag}'"))?; + match flag.as_str() { + "--label" => config.label = value, + "--scenario" => config.scenario = Scenario::parse(&value)?, + _ => return Err(format!("unsupported argument '{flag}'")), + } + } + + Ok(config) +} + +fn make_trace(config: &Config) -> Vec { + (0..UPDATES) + .map(|event_index| { + let source_index = event_index % SOURCES; + let source_scan_index = event_index / SOURCES; + let now_nsec = NANOS_PER_SECOND + event_index as i128 * EVENT_PERIOD_NSEC; + TraceEvent { + update: make_update(config.scenario, source_index, source_scan_index, now_nsec), + now_nsec, + } + }) + .collect() +} + +fn make_update( + scenario: Scenario, + source_index: usize, + scan_index: usize, + stamp_nsec: i128, +) -> MapRegionUpdate { + let mut source = MapObservationSource { + header: Header { + frame_id: "map".to_string(), + ..Default::default() + }, + source_id: format!("robot_{source_index}/scan"), + robot_name: format!("robot_{source_index}"), + map_name: "benchmark_map".to_string(), + default_ttl_sec: TTL_SEC, + ..Default::default() + }; + source.header.stamp.sec = (stamp_nsec / NANOS_PER_SECOND) as i32; + source.header.stamp.nanosec = (stamp_nsec % NANOS_PER_SECOND) as u32; + source.robot_pose.position.x = match scenario { + Scenario::RollingMoving => 7.0 + (scan_index % 40) as f64 * 0.1, + Scenario::Reset | Scenario::RollingOverlap => 9.0, + }; + let source_fraction = source_index as f64 / SOURCES.saturating_sub(1).max(1) as f64; + source.robot_pose.position.y = 5.0 + 7.0 * source_fraction; + source.robot_pose.orientation.w = 1.0; + + let mut clear_patch = MapRegionPatch { + update_type: MapRegionPatch::UPDATE_CLEAR, + occupancy_value: 0, + ttl_sec: TTL_SEC, + ..Default::default() + }; + let mut obstacle_patch = MapRegionPatch { + update_type: MapRegionPatch::UPDATE_OBSTACLE, + occupancy_value: 100, + ttl_sec: TTL_SEC, + ..Default::default() + }; + + let angle_step = std::f64::consts::TAU / BEAMS as f64; + for beam in 0..BEAMS { + let start_angle = beam as f64 * angle_step; + let end_angle = (beam + 1) as f64 * angle_step; + let varying_radius = SCAN_RADIUS - 0.35 * ((beam + scan_index) % 7) as f64 / 6.0; + let start = ( + varying_radius * start_angle.cos(), + varying_radius * start_angle.sin(), + ); + let end = ( + varying_radius * end_angle.cos(), + varying_radius * end_angle.sin(), + ); + clear_patch.regions.push(Region { + hint: Region::HINT_CONVEX_POLYGON, + points: vec![ + 0.0, + 0.0, + start.0 as f32, + start.1 as f32, + end.0 as f32, + end.1 as f32, + ], + }); + + if beam % 6 == 0 { + obstacle_patch.regions.push(Region { + hint: Region::HINT_POINT, + points: vec![start.0 as f32, start.1 as f32], + }); + } + } + + MapRegionUpdate { + source, + reset_source: matches!(scenario, Scenario::Reset), + patches: vec![clear_patch, obstacle_patch], + } +} + +fn replay(trace: &[TraceEvent], record_samples: bool) -> ReplayResult { + let mut map = LayeredMap::new(Duration::from_secs(TTL_SEC as u64)); + map.set_static_map(static_grid()); + let mut samples = Samples::default(); + let mut hasher = DefaultHasher::new(); + + for event in trace.iter().cloned() { + let total_start = Instant::now(); + + let prune_start = Instant::now(); + black_box(map.prune_expired(event.now_nsec)); + let prune_elapsed = prune_start.elapsed(); + + let ingest_start = Instant::now(); + black_box(map.ingest_region_update(event.update, event.now_nsec)); + let ingest_elapsed = ingest_start.elapsed(); + + let compose_start = Instant::now(); + let grid = map.compose().expect("benchmark always has a static map"); + let compose_elapsed = compose_start.elapsed(); + + let total_elapsed = total_start.elapsed(); + grid.data.hash(&mut hasher); + black_box(&grid); + + if record_samples { + samples.prune.push(prune_elapsed); + samples.ingest.push(ingest_elapsed); + samples.compose.push(compose_elapsed); + samples.total.push(total_elapsed); + } + } + + ReplayResult { + samples, + checksum: hasher.finish(), + } +} + +fn static_grid() -> OccupancyGrid { + OccupancyGrid { + header: Header { + frame_id: "map".to_string(), + ..Default::default() + }, + info: MapMetaData { + resolution: MAP_RESOLUTION, + width: MAP_WIDTH, + height: MAP_HEIGHT, + origin: Pose { + position: Point::default(), + orientation: Quaternion { + w: 1.0, + ..Default::default() + }, + }, + ..Default::default() + }, + data: vec![0; (MAP_WIDTH * MAP_HEIGHT) as usize], + ..Default::default() + } +} + +fn print_stats(name: &str, mut values: Vec) { + values.sort_unstable(); + let mean_ms = + values.iter().map(Duration::as_secs_f64).sum::() * 1000.0 / values.len() as f64; + println!( + " {name:<10} {:>10.3} {:>10.3} {:>10.3} {:>10.3}", + mean_ms, + percentile(&values, 50).as_secs_f64() * 1000.0, + percentile(&values, 95).as_secs_f64() * 1000.0, + percentile(&values, 99).as_secs_f64() * 1000.0, + ); +} + +fn percentile(values: &[Duration], percentile: usize) -> Duration { + let index = ((values.len() - 1) * percentile).div_ceil(100); + values[index] +} + +fn peak_rss_kib() -> Option { + fs::read_to_string("/proc/self/status") + .ok()? + .lines() + .find_map(|line| { + let value = line.strip_prefix("VmHWM:")?; + value.split_whitespace().next()?.parse().ok() + }) +} diff --git a/map_server/rmf_layered_map_server/src/lib.rs b/map_server/rmf_layered_map_server/src/lib.rs index 50214d6..2b176d5 100644 --- a/map_server/rmf_layered_map_server/src/lib.rs +++ b/map_server/rmf_layered_map_server/src/lib.rs @@ -19,26 +19,32 @@ use ros_env::{ rmf_layered_map_msgs::msg::{MapRegionPatch, MapRegionUpdate}, rmf_prototype_msgs::msg::Region, }; -use std::{collections::HashMap, time::Duration}; +use std::{ + collections::{HashMap, HashSet, VecDeque}, + time::Duration, +}; const NANOS_PER_SECOND: i128 = 1_000_000_000; const MAP_QOS_DEPTH: u32 = 10; +const MAX_PENDING_REGION_UPDATES: usize = 1024; -#[derive(Clone, Copy, Debug, PartialEq, Eq)] +#[derive(Clone, Copy, Debug, Hash, PartialEq, Eq)] enum DynamicUpdateType { Obstacle, Clear, } -#[derive(Clone, Debug)] -struct DynamicObservation { - source_id: String, - map_name: String, - frame_id: String, +#[derive(Clone, Debug, Hash, PartialEq, Eq)] +struct DynamicCellKey { update_type: DynamicUpdateType, + cell_index: usize, +} + +#[derive(Clone, Debug)] +struct DynamicCell { occupancy_value: i8, - regions: Vec, expires_at_nsec: i128, + sequence: u64, } #[derive(Clone, Debug, Hash, PartialEq, Eq)] @@ -50,9 +56,10 @@ struct ObservationSourceKey { #[derive(Clone, Debug)] pub struct LayeredMap { static_grid: Option, - dynamic_observations: Vec, + dynamic_cells: HashMap>>, latest_source_stamps: HashMap, default_ttl_nsec: i128, + next_sequence: u64, revision: u64, } @@ -60,9 +67,10 @@ impl LayeredMap { pub fn new(default_ttl: Duration) -> Self { Self { static_grid: None, - dynamic_observations: Vec::new(), + dynamic_cells: HashMap::new(), latest_source_stamps: HashMap::new(), default_ttl_nsec: duration_to_nsec(default_ttl), + next_sequence: 0, revision: 0, } } @@ -72,11 +80,18 @@ impl LayeredMap { } pub fn dynamic_observation_count(&self) -> usize { - self.dynamic_observations.len() + self.dynamic_cells.values().map(HashMap::len).sum() } pub fn set_static_map(&mut self, mut grid: OccupancyGrid) { normalize_grid_data(&mut grid); + if self + .static_grid + .as_ref() + .is_some_and(|current| !same_grid_geometry(current, &grid)) + { + self.dynamic_cells.clear(); + } self.static_grid = Some(grid); self.revision = self.revision.wrapping_add(1); } @@ -85,6 +100,9 @@ impl LayeredMap { if source_validation_error(&update, self.static_frame_id()).is_some() { return false; } + let Some(static_grid) = self.static_grid.as_ref() else { + return false; + }; let reset_source = update.reset_source; let key = ObservationSourceKey { @@ -105,14 +123,10 @@ impl LayeredMap { let mut changed = false; if reset_source { - let before = self.dynamic_observations.len(); - self.dynamic_observations - .retain(|obs| obs.source_id != key.source_id || obs.map_name != key.map_name); - changed |= before != self.dynamic_observations.len(); + changed |= self.dynamic_cells.remove(&key).is_some(); } let default_ttl_sec = update.source.default_ttl_sec; - let frame_id = update.source.header.frame_id; let robot_pose = update.source.robot_pose; let mut patches = update.patches; patches.sort_by_key(|patch| match patch.update_type { @@ -128,16 +142,6 @@ impl LayeredMap { _ => continue, }; - let regions: Vec<_> = patch - .regions - .into_iter() - .filter(|region| region_validation_error(region).is_none()) - .map(|region| transform_region(region, &robot_pose)) - .collect(); - if regions.is_empty() { - continue; - } - let ttl_nsec = positive_seconds_to_nsec(patch.ttl_sec) .or_else(|| positive_seconds_to_nsec(default_ttl_sec)) .unwrap_or(self.default_ttl_nsec); @@ -157,15 +161,35 @@ impl LayeredMap { DynamicUpdateType::Clear => patch.occupancy_value.clamp(0, 100), }; - self.dynamic_observations.push(DynamicObservation { - source_id: key.source_id.clone(), - map_name: key.map_name.clone(), - frame_id: frame_id.clone(), - update_type, - occupancy_value, - regions, - expires_at_nsec, - }); + let cell_indices = patch + .regions + .into_iter() + .filter(|region| region_validation_error(region).is_none()) + .map(|region| transform_region(region, &robot_pose)) + .flat_map(|region| rasterized_indices(static_grid, ®ion)) + .collect::>(); + if cell_indices.is_empty() { + continue; + } + + let sequence = self.next_sequence; + self.next_sequence = self.next_sequence.wrapping_add(1); + let source_cells = self.dynamic_cells.entry(key.clone()).or_default(); + for cell_index in cell_indices { + let history = source_cells + .entry(DynamicCellKey { + update_type, + cell_index, + }) + .or_default(); + // Keep older entries only when they may outlive the new one. + history.retain(|cell| cell.expires_at_nsec > expires_at_nsec); + history.push(DynamicCell { + occupancy_value, + expires_at_nsec, + sequence, + }); + } changed = true; } @@ -180,10 +204,18 @@ impl LayeredMap { } pub fn prune_expired(&mut self, now_nsec: i128) -> bool { - let before = self.dynamic_observations.len(); - self.dynamic_observations - .retain(|obs| obs.expires_at_nsec > now_nsec); - let changed = before != self.dynamic_observations.len(); + let mut changed = false; + self.dynamic_cells.retain(|_, cells| { + cells.retain(|_, history| { + history.retain(|cell| { + let active = cell.expires_at_nsec > now_nsec; + changed |= !active; + active + }); + !history.is_empty() + }); + !cells.is_empty() + }); if changed { self.revision = self.revision.wrapping_add(1); } @@ -193,20 +225,26 @@ impl LayeredMap { pub fn compose(&self) -> Option { let mut composed = self.static_grid.clone()?; normalize_grid_data(&mut composed); - let frame_id = composed.header.frame_id.clone(); - - for observation in self - .dynamic_observations - .iter() - .filter(|obs| obs.frame_id == frame_id && obs.update_type == DynamicUpdateType::Clear) - { - rasterize_observation(&mut composed, observation); - } - for observation in self.dynamic_observations.iter().filter(|obs| { - obs.frame_id == frame_id && obs.update_type == DynamicUpdateType::Obstacle - }) { - rasterize_observation(&mut composed, observation); + for update_type in [DynamicUpdateType::Clear, DynamicUpdateType::Obstacle] { + let mut cells = self + .dynamic_cells + .values() + .flat_map(|source_cells| source_cells.iter()) + .filter_map(|(key, history)| { + if key.update_type != update_type { + return None; + } + history.last().map(|cell| (key, cell)) + }) + .collect::>(); + cells.sort_by_key(|(_, cell)| cell.sequence); + + for (key, cell) in cells { + if let Some(value) = composed.data.get_mut(key.cell_index) { + *value = cell.occupancy_value; + } + } } Some(composed) @@ -246,9 +284,23 @@ impl Default for LayeredMapServerConfig { } } +fn push_pending_region_update( + updates: &mut VecDeque, + update: MapRegionUpdate, +) -> Option { + let dropped = if updates.len() >= MAX_PENDING_REGION_UPDATES { + updates.pop_front() + } else { + None + }; + updates.push_back(update); + dropped +} + pub struct LayeredMapServer { node: Node, map: LayeredMap, + pending_region_updates: VecDeque, map_publisher: rclrs::Publisher, last_published_revision: Option, } @@ -264,6 +316,7 @@ impl LayeredMapServer { Ok(Self { node, map: LayeredMap::new(config.default_ttl), + pending_region_updates: VecDeque::new(), map_publisher, last_published_revision: None, }) @@ -271,6 +324,25 @@ impl LayeredMapServer { fn handle_static_map(&mut self, msg: OccupancyGrid) { self.map.set_static_map(msg); + let pending_updates = std::mem::take(&mut self.pending_region_updates); + let pending_count = pending_updates.len(); + if pending_count > 0 { + let now_nsec = self.now_nsec(); + let mut changed_count = 0; + for update in pending_updates { + log_region_update_errors(&self.node, &update, self.map.static_frame_id()); + if self.map.ingest_region_update(update, now_nsec) { + changed_count += 1; + } + } + rclrs::log!( + self.node.logger(), + "Rasterized {} queued region updates; {} changed the map", + pending_count, + changed_count + ); + } + rclrs::log!( self.node.logger(), "Received static map, revision {}", @@ -280,12 +352,36 @@ impl LayeredMapServer { } fn handle_region_update(&mut self, msg: MapRegionUpdate) { + if self.map.static_grid.is_none() { + log_region_update_errors(&self.node, &msg, None); + if source_validation_error(&msg, None).is_some() { + return; + } + + let source_id = msg.source.source_id.clone(); + if let Some(dropped) = push_pending_region_update(&mut self.pending_region_updates, msg) + { + rclrs::log_warn!( + self.node.logger(), + "Region update queue is full; dropped oldest update from '{}'", + dropped.source.source_id + ); + } + rclrs::log!( + self.node.logger(), + "Queued region update from '{}' until a static map is available ({} queued)", + source_id, + self.pending_region_updates.len() + ); + return; + } + log_region_update_errors(&self.node, &msg, self.map.static_frame_id()); let now_nsec = self.now_nsec(); if self.map.ingest_region_update(msg, now_nsec) { rclrs::log!( self.node.logger(), - "Accepted region update, revision {}, active observations {}", + "Accepted region update, revision {}, active rasterized cells {}", self.map.revision(), self.map.dynamic_observation_count() ); @@ -298,7 +394,7 @@ impl LayeredMapServer { if self.map.prune_expired(now_nsec) { rclrs::log!( self.node.logger(), - "Pruned expired observations, revision {}, active observations {}", + "Pruned expired observations, revision {}, active rasterized cells {}", self.map.revision(), self.map.dynamic_observation_count() ); @@ -569,14 +665,12 @@ fn grid_len(grid: &OccupancyGrid) -> Option { width.checked_mul(height) } -fn rasterize_observation(grid: &mut OccupancyGrid, observation: &DynamicObservation) { - for region in &observation.regions { - for index in rasterized_indices(grid, region) { - if let Some(cell) = grid.data.get_mut(index) { - *cell = observation.occupancy_value; - } - } - } +fn same_grid_geometry(lhs: &OccupancyGrid, rhs: &OccupancyGrid) -> bool { + lhs.header.frame_id == rhs.header.frame_id + && lhs.info.width == rhs.info.width + && lhs.info.height == rhs.info.height + && lhs.info.resolution == rhs.info.resolution + && lhs.info.origin == rhs.info.origin } fn rasterized_indices(grid: &OccupancyGrid, region: &Region) -> Vec { @@ -949,13 +1043,14 @@ mod tests { let mut map = LayeredMap::default(); map.set_static_map(static_grid(3, 3, 0)); - assert!(map.ingest_region_update( + assert!(!map.ingest_region_update( update( MapRegionPatch::UPDATE_OBSTACLE, vec![rectangle(10.0, 10.0, 11.0, 11.0)] ), 0, )); + assert_eq!(map.dynamic_observation_count(), 0); let composed = map.compose().unwrap(); assert!(composed.data.iter().all(|cell| *cell == 0)); @@ -1167,4 +1262,143 @@ mod tests { assert_eq!(composed.data[1 * 5 + 1], 0); assert_eq!(composed.data[3 * 5 + 3], 100); } + + #[test] + fn repeated_regions_refresh_rasterized_cells_without_stacking() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + + let mut first = update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]); + first.patches[0].occupancy_value = 40; + first.patches[0].ttl_sec = 1.0; + assert!(map.ingest_region_update(first, 0)); + assert_eq!(map.dynamic_observation_count(), 1); + + let mut refreshed = update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]); + refreshed.source.header.stamp.sec = 2; + refreshed.patches[0].occupancy_value = 80; + refreshed.patches[0].ttl_sec = 10.0; + assert!(map.ingest_region_update(refreshed, 0)); + assert_eq!(map.dynamic_observation_count(), 1); + assert!(!map.prune_expired(3 * NANOS_PER_SECOND)); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 3 + 1], 80); + + assert!(map.prune_expired(13 * NANOS_PER_SECOND)); + assert_eq!(map.dynamic_observation_count(), 0); + } + + #[test] + fn longer_lived_cell_evidence_resurfaces_after_newer_evidence_expires() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + + let mut first = update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]); + first.patches[0].occupancy_value = 40; + first.patches[0].ttl_sec = 10.0; + assert!(map.ingest_region_update(first, 0)); + + let mut newer = update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]); + newer.source.header.stamp.sec = 2; + newer.patches[0].occupancy_value = 80; + newer.patches[0].ttl_sec = 1.0; + assert!(map.ingest_region_update(newer, 0)); + assert_eq!(map.dynamic_observation_count(), 1); + assert_eq!(map.compose().unwrap().data[1 * 3 + 1], 80); + + assert!(map.prune_expired(4 * NANOS_PER_SECOND)); + assert_eq!(map.dynamic_observation_count(), 1); + assert_eq!(map.compose().unwrap().data[1 * 3 + 1], 40); + + assert!(map.prune_expired(12 * NANOS_PER_SECOND)); + assert_eq!(map.dynamic_observation_count(), 0); + } + + #[test] + fn most_recent_source_wins_between_same_type_cell_contributions() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + + let mut first = update_from( + "robot_1/local_costmap", + MapRegionPatch::UPDATE_OBSTACLE, + vec![point(1.0, 1.0)], + ); + first.patches[0].occupancy_value = 40; + assert!(map.ingest_region_update(first, 0)); + + let mut second = update_from( + "robot_2/local_costmap", + MapRegionPatch::UPDATE_OBSTACLE, + vec![point(1.0, 1.0)], + ); + second.patches[0].occupancy_value = 80; + second.patches[0].ttl_sec = 1.0; + assert!(map.ingest_region_update(second, 0)); + assert_eq!(map.dynamic_observation_count(), 2); + + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 3 + 1], 80); + + assert!(map.prune_expired(3 * NANOS_PER_SECOND)); + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 3 + 1], 40); + } + + #[test] + fn static_map_updates_preserve_cells_only_when_geometry_matches() { + let mut map = LayeredMap::default(); + map.set_static_map(static_grid(3, 3, 0)); + assert!(map.ingest_region_update( + update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]), + 0, + )); + + map.set_static_map(static_grid(3, 3, -1)); + + assert_eq!(map.dynamic_observation_count(), 1); + let composed = map.compose().unwrap(); + assert_eq!(composed.data[1 * 3 + 1], 100); + assert_eq!(composed.data[0], -1); + + map.set_static_map(static_grid(4, 4, 0)); + + assert_eq!(map.dynamic_observation_count(), 0); + assert!(map.compose().unwrap().data.iter().all(|cell| *cell == 0)); + } + + #[test] + fn updates_are_rejected_until_a_static_grid_can_rasterize_them() { + let mut map = LayeredMap::default(); + + assert!(!map.ingest_region_update( + update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]), + 0, + )); + assert_eq!(map.dynamic_observation_count(), 0); + } + + #[test] + fn pending_region_update_queue_is_bounded_and_fifo() { + let mut pending = VecDeque::new(); + + for index in 0..=MAX_PENDING_REGION_UPDATES { + let mut update = update(MapRegionPatch::UPDATE_OBSTACLE, vec![point(1.0, 1.0)]); + update.source.source_id = format!("source_{index}"); + let dropped = push_pending_region_update(&mut pending, update); + if index < MAX_PENDING_REGION_UPDATES { + assert!(dropped.is_none()); + } else { + assert_eq!(dropped.unwrap().source.source_id, "source_0"); + } + } + + assert_eq!(pending.len(), MAX_PENDING_REGION_UPDATES); + assert_eq!(pending.front().unwrap().source.source_id, "source_1"); + assert_eq!( + pending.back().unwrap().source.source_id, + format!("source_{MAX_PENDING_REGION_UPDATES}") + ); + } }