feat(costmap): generic grid projection primitives and typed layer access

Cargo.toml

@@ -46,6 +46,11 @@ name = "occupancy_raycast"
 path = "examples/occupancy_raycast.rs"
 required-features = ["rerun"]
 
+[[example]]
+name = "learned_traversability"
+path = "examples/learned_traversability.rs"
+required-features = ["rerun"]
+
 [[bench]]
 name = "load_warehouse"
 harness = false

examples/learned_traversability.rs

@@ -0,0 +1,190 @@
+//! # Local Costmap with Learned Traversability
+//!
+//! This example demonstrates a rolling-window local costmap where a mock "learned traversability"
+//! model fills a semantic grid each frame. The model predicts terrain traversability (0.0 = free,
+//! 1.0 = impassable), which is projected into the master costmap and then inflated for safe planning.
+//!
+//! The pipeline is: **semantic traversability layer** (mock learned model output) → **projection**
+//! (conversion to cost space) → **inflation layer** → single master grid.
+//!
+//! ## Robotics Context
+//! This pattern demonstrates how learned/perception-based traversability models can be integrated
+//! into a costmap pipeline:
+//! - **Semantic segmentation**: A neural network predicts traversability for each cell (e.g., 0.9
+//!   for rocky terrain, 0.0 for smooth grass).
+//! - **Projection**: Convert semantic values to costmap costs via a configurable closure.
+//! - **Rolling window**: Keep the semantic grid centered on the robot as it moves.
+//! - **Inflation**: Expand obstacles to account for robot size and safety margins.
+//!
+//! The robot in this example drives across a high-cost terrain band and the control decision is
+//! printed when high-traversability costs are detected underneath the robot.
+
+use std::error::Error;
+use std::time::Duration;
+
+use costmap::rerun_viz::{log_costmap, log_point3d};
+use costmap::types::{COST_FREE, Pose2};
+use costmap::{Grid2d, MapInfo, MergePolicy, ProjectionLayer, cost_from_unit};
+use costmap::{InflationConfig, LayeredCostmap, WavefrontInflationLayer};
+use glam::{UVec2, Vec2, Vec3};
+
+// Simulation parameters
+const DELAY_MS: u64 = 100;
+const WAYPOINT_SPEED_MPS: f32 = 1.1;
+const WAYPOINTS: &[(f32, f32)] = &[(2.0, 3.0), (7.0, 3.0), (7.0, 5.0), (2.0, 5.0)];
+
+// Local costmap configuration
+/// Size of the robot-centered rolling window costmap (square, in cells)
+const LOCAL_SIZE_CELLS: u32 = 120;
+const RESOLUTION: f32 = 0.1;
+
+// Visualization Z-heights for layering elements in Rerun
+const Z_LOCAL: f32 = 0.12;
+const Z_ROBOT: f32 = 0.35;
+
+fn main() -> Result<(), Box<dyn Error>> {
+    let resolution = RESOLUTION;
+    let local_info = MapInfo::square(LOCAL_SIZE_CELLS, resolution);
+    let rec = rerun::RecordingStreamBuilder::new("costmap_learned_traversability").spawn()?;
+
+    let mut layered = LayeredCostmap::new(local_info, COST_FREE, true);
+    let id = layered.add_layer(Box::new(ProjectionLayer::from_grid(
+        Grid2d::<f32>::new_with_value(local_info, 0.0),
+        MergePolicy::Max,
+        true, // rolling window
+        |t| Some(cost_from_unit(*t)),
+    )));
+    layered.add_layer(Box::new(WavefrontInflationLayer::new(InflationConfig {
+        inflation_radius_m: 0.4,
+        inscribed_radius_m: 0.1,
+        cost_scaling_factor: 3.0,
+        ..Default::default()
+    })));
+
+    let waypoints: Vec<Vec2> = WAYPOINTS.iter().map(|(x, y)| Vec2::new(*x, *y)).collect();
+    let (segment_lengths, total_length) = build_segments(&waypoints);
+    let dt = DELAY_MS as f32 / 1000.0;
+
+    let mut frame_idx: i64 = 0;
+    loop {
+        rec.set_time_sequence("frame", frame_idx);
+        let distance = (frame_idx as f32) * WAYPOINT_SPEED_MPS * dt;
+        let (robot_pos, heading_dir) =
+            sample_path(&waypoints, &segment_lengths, total_length, distance);
+        let heading = heading_dir.y.atan2(heading_dir.x);
+        let robot = Pose2::new(robot_pos, heading);
+
+        // Mock "learned traversability model": fill the source world-anchored. The
+        // rolling-window ProjectionLayer re-centres the grid on the robot during
+        // update_map, and update_origin preserves data by world position, so we just
+        // write each cell's cost from its current world location.
+        {
+            let proj = layered.layer_mut::<ProjectionLayer<f32>>(id).unwrap();
+            let src = proj.source_mut();
+            let (w, h) = (src.width(), src.height());
+            for y in 0..h {
+                for x in 0..w {
+                    let cell = UVec2::new(x, y);
+                    let world = src.map_to_world(cell);
+                    // Impassable terrain band in world coords (x in [4.0, 4.5]).
+                    // Traversability 1.0 projects to COST_LETHAL, which seeds inflation.
+                    let t = if world.x > 4.0 && world.x < 4.5 {
+                        1.0
+                    } else {
+                        0.0
+                    };
+                    let _ = src.set(cell, t);
+                }
+            }
+        }
+
+        layered.update_map(robot);
+
+        // Control decision: read traversability back under the robot.
+        {
+            let proj = layered.layer::<ProjectionLayer<f32>>(id).unwrap();
+            let src = proj.source();
+            if let Some(cell) = src.world_to_map(robot_pos) {
+                let t = src.get(cell).copied().unwrap_or(0.0);
+                if t > 0.5 {
+                    println!(
+                        "[frame {frame_idx}] high-cost terrain (t={t:.2}) under robot -> SLOW DOWN"
+                    );
+                }
+            }
+        }
+
+        log_costmap(&rec, "world/local_costmap", layered.master(), Z_LOCAL)?;
+        log_point3d(
+            &rec,
+            "world/robot",
+            Vec3::new(robot_pos.x, robot_pos.y, Z_ROBOT),
+            Some(rerun::Color::from_rgb(0, 200, 255)),
+            Some(5.0),
+        )?;
+
+        if DELAY_MS > 0 {
+            std::thread::sleep(Duration::from_millis(DELAY_MS));
+        }
+        frame_idx = frame_idx.wrapping_add(1);
+    }
+}
+
+// Helper functions for path following - not core library APIs
+
+fn build_segments(waypoints: &[Vec2]) -> (Vec<f32>, f32) {
+    let mut lengths = Vec::with_capacity(waypoints.len());
+    let mut total = 0.0;
+
+    if waypoints.len() < 2 {
+        return (lengths, total);
+    }
+
+    for idx in 0..waypoints.len() {
+        let a = waypoints[idx];
+        let b = waypoints[(idx + 1) % waypoints.len()];
+        let len = (b - a).length();
+        lengths.push(len);
+        total += len;
+    }
+
+    (lengths, total)
+}
+
+fn sample_path(
+    waypoints: &[Vec2],
+    segment_lengths: &[f32],
+    total_length: f32,
+    distance: f32,
+) -> (Vec2, Vec2) {
+    if waypoints.is_empty() {
+        return (Vec2::ZERO, Vec2::X);
+    }
+
+    if waypoints.len() == 1 || total_length <= 0.0 {
+        return (waypoints[0], Vec2::X);
+    }
+
+    let mut remaining = distance.rem_euclid(total_length);
+
+    for (idx, seg_len) in segment_lengths.iter().enumerate() {
+        if *seg_len <= 0.0 {
+            continue;
+        }
+        if remaining <= *seg_len {
+            let a = waypoints[idx];
+            let b = waypoints[(idx + 1) % waypoints.len()];
+            let dir = (b - a).normalize_or_zero();
+            let pos = a + dir * remaining;
+            let heading = if dir.length_squared() == 0.0 {
+                Vec2::X
+            } else {
+                dir
+            };
+            return (pos, heading);
+        }
+        remaining -= *seg_len;
+    }
+
+    (waypoints[0], Vec2::X)
+}

