rust_robotics_planning/
adaptive_costmap_namo.rs

1//! Adaptive movable-obstacle costmap slice inspired by Costmap-NAMO.
2//!
3//! This module models the core planning-side behavior without ROS2/Nav2:
4//! partially known cells can be labeled movable, their cost is adapted from
5//! commanded-vs-actual progress, and the result can be converted into a
6//! traversal-risk grid for graph planning.
7
8use rust_robotics_core::{RoboticsError, RoboticsResult};
9
10use crate::traversal_risk_graph::TerrainRiskCell;
11
12/// Semantic state of one adaptive costmap cell.
13#[derive(Debug, Clone, Copy, PartialEq, Eq)]
14pub enum AdaptiveCostmapCellState {
15    Free,
16    Unknown,
17    StaticObstacle,
18    MovableObstacle,
19}
20
21/// One adaptive costmap cell.
22#[derive(Debug, Clone, Copy, PartialEq)]
23pub struct AdaptiveCostmapCell {
24    pub state: AdaptiveCostmapCellState,
25    pub cost: f64,
26}
27
28impl AdaptiveCostmapCell {
29    pub fn free() -> Self {
30        Self {
31            state: AdaptiveCostmapCellState::Free,
32            cost: 0.0,
33        }
34    }
35}
36
37/// Configuration for adaptive movable-obstacle cost updates.
38#[derive(Debug, Clone, PartialEq)]
39pub struct AdaptiveCostmapNamoConfig {
40    pub width: i32,
41    pub height: i32,
42    pub unknown_cost: f64,
43    pub movable_initial_cost: f64,
44    pub movable_cost_increment: f64,
45    pub movable_cost_decrement: f64,
46    pub static_obstacle_cost: f64,
47    pub lethal_cost: f64,
48    pub stuck_command_speed: f64,
49    pub stuck_actual_speed_ratio: f64,
50    pub progress_distance: f64,
51}
52
53impl AdaptiveCostmapNamoConfig {
54    pub fn new(width: i32, height: i32) -> Self {
55        Self {
56            width,
57            height,
58            unknown_cost: 25.0,
59            movable_initial_cost: 20.0,
60            movable_cost_increment: 30.0,
61            movable_cost_decrement: 15.0,
62            static_obstacle_cost: 100.0,
63            lethal_cost: 100.0,
64            stuck_command_speed: 0.05,
65            stuck_actual_speed_ratio: 0.2,
66            progress_distance: 0.02,
67        }
68    }
69}
70
71/// Commanded/observed motion over one update window.
72#[derive(Debug, Clone, Copy, PartialEq)]
73pub struct MotionProgressObservation {
74    pub commanded_speed: f64,
75    pub actual_speed: f64,
76    pub odom_delta: f64,
77}
78
79impl MotionProgressObservation {
80    pub fn stuck(commanded_speed: f64) -> Self {
81        Self {
82            commanded_speed,
83            actual_speed: 0.0,
84            odom_delta: 0.0,
85        }
86    }
87
88    pub fn moving(commanded_speed: f64, actual_speed: f64, odom_delta: f64) -> Self {
89        Self {
90            commanded_speed,
91            actual_speed,
92            odom_delta,
93        }
94    }
95}
96
97/// Adaptive costmap state.
98#[derive(Debug, Clone, PartialEq)]
99pub struct AdaptiveCostmapNamo {
100    config: AdaptiveCostmapNamoConfig,
101    cells: Vec<Vec<AdaptiveCostmapCell>>,
102}
103
104impl AdaptiveCostmapNamo {
105    pub fn new(config: AdaptiveCostmapNamoConfig) -> RoboticsResult<Self> {
106        validate_config(&config)?;
107        let cells =
108            vec![vec![AdaptiveCostmapCell::free(); config.height as usize]; config.width as usize];
109        Ok(Self { config, cells })
110    }
111
112    pub fn width(&self) -> i32 {
113        self.config.width
114    }
115
116    pub fn height(&self) -> i32 {
117        self.config.height
118    }
119
120    pub fn cell(&self, x: i32, y: i32) -> RoboticsResult<AdaptiveCostmapCell> {
121        self.validate_cell(x, y)?;
122        Ok(self.cells[x as usize][y as usize])
123    }
124
125    pub fn set_cell_state(
126        &mut self,
127        x: i32,
128        y: i32,
129        state: AdaptiveCostmapCellState,
130    ) -> RoboticsResult<()> {
131        self.validate_cell(x, y)?;
132        let cost = match state {
133            AdaptiveCostmapCellState::Free => 0.0,
134            AdaptiveCostmapCellState::Unknown => self.config.unknown_cost,
135            AdaptiveCostmapCellState::StaticObstacle => self.config.static_obstacle_cost,
136            AdaptiveCostmapCellState::MovableObstacle => self.config.movable_initial_cost,
137        };
138        self.cells[x as usize][y as usize] = AdaptiveCostmapCell { state, cost };
139        Ok(())
140    }
141
142    pub fn mark_movable_obstacle(&mut self, x: i32, y: i32) -> RoboticsResult<()> {
143        self.set_cell_state(x, y, AdaptiveCostmapCellState::MovableObstacle)
144    }
145
146    pub fn mark_static_obstacle(&mut self, x: i32, y: i32) -> RoboticsResult<()> {
147        self.set_cell_state(x, y, AdaptiveCostmapCellState::StaticObstacle)
148    }
149
150    pub fn mark_unknown(&mut self, x: i32, y: i32) -> RoboticsResult<()> {
151        self.set_cell_state(x, y, AdaptiveCostmapCellState::Unknown)
152    }
153
154    /// Update selected movable-obstacle cells from a progress observation.
155    ///
156    /// Stuck observations increase cost toward lethal. Good progress decreases
157    /// cost back toward `movable_initial_cost`.
158    pub fn update_movable_costs(
159        &mut self,
160        movable_cells: &[(i32, i32)],
161        observation: MotionProgressObservation,
162    ) -> RoboticsResult<usize> {
163        validate_observation(&observation)?;
164        let stuck = self.is_stuck(observation);
165        let progressing = self.is_progressing(observation);
166        let mut changed = 0;
167
168        for &(x, y) in movable_cells {
169            self.validate_cell(x, y)?;
170            let cell = &mut self.cells[x as usize][y as usize];
171            if cell.state != AdaptiveCostmapCellState::MovableObstacle {
172                continue;
173            }
174
175            let old_cost = cell.cost;
176            if stuck {
177                cell.cost =
178                    (cell.cost + self.config.movable_cost_increment).min(self.config.lethal_cost);
179            } else if progressing {
180                cell.cost = (cell.cost - self.config.movable_cost_decrement)
181                    .max(self.config.movable_initial_cost);
182            }
183            if (cell.cost - old_cost).abs() > 1e-9 {
184                changed += 1;
185            }
186        }
187
188        Ok(changed)
189    }
190
191    /// Convert the adaptive costmap into traversal-risk cells.
192    pub fn to_traversal_risk_cells(&self, block_lethal_movable: bool) -> Vec<Vec<TerrainRiskCell>> {
193        let mut terrain = vec![
194            vec![TerrainRiskCell::free(); self.config.height as usize];
195            self.config.width as usize
196        ];
197        // x and y index both `self.cells` and `terrain`, so an enumerate-based
198        // rewrite would be less clear than the explicit grid sweep.
199        #[allow(clippy::needless_range_loop)]
200        for x in 0..self.config.width as usize {
201            for y in 0..self.config.height as usize {
202                let cell = self.cells[x][y];
203                terrain[x][y] = match cell.state {
204                    AdaptiveCostmapCellState::Free => TerrainRiskCell::free(),
205                    AdaptiveCostmapCellState::Unknown => TerrainRiskCell {
206                        blocked: false,
207                        traversability_risk: 0.0,
208                        stability_risk: 0.0,
209                        exposure_risk: normalize_cost(cell.cost, self.config.lethal_cost),
210                    },
211                    AdaptiveCostmapCellState::StaticObstacle => TerrainRiskCell::blocked(),
212                    AdaptiveCostmapCellState::MovableObstacle => TerrainRiskCell {
213                        blocked: block_lethal_movable && cell.cost >= self.config.lethal_cost,
214                        traversability_risk: 0.0,
215                        stability_risk: 0.0,
216                        exposure_risk: normalize_cost(cell.cost, self.config.lethal_cost),
217                    },
218                };
219            }
220        }
221        terrain
222    }
223
224    fn validate_cell(&self, x: i32, y: i32) -> RoboticsResult<()> {
225        if x >= 0 && y >= 0 && x < self.config.width && y < self.config.height {
226            Ok(())
227        } else {
228            Err(RoboticsError::InvalidParameter(format!(
229                "cell ({}, {}) is out of bounds ({}x{})",
230                x, y, self.config.width, self.config.height
231            )))
232        }
233    }
234
235    fn is_stuck(&self, observation: MotionProgressObservation) -> bool {
236        observation.commanded_speed >= self.config.stuck_command_speed
237            && observation.actual_speed
238                <= observation.commanded_speed * self.config.stuck_actual_speed_ratio
239            && observation.odom_delta < self.config.progress_distance
240    }
241
242    fn is_progressing(&self, observation: MotionProgressObservation) -> bool {
243        observation.commanded_speed >= self.config.stuck_command_speed
244            && (observation.actual_speed
245                > observation.commanded_speed * self.config.stuck_actual_speed_ratio
246                || observation.odom_delta >= self.config.progress_distance)
247    }
248}
249
250fn validate_config(config: &AdaptiveCostmapNamoConfig) -> RoboticsResult<()> {
251    if config.width <= 0 || config.height <= 0 {
252        return Err(RoboticsError::InvalidParameter(
253            "width and height must be positive".to_string(),
254        ));
255    }
256    for (label, value, allow_zero) in [
257        ("unknown_cost", config.unknown_cost, true),
258        ("movable_initial_cost", config.movable_initial_cost, true),
259        (
260            "movable_cost_increment",
261            config.movable_cost_increment,
262            false,
263        ),
264        (
265            "movable_cost_decrement",
266            config.movable_cost_decrement,
267            false,
268        ),
269        ("static_obstacle_cost", config.static_obstacle_cost, false),
270        ("lethal_cost", config.lethal_cost, false),
271        ("stuck_command_speed", config.stuck_command_speed, false),
272        (
273            "stuck_actual_speed_ratio",
274            config.stuck_actual_speed_ratio,
275            true,
276        ),
277        ("progress_distance", config.progress_distance, true),
278    ] {
279        if !value.is_finite() || value < 0.0 || (!allow_zero && value == 0.0) {
280            return Err(RoboticsError::InvalidParameter(format!(
281                "{} must be finite and {}",
282                label,
283                if allow_zero {
284                    "non-negative"
285                } else {
286                    "positive"
287                }
288            )));
289        }
290    }
291    if config.movable_initial_cost > config.lethal_cost
292        || config.unknown_cost > config.lethal_cost
293        || config.static_obstacle_cost > config.lethal_cost
294    {
295        return Err(RoboticsError::InvalidParameter(
296            "initial/static/unknown costs must not exceed lethal_cost".to_string(),
297        ));
298    }
299    Ok(())
300}
301
302fn validate_observation(observation: &MotionProgressObservation) -> RoboticsResult<()> {
303    for (label, value) in [
304        ("commanded_speed", observation.commanded_speed),
305        ("actual_speed", observation.actual_speed),
306        ("odom_delta", observation.odom_delta),
307    ] {
308        if value < 0.0 || !value.is_finite() {
309            return Err(RoboticsError::InvalidParameter(format!(
310                "{} must be finite and non-negative",
311                label
312            )));
313        }
314    }
315    Ok(())
316}
317
318fn normalize_cost(cost: f64, lethal_cost: f64) -> f64 {
319    if lethal_cost <= 0.0 {
320        0.0
321    } else {
322        (cost / lethal_cost).clamp(0.0, 1.0)
323    }
324}
325
326#[cfg(test)]
327mod tests {
328    use super::*;
329
330    fn costmap() -> AdaptiveCostmapNamo {
331        AdaptiveCostmapNamo::new(AdaptiveCostmapNamoConfig::new(6, 4)).unwrap()
332    }
333
334    #[test]
335    fn stuck_observation_raises_movable_cost() {
336        let mut map = costmap();
337        map.mark_movable_obstacle(2, 1).unwrap();
338
339        let changed = map
340            .update_movable_costs(&[(2, 1)], MotionProgressObservation::stuck(0.5))
341            .unwrap();
342
343        assert_eq!(changed, 1);
344        assert_eq!(map.cell(2, 1).unwrap().cost, 50.0);
345    }
346
347    #[test]
348    fn cost_caps_at_lethal() {
349        let mut map = costmap();
350        map.mark_movable_obstacle(2, 1).unwrap();
351        for _ in 0..5 {
352            map.update_movable_costs(&[(2, 1)], MotionProgressObservation::stuck(0.5))
353                .unwrap();
354        }
355
356        assert_eq!(map.cell(2, 1).unwrap().cost, 100.0);
357    }
358
359    #[test]
360    fn progress_observation_decays_to_initial_cost() {
361        let mut map = costmap();
362        map.mark_movable_obstacle(2, 1).unwrap();
363        map.update_movable_costs(&[(2, 1)], MotionProgressObservation::stuck(0.5))
364            .unwrap();
365        map.update_movable_costs(&[(2, 1)], MotionProgressObservation::moving(0.5, 0.4, 0.2))
366            .unwrap();
367        map.update_movable_costs(&[(2, 1)], MotionProgressObservation::moving(0.5, 0.4, 0.2))
368            .unwrap();
369        map.update_movable_costs(&[(2, 1)], MotionProgressObservation::moving(0.5, 0.4, 0.2))
370            .unwrap();
371
372        assert_eq!(map.cell(2, 1).unwrap().cost, 20.0);
373    }
374
375    #[test]
376    fn non_movable_cells_are_not_adapted() {
377        let mut map = costmap();
378        map.mark_unknown(2, 1).unwrap();
379        let changed = map
380            .update_movable_costs(&[(2, 1)], MotionProgressObservation::stuck(0.5))
381            .unwrap();
382
383        assert_eq!(changed, 0);
384        assert_eq!(
385            map.cell(2, 1).unwrap().state,
386            AdaptiveCostmapCellState::Unknown
387        );
388    }
389
390    #[test]
391    fn traversal_risk_conversion_blocks_static_and_lethal_movable() {
392        let mut map = costmap();
393        map.mark_static_obstacle(1, 1).unwrap();
394        map.mark_movable_obstacle(2, 1).unwrap();
395        for _ in 0..3 {
396            map.update_movable_costs(&[(2, 1)], MotionProgressObservation::stuck(0.5))
397                .unwrap();
398        }
399        let terrain = map.to_traversal_risk_cells(true);
400
401        assert!(terrain[1][1].blocked);
402        assert!(terrain[2][1].blocked);
403        assert_eq!(terrain[2][1].exposure_risk, 1.0);
404    }
405
406    #[test]
407    fn rejects_invalid_config_and_coordinates() {
408        let config = AdaptiveCostmapNamoConfig::new(0, 4);
409        assert!(matches!(
410            AdaptiveCostmapNamo::new(config),
411            Err(RoboticsError::InvalidParameter(_))
412        ));
413        let mut map = costmap();
414        assert!(matches!(
415            map.mark_movable_obstacle(10, 1),
416            Err(RoboticsError::InvalidParameter(_))
417        ));
418    }
419}
rust_robotics_planning/adaptive_costmap_namo.rs

rust_robotics_planning/
adaptive_costmap_namo.rs
