rust_robotics_planning/
moving_ai.rs

1//! Loader utilities for the Moving AI Lab grid benchmark formats.
2//!
3//! These helpers parse `.map` and `.scen` files and convert them into the
4//! occupancy-grid obstacle format used by the existing planners.
5
6use std::fs;
7use std::path::Path;
8
9use rust_robotics_core::{Obstacles, Point2D, RoboticsError, RoboticsResult};
10
11/// Parsed Moving AI Lab map.
12#[derive(Debug, Clone, PartialEq, Eq)]
13pub struct MovingAiMap {
14    pub width: usize,
15    pub height: usize,
16    tiles: Vec<Vec<char>>,
17}
18
19impl MovingAiMap {
20    /// Parse a MovingAI `.map` payload.
21    pub fn parse_str(input: &str) -> RoboticsResult<Self> {
22        let mut lines = input
23            .lines()
24            .map(str::trim_end)
25            .filter(|line| !line.trim().is_empty());
26
27        let map_type = lines.next().ok_or_else(|| {
28            RoboticsError::InvalidParameter("MovingAI map is missing the type header".to_string())
29        })?;
30        if map_type.trim() != "type octile" {
31            return Err(RoboticsError::InvalidParameter(format!(
32                "unsupported MovingAI map type {:?}, expected \"type octile\"",
33                map_type
34            )));
35        }
36
37        let height = parse_dimension(
38            lines.next().ok_or_else(|| {
39                RoboticsError::InvalidParameter(
40                    "MovingAI map is missing the height header".to_string(),
41                )
42            })?,
43            "height",
44        )?;
45        let width = parse_dimension(
46            lines.next().ok_or_else(|| {
47                RoboticsError::InvalidParameter(
48                    "MovingAI map is missing the width header".to_string(),
49                )
50            })?,
51            "width",
52        )?;
53
54        let map_marker = lines.next().ok_or_else(|| {
55            RoboticsError::InvalidParameter("MovingAI map is missing the map marker".to_string())
56        })?;
57        if map_marker.trim() != "map" {
58            return Err(RoboticsError::InvalidParameter(format!(
59                "expected \"map\" marker, got {:?}",
60                map_marker
61            )));
62        }
63
64        let mut tiles = Vec::with_capacity(height);
65        for row_index in 0..height {
66            let row = lines.next().ok_or_else(|| {
67                RoboticsError::InvalidParameter(format!(
68                    "MovingAI map ended early at row {} of {}",
69                    row_index, height
70                ))
71            })?;
72            let chars: Vec<char> = row.chars().collect();
73            if chars.len() != width {
74                return Err(RoboticsError::InvalidParameter(format!(
75                    "row {} has width {}, expected {}",
76                    row_index,
77                    chars.len(),
78                    width
79                )));
80            }
81            for tile in &chars {
82                if !is_supported_tile(*tile) {
83                    return Err(RoboticsError::InvalidParameter(format!(
84                        "unsupported MovingAI tile {:?} at row {}",
85                        tile, row_index
86                    )));
87                }
88            }
89            tiles.push(chars);
90        }
91
92        Ok(Self {
93            width,
94            height,
95            tiles,
96        })
97    }
98
99    /// Load a MovingAI `.map` file from disk.
100    pub fn from_file(path: impl AsRef<Path>) -> RoboticsResult<Self> {
101        let content = fs::read_to_string(path)?;
102        Self::parse_str(&content)
103    }
104
105    /// Return whether a tile is passable in the occupancy-only approximation.
106    pub fn is_passable(&self, x: usize, y: usize) -> RoboticsResult<bool> {
107        let tile = self.tile_at(x, y)?;
108        Ok(matches!(tile, '.' | 'G' | 'S' | 'W'))
109    }
110
111    /// Convert the map into obstacle points for the existing planners.
112    ///
113    /// The generated obstacle set adds a one-cell border so edge cells remain
114    /// addressable while outside-the-map coordinates stay invalid.
115    pub fn to_obstacles(&self) -> Obstacles {
116        let mut obstacles = Obstacles::new();
117        let width = self.width as i32;
118        let height = self.height as i32;
119
120        for x in 0..=width + 1 {
121            obstacles.push(Point2D::new(x as f64, 0.0));
122            obstacles.push(Point2D::new(x as f64, (height + 1) as f64));
123        }
124        for y in 0..=height + 1 {
125            obstacles.push(Point2D::new(0.0, y as f64));
126            obstacles.push(Point2D::new((width + 1) as f64, y as f64));
127        }
128
129        for y in 0..self.height {
130            for x in 0..self.width {
131                if !self.is_passable(x, y).expect("bounds checked by iteration") {
132                    obstacles.push(Point2D::new((x + 1) as f64, (y + 1) as f64));
133                }
134            }
135        }
136
137        obstacles
138    }
139
140    /// Translate a MovingAI grid coordinate into planner world coordinates.
141    pub fn planning_point(&self, x: usize, y: usize) -> RoboticsResult<Point2D> {
142        if !self.is_passable(x, y)? {
143            return Err(RoboticsError::InvalidParameter(format!(
144                "scenario point ({}, {}) is not traversable",
145                x, y
146            )));
147        }
148        Ok(Point2D::new((x + 1) as f64, (y + 1) as f64))
149    }
150
151    fn tile_at(&self, x: usize, y: usize) -> RoboticsResult<char> {
152        if x >= self.width || y >= self.height {
153            return Err(RoboticsError::InvalidParameter(format!(
154                "tile coordinate ({}, {}) is out of bounds for {}x{} map",
155                x, y, self.width, self.height
156            )));
157        }
158        Ok(self.tiles[y][x])
159    }
160}
161
162/// One scenario row from a MovingAI `.scen` file.
163#[derive(Debug, Clone, PartialEq)]
164pub struct MovingAiScenario {
165    pub bucket: u32,
166    pub map: String,
167    pub width: usize,
168    pub height: usize,
169    pub start_x: usize,
170    pub start_y: usize,
171    pub goal_x: usize,
172    pub goal_y: usize,
173    pub optimal_length: f64,
174}
175
176impl MovingAiScenario {
177    /// Parse a MovingAI `.scen` payload.
178    pub fn parse_str(input: &str) -> RoboticsResult<Vec<Self>> {
179        let mut lines = input.lines().map(str::trim).filter(|line| !line.is_empty());
180
181        let version = lines.next().ok_or_else(|| {
182            RoboticsError::InvalidParameter(
183                "MovingAI scenario file is missing the version header".to_string(),
184            )
185        })?;
186        if !version.starts_with("version ") {
187            return Err(RoboticsError::InvalidParameter(format!(
188                "expected scenario version header, got {:?}",
189                version
190            )));
191        }
192
193        let mut scenarios = Vec::new();
194        for (line_index, line) in lines.enumerate() {
195            let fields: Vec<&str> = line.split_whitespace().collect();
196            if fields.len() != 9 {
197                return Err(RoboticsError::InvalidParameter(format!(
198                    "scenario line {} has {} fields, expected 9",
199                    line_index + 2,
200                    fields.len()
201                )));
202            }
203
204            let scenario = Self {
205                bucket: parse_integer(fields[0], "bucket", line_index)?,
206                map: fields[1].to_string(),
207                width: parse_integer(fields[2], "map width", line_index)?,
208                height: parse_integer(fields[3], "map height", line_index)?,
209                start_x: parse_integer(fields[4], "start x", line_index)?,
210                start_y: parse_integer(fields[5], "start y", line_index)?,
211                goal_x: parse_integer(fields[6], "goal x", line_index)?,
212                goal_y: parse_integer(fields[7], "goal y", line_index)?,
213                optimal_length: parse_float(fields[8], "optimal length", line_index)?,
214            };
215            scenarios.push(scenario);
216        }
217
218        if scenarios.is_empty() {
219            return Err(RoboticsError::InvalidParameter(
220                "scenario file does not contain any scenario rows".to_string(),
221            ));
222        }
223
224        Ok(scenarios)
225    }
226
227    /// Load MovingAI scenarios from disk.
228    pub fn from_file(path: impl AsRef<Path>) -> RoboticsResult<Vec<Self>> {
229        let content = fs::read_to_string(path)?;
230        Self::parse_str(&content)
231    }
232}
233
234fn parse_dimension(line: &str, field: &str) -> RoboticsResult<usize> {
235    let value = line
236        .strip_prefix(&format!("{field} "))
237        .ok_or_else(|| {
238            RoboticsError::InvalidParameter(format!("expected {:?} header, got {:?}", field, line))
239        })?
240        .trim();
241    value.parse::<usize>().map_err(|err| {
242        RoboticsError::InvalidParameter(format!(
243            "failed to parse {} from {:?}: {}",
244            field, line, err
245        ))
246    })
247}
248
249fn parse_integer<T>(value: &str, field: &str, line_index: usize) -> RoboticsResult<T>
250where
251    T: std::str::FromStr,
252    T::Err: std::fmt::Display,
253{
254    value.parse::<T>().map_err(|err| {
255        RoboticsError::InvalidParameter(format!(
256            "failed to parse {} on scenario line {}: {}",
257            field,
258            line_index + 2,
259            err
260        ))
261    })
262}
263
264fn parse_float(value: &str, field: &str, line_index: usize) -> RoboticsResult<f64> {
265    let parsed = value.parse::<f64>().map_err(|err| {
266        RoboticsError::InvalidParameter(format!(
267            "failed to parse {} on scenario line {}: {}",
268            field,
269            line_index + 2,
270            err
271        ))
272    })?;
273    if !parsed.is_finite() {
274        return Err(RoboticsError::InvalidParameter(format!(
275            "{} on scenario line {} must be finite",
276            field,
277            line_index + 2
278        )));
279    }
280    Ok(parsed)
281}
282
283fn is_supported_tile(tile: char) -> bool {
284    matches!(tile, '.' | 'G' | '@' | 'O' | 'T' | 'S' | 'W')
285}
286
287#[cfg(test)]
288mod tests {
289    use super::*;
290    use crate::a_star::{AStarConfig, AStarPlanner};
291
292    const SAMPLE_MAP: &str = include_str!("testdata/moving_ai/sample.map");
293    const SAMPLE_SCEN: &str = include_str!("testdata/moving_ai/sample.map.scen");
294
295    #[test]
296    #[ignore = "long-running MovingAI benchmark"]
297    fn parses_sample_map() {
298        let map = MovingAiMap::parse_str(SAMPLE_MAP).expect("sample map should parse");
299
300        assert_eq!(map.width, 5);
301        assert_eq!(map.height, 5);
302        assert!(map.is_passable(0, 0).unwrap());
303        assert!(!map.is_passable(2, 0).unwrap());
304        assert!(!map.is_passable(2, 1).unwrap());
305        assert!(!map.is_passable(2, 2).unwrap());
306        assert!(map.is_passable(1, 1).unwrap());
307        assert!(map.is_passable(2, 3).unwrap());
308    }
309
310    #[test]
311    fn parses_sample_scenarios() {
312        let scenarios =
313            MovingAiScenario::parse_str(SAMPLE_SCEN).expect("sample scenario should parse");
314
315        assert_eq!(scenarios.len(), 2);
316        assert_eq!(scenarios[0].map, "sample.map");
317        assert_eq!(scenarios[0].start_x, 0);
318        assert_eq!(scenarios[0].goal_y, 4);
319        assert!((scenarios[1].optimal_length - std::f64::consts::SQRT_2).abs() < 1e-9);
320    }
321
322    #[test]
323    #[ignore = "long-running MovingAI benchmark"]
324    fn sample_scenario_is_solved_by_astar() {
325        let map = MovingAiMap::parse_str(SAMPLE_MAP).expect("sample map should parse");
326        let scenario = MovingAiScenario::parse_str(SAMPLE_SCEN)
327            .expect("sample scenario should parse")
328            .into_iter()
329            .next()
330            .expect("sample scenario should contain at least one row");
331
332        let planner = AStarPlanner::from_obstacle_points(
333            &map.to_obstacles(),
334            AStarConfig {
335                resolution: 1.0,
336                robot_radius: 0.5,
337                heuristic_weight: 1.0,
338            },
339        )
340        .expect("A* planner should build from sample map");
341
342        let path = planner
343            .plan(
344                map.planning_point(scenario.start_x, scenario.start_y)
345                    .expect("sample start should be valid"),
346                map.planning_point(scenario.goal_x, scenario.goal_y)
347                    .expect("sample goal should be valid"),
348            )
349            .expect("sample scenario should be solvable");
350
351        assert!(!path.is_empty());
352        assert!((path.total_length() - scenario.optimal_length).abs() < 1e-9);
353    }
354}
rust_robotics_planning/moving_ai.rs

rust_robotics_planning/
moving_ai.rs