examples/local_costmap_lidar.rs

@@ -28,7 +28,7 @@ use costmap::rerun_viz::{log_costmap, log_occupancy_grid, log_point3d};
 use costmap::types::{Bounds, COST_FREE, COST_LETHAL, COST_UNKNOWN, CellRegion, Pose2};
 use costmap::{Costmap, raycast::RayHit2D};
 use costmap::{Grid2d, MapInfo, OccupancyGrid, RosMapLoader, WavefrontInflationLayer};
-use costmap::{InflationConfig, costmap::merge_overwrite};
+use costmap::{InflationConfig, MergePolicy, ProjectionLayer};
 use costmap::{Layer, LayeredCostmap};
 use glam::{Vec2, Vec3};
 
@@ -53,23 +53,29 @@ const Z_LOCAL: f32 = 0.12;
 const Z_ROBOT: f32 = 0.35;
 
 /// Example-only layer: simulates lidar by raycasting on a global occupancy grid.
-/// Keeps an internal obstacle grid (like Nav2's layer costmap_) so observations
-/// persist across frames; each update we shift it, draw new rays, then write to master.
+///
+/// Composes a [`ProjectionLayer<u8>`] for the storage + rolling-window + merge half
+/// (the Nav2 `CostmapLayer` role): observations persist across frames in its owned
+/// `source` grid, and projecting it into the master is the library's job. This layer
+/// only adds the sensor-specific part — drawing rays into that grid each update.
 ///
 /// This would normally be done by listening to a laser scan topic which would
 /// update the obstacle layer.
 struct SimLidarLayer {
     global_grid: Arc<OccupancyGrid>,
-    /// Internal costmap that persists between updates (Nav2-style layer costmap_).
-    obstacle_grid: Costmap,
+    /// Owns the persistent obstacle grid and projects it into the master. With
+    /// `rolling_window: true` the projection layer re-centres the grid on the robot in
+    /// its `update_bounds`, so by the time we draw rays in `update_costs` the grid is
+    /// already centred.
+    proj: ProjectionLayer<u8>,
     last_robot: Pose2,
     max_range_m: f32,
     n_beams: usize,
 }
 
 impl Layer for SimLidarLayer {
     fn reset(&mut self) {
-        self.obstacle_grid.clear();
+        self.proj.reset();
     }
 
     fn is_clearable(&self) -> bool {
@@ -78,13 +84,15 @@ impl Layer for SimLidarLayer {
 
     fn update_bounds(&mut self, robot: Pose2, bounds: &mut Bounds) {
         self.last_robot = robot;
-        bounds.expand_to_include(robot.position);
-        bounds.expand_by(self.max_range_m);
+        // Delegate to the projection layer: re-centres the obstacle grid (rolling
+        // window) and expands the bounds to its extent.
+        self.proj.update_bounds(robot, bounds);
     }
 
     fn update_costs(&mut self, master: &mut Costmap, region: CellRegion) {
-        // 1) Update internal grid origin (rolling window) and draw new rays into it.
-        self.obstacle_grid.update_center(self.last_robot.position);
+        // The obstacle grid is already centred on the robot (done in update_bounds).
+        // Draw new sensor rays into it, then project it into the master.
+        let obstacle_grid = self.proj.source_mut();
         let beam_step = TAU / self.n_beams as f32;
         for beam_idx in 0..self.n_beams {
             let angle = self.last_robot.yaw + beam_step * beam_idx as f32;
@@ -94,18 +102,16 @@ impl Layer for SimLidarLayer {
                 .raycast_dda(self.last_robot.position, dir, self.max_range_m);
             let t = RayHit2D::distance_or(hit, self.max_range_m);
             let endpoint = hit.map(|_| COST_LETHAL);
-            self.obstacle_grid
-                .clear_ray(self.last_robot.position, dir, t, COST_FREE, endpoint);
+            obstacle_grid.clear_ray(self.last_robot.position, dir, t, COST_FREE, endpoint);
         }
-        // 2) Write layer to master (do not copy unknown).
-        merge_overwrite(master, &self.obstacle_grid, region);
+        self.proj.update_costs(master, region);
     }
 }
 
 fn main() -> Result<(), Box<dyn Error>> {
     // Step 1: Load the global map (static environment representation)
     let grid = RosMapLoader::load_from_yaml(DEFAULT_YAML_PATH)?;
-    let info = grid.info().clone();
+    let info = *grid.info();
     let global_grid = Arc::new(grid);
 
     // Step 2: Set up visualization (optional - Rerun is not required to use the library)
@@ -117,10 +123,15 @@ fn main() -> Result<(), Box<dyn Error>> {
 
     // Step 3: Create layered costmap (rolling window, sensor layer + inflation layer)
     let local_info = MapInfo::square(LOCAL_SIZE_CELLS, info.resolution);
-    let mut layered = LayeredCostmap::new(local_info.clone(), COST_FREE, true);
+    let mut layered = LayeredCostmap::new(local_info, COST_FREE, true);
     layered.add_layer(Box::new(SimLidarLayer {
         global_grid: Arc::clone(&global_grid),
-        obstacle_grid: Grid2d::<u8>::new_with_value(local_info.clone(), COST_UNKNOWN),
+        proj: ProjectionLayer::from_grid(
+            Grid2d::<u8>::new_with_value(local_info, COST_UNKNOWN),
+            MergePolicy::Overwrite,
+            true, // rolling window: re-centres the obstacle grid on the robot each update
+            |c| (*c != COST_UNKNOWN).then_some(*c),
+        ),
         last_robot: Pose2::default(),
         max_range_m: MAX_RANGE_M,
         n_beams: N_BEAMS,

src/costmap/layered.rs

@@ -7,20 +7,33 @@
 //! The update loop aggregates bounds from all layers, resets the master region,
 //! then calls each layer's `update_costs` in order.
 
+use std::any::Any;
+
 use glam::{UVec2, Vec2};
 
 use crate::{
     Costmap,
     types::{Bounds, CellRegion, Footprint, MapInfo, Pose2},
 };
 
+/// Identifier returned by [`LayeredCostmap::add_layer`], used to fetch a layer
+/// back by its concrete type with [`LayeredCostmap::layer`] /
+/// [`LayeredCostmap::layer_mut`].
+#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
+pub struct LayerId(usize);
+
 /// A layer is a component which acts on the base master grid. It may contain
 /// its own grid but doesn't need to.
 ///
 /// Each layer is processed in order.
 ///
 /// Each update is limited by a set of bounds passed from higher layers.
-pub trait Layer {
+///
+/// The `Any` supertrait enables [`LayeredCostmap::layer`] /
+/// [`LayeredCostmap::layer_mut`] to downcast a stored layer back to its concrete
+/// type so callers can ingest into or query a layer's internal grid after it has
+/// been added to the stack.
+pub trait Layer: Any {
     /// Reset the layer to its initial state.
     fn reset(&mut self);
 
@@ -70,8 +83,30 @@ impl LayeredCostmap {
     }
 
     /// Add a layer. Order matters: layers are updated in insertion order.
-    pub fn add_layer(&mut self, layer: Box<dyn Layer>) {
+    ///
+    /// Returns a [`LayerId`] that can be passed to [`Self::layer`] /
+    /// [`Self::layer_mut`] to access the layer by its concrete type later.
+    pub fn add_layer(&mut self, layer: Box<dyn Layer>) -> LayerId {
+        let id = LayerId(self.layers.len());
         self.layers.push(layer);
+        id
+    }
+
+    /// Borrow a previously added layer as its concrete type `T`.
+    ///
+    /// Returns `None` if the id is unknown or the layer is not a `T`.
+    pub fn layer<T: Layer>(&self, id: LayerId) -> Option<&T> {
+        let layer: &dyn Layer = self.layers.get(id.0)?.as_ref();
+        (layer as &dyn Any).downcast_ref::<T>()
+    }
+
+    /// Mutably borrow a previously added layer as its concrete type `T`.
+    ///
+    /// Returns `None` if the id is unknown or the layer is not a `T`. This is the
+    /// path used to ingest data into a layer's internal grid between updates.
+    pub fn layer_mut<T: Layer>(&mut self, id: LayerId) -> Option<&mut T> {
+        let layer: &mut dyn Layer = self.layers.get_mut(id.0)?.as_mut();
+        (layer as &mut dyn Any).downcast_mut::<T>()
     }
 
     /// Immutable reference to the master grid.