1use std::cmp::Ordering;
7use std::collections::{BinaryHeap, HashMap};
8
9use crate::grid::{GridMap, Node};
10use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
11
12#[derive(Debug, Clone)]
14pub struct AStarConfig {
15 pub resolution: f64,
17 pub robot_radius: f64,
19 pub heuristic_weight: f64,
22}
23
24impl Default for AStarConfig {
25 fn default() -> Self {
26 Self {
27 resolution: 1.0,
28 robot_radius: 0.5,
29 heuristic_weight: 1.0,
30 }
31 }
32}
33
34impl AStarConfig {
35 pub fn validate(&self) -> RoboticsResult<()> {
36 if !self.resolution.is_finite() || self.resolution <= 0.0 {
37 return Err(RoboticsError::InvalidParameter(format!(
38 "resolution must be positive and finite, got {}",
39 self.resolution
40 )));
41 }
42 if !self.robot_radius.is_finite() || self.robot_radius < 0.0 {
43 return Err(RoboticsError::InvalidParameter(format!(
44 "robot_radius must be non-negative and finite, got {}",
45 self.robot_radius
46 )));
47 }
48 if !self.heuristic_weight.is_finite() || self.heuristic_weight < 0.0 {
49 return Err(RoboticsError::InvalidParameter(format!(
50 "heuristic_weight must be non-negative and finite, got {}",
51 self.heuristic_weight
52 )));
53 }
54
55 Ok(())
56 }
57}
58
59#[derive(Debug)]
61struct PriorityNode {
62 x: i32,
63 y: i32,
64 cost: f64,
65 priority: f64,
66 index: usize,
67}
68
69impl Eq for PriorityNode {}
70
71impl PartialEq for PriorityNode {
72 fn eq(&self, other: &Self) -> bool {
73 self.priority == other.priority
74 }
75}
76
77impl Ord for PriorityNode {
78 fn cmp(&self, other: &Self) -> Ordering {
79 other
81 .priority
82 .partial_cmp(&self.priority)
83 .unwrap_or(Ordering::Equal)
84 }
85}
86
87impl PartialOrd for PriorityNode {
88 fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
89 Some(self.cmp(other))
90 }
91}
92
93pub struct AStarPlanner {
95 grid_map: GridMap,
96 config: AStarConfig,
97 motion: Vec<(i32, i32, f64)>,
98}
99
100#[derive(Debug, Clone, Copy, Default, PartialEq, Eq)]
102pub struct AStarSearchStats {
103 pub expanded_nodes: usize,
104 pub generated_nodes: usize,
105 pub skipped_closed_nodes: usize,
106 pub max_frontier_len: usize,
107}
108
109impl AStarPlanner {
110 pub fn new(ox: &[f64], oy: &[f64], config: AStarConfig) -> Self {
112 Self::try_new(ox, oy, config).expect(
113 "invalid A* planner input: obstacle list must be non-empty and valid, and config values must be positive/finite",
114 )
115 }
116
117 pub fn try_new(ox: &[f64], oy: &[f64], config: AStarConfig) -> RoboticsResult<Self> {
119 config.validate()?;
120 let grid_map = GridMap::try_new(ox, oy, config.resolution, config.robot_radius)?;
121 let motion = Self::get_motion_model();
122
123 Ok(AStarPlanner {
124 grid_map,
125 config,
126 motion,
127 })
128 }
129
130 pub fn from_obstacles(ox: &[f64], oy: &[f64], resolution: f64, robot_radius: f64) -> Self {
132 let config = AStarConfig {
133 resolution,
134 robot_radius,
135 ..Default::default()
136 };
137 Self::new(ox, oy, config)
138 }
139
140 pub fn from_obstacle_points(
142 obstacles: &Obstacles,
143 config: AStarConfig,
144 ) -> RoboticsResult<Self> {
145 config.validate()?;
146 let grid_map = GridMap::from_obstacles(obstacles, config.resolution, config.robot_radius)?;
147 let motion = Self::get_motion_model();
148
149 Ok(AStarPlanner {
150 grid_map,
151 config,
152 motion,
153 })
154 }
155
156 #[deprecated(note = "use plan() or plan_xy() instead")]
158 pub fn planning(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> Option<(Vec<f64>, Vec<f64>)> {
159 match self.plan_xy(sx, sy, gx, gy) {
160 Ok(path) => Some((path.x_coords(), path.y_coords())),
161 Err(_) => None,
162 }
163 }
164
165 pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
167 self.plan_impl(start, goal).map(|(path, _stats)| path)
168 }
169
170 pub fn plan_xy(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> RoboticsResult<Path2D> {
172 self.plan_impl(Point2D::new(sx, sy), Point2D::new(gx, gy))
173 .map(|(path, _stats)| path)
174 }
175
176 pub fn plan_with_stats(
178 &self,
179 start: Point2D,
180 goal: Point2D,
181 ) -> RoboticsResult<(Path2D, AStarSearchStats)> {
182 self.plan_impl(start, goal)
183 }
184
185 pub fn plan_with_observer(
192 &self,
193 start: Point2D,
194 goal: Point2D,
195 mut on_expand: impl FnMut(Point2D),
196 ) -> RoboticsResult<Path2D> {
197 self.search(start, goal, &mut on_expand)
198 .map(|(path, _stats)| path)
199 }
200
201 pub fn grid_map(&self) -> &GridMap {
203 &self.grid_map
204 }
205
206 fn calc_heuristic(&self, n1_x: i32, n1_y: i32, n2_x: i32, n2_y: i32) -> f64 {
207 self.config.heuristic_weight * (((n1_x - n2_x).pow(2) + (n1_y - n2_y).pow(2)) as f64).sqrt()
208 }
209
210 fn get_motion_model() -> Vec<(i32, i32, f64)> {
211 vec![
213 (1, 0, 1.0),
214 (0, 1, 1.0),
215 (-1, 0, 1.0),
216 (0, -1, 1.0),
217 (-1, -1, std::f64::consts::SQRT_2),
218 (-1, 1, std::f64::consts::SQRT_2),
219 (1, -1, std::f64::consts::SQRT_2),
220 (1, 1, std::f64::consts::SQRT_2),
221 ]
222 }
223
224 fn build_path(&self, goal_index: usize, node_storage: &[Node]) -> Path2D {
225 let mut points = Vec::new();
226 let mut current_index = Some(goal_index);
227
228 while let Some(index) = current_index {
229 let node = &node_storage[index];
230 points.push(Point2D::new(
231 self.grid_map.calc_x_position(node.x),
232 self.grid_map.calc_y_position(node.y),
233 ));
234 current_index = node.parent_index;
235 }
236
237 points.reverse();
238 Path2D::from_points(points)
239 }
240
241 fn ensure_query_is_valid(&self, x: i32, y: i32, label: &str) -> RoboticsResult<()> {
242 if self.grid_map.is_valid(x, y) {
243 return Ok(());
244 }
245
246 Err(RoboticsError::PlanningError(format!(
247 "{} position is invalid",
248 label
249 )))
250 }
251
252 fn plan_impl(
253 &self,
254 start: Point2D,
255 goal: Point2D,
256 ) -> RoboticsResult<(Path2D, AStarSearchStats)> {
257 self.search(start, goal, &mut |_| {})
258 }
259
260 fn search(
261 &self,
262 start: Point2D,
263 goal: Point2D,
264 on_expand: &mut impl FnMut(Point2D),
265 ) -> RoboticsResult<(Path2D, AStarSearchStats)> {
266 let start_x = self.grid_map.calc_x_index(start.x);
267 let start_y = self.grid_map.calc_y_index(start.y);
268 let goal_x = self.grid_map.calc_x_index(goal.x);
269 let goal_y = self.grid_map.calc_y_index(goal.y);
270
271 self.ensure_query_is_valid(start_x, start_y, "Start")?;
272 self.ensure_query_is_valid(goal_x, goal_y, "Goal")?;
273
274 let mut open_set = BinaryHeap::new();
275 let mut closed_set = HashMap::new();
276 let mut node_storage: Vec<Node> = Vec::new();
277 let mut stats = AStarSearchStats::default();
278
279 node_storage.push(Node::new(start_x, start_y, 0.0, None));
281 let start_index = 0;
282
283 open_set.push(PriorityNode {
284 x: start_x,
285 y: start_y,
286 cost: 0.0,
287 priority: self.calc_heuristic(start_x, start_y, goal_x, goal_y),
288 index: start_index,
289 });
290 stats.max_frontier_len = open_set.len();
291
292 while let Some(current) = open_set.pop() {
293 let current_grid_index = self.grid_map.calc_index(current.x, current.y);
294
295 if closed_set.contains_key(¤t_grid_index) {
297 stats.skipped_closed_nodes += 1;
298 continue;
299 }
300
301 stats.expanded_nodes += 1;
302 on_expand(Point2D::new(
303 self.grid_map.calc_x_position(current.x),
304 self.grid_map.calc_y_position(current.y),
305 ));
306
307 if current.x == goal_x && current.y == goal_y {
309 return Ok((self.build_path(current.index, &node_storage), stats));
310 }
311
312 closed_set.insert(current_grid_index, current.index);
314
315 for &(dx, dy, move_cost) in &self.motion {
317 let new_x = current.x + dx;
318 let new_y = current.y + dy;
319 let new_cost = current.cost + move_cost;
320 let new_grid_index = self.grid_map.calc_index(new_x, new_y);
321
322 if !self.grid_map.is_valid_offset(current.x, current.y, dx, dy) {
324 continue;
325 }
326 if closed_set.contains_key(&new_grid_index) {
327 continue;
328 }
329
330 node_storage.push(Node::new(new_x, new_y, new_cost, Some(current.index)));
332 let new_index = node_storage.len() - 1;
333 stats.generated_nodes += 1;
334
335 let priority = new_cost + self.calc_heuristic(new_x, new_y, goal_x, goal_y);
336 open_set.push(PriorityNode {
337 x: new_x,
338 y: new_y,
339 cost: new_cost,
340 priority,
341 index: new_index,
342 });
343 stats.max_frontier_len = stats.max_frontier_len.max(open_set.len());
344 }
345 }
346
347 Err(RoboticsError::PlanningError("No path found".to_string()))
348 }
349}
350
351impl PathPlanner for AStarPlanner {
352 fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
353 self.plan_impl(start, goal).map(|(path, _stats)| path)
354 }
355}
356
357#[cfg(test)]
358mod tests {
359 use super::*;
360 use crate::moving_ai::{MovingAiMap, MovingAiScenario};
361
362 use rust_robotics_core::Obstacles;
363
364 fn create_simple_obstacles() -> (Vec<f64>, Vec<f64>) {
365 let mut ox = Vec::new();
366 let mut oy = Vec::new();
367
368 for i in 0..11 {
370 ox.push(i as f64);
371 oy.push(0.0);
372 ox.push(i as f64);
373 oy.push(10.0);
374 ox.push(0.0);
375 oy.push(i as f64);
376 ox.push(10.0);
377 oy.push(i as f64);
378 }
379
380 for i in 4..7 {
382 ox.push(5.0);
383 oy.push(i as f64);
384 }
385
386 (ox, oy)
387 }
388
389 #[test]
390 fn test_a_star_finds_path() {
391 let (ox, oy) = create_simple_obstacles();
392 let planner = AStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
393
394 let start = Point2D::new(2.0, 2.0);
395 let goal = Point2D::new(8.0, 8.0);
396
397 let result = planner.plan(start, goal);
398 assert!(result.is_ok());
399
400 let path = result.unwrap();
401 assert!(!path.is_empty());
402 }
403
404 #[test]
405 #[allow(deprecated)]
406 fn test_a_star_legacy_interface() {
407 let (ox, oy) = create_simple_obstacles();
408 let planner = AStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
409
410 let result = planner.planning(2.0, 2.0, 8.0, 8.0);
411 assert!(result.is_some());
412
413 let (rx, ry) = result.unwrap();
414 assert!(!rx.is_empty());
415 assert_eq!(rx.len(), ry.len());
416 }
417
418 #[test]
419 fn test_a_star_from_obstacle_points() {
420 let (ox, oy) = create_simple_obstacles();
421 let obstacles = Obstacles::try_from_xy(&ox, &oy).unwrap();
422 let planner =
423 AStarPlanner::from_obstacle_points(&obstacles, AStarConfig::default()).unwrap();
424
425 let path = planner.plan_xy(2.0, 2.0, 8.0, 8.0).unwrap();
426 assert!(!path.is_empty());
427 }
428
429 #[test]
430 fn test_a_star_try_new_rejects_invalid_config() {
431 let (ox, oy) = create_simple_obstacles();
432 let config = AStarConfig {
433 heuristic_weight: -1.0,
434 ..Default::default()
435 };
436
437 let err = match AStarPlanner::try_new(&ox, &oy, config) {
438 Ok(_) => panic!("expected invalid config to be rejected"),
439 Err(err) => err,
440 };
441 assert!(matches!(err, RoboticsError::InvalidParameter(_)));
442
443 let dijkstra = AStarConfig {
445 heuristic_weight: 0.0,
446 ..Default::default()
447 };
448 assert!(AStarPlanner::try_new(&ox, &oy, dijkstra).is_ok());
449 }
450
451 #[test]
452 fn test_a_star_preserves_asymmetric_world_coordinates() {
453 let mut obstacles = Obstacles::new();
454
455 for x in 10..=20 {
456 obstacles.push(Point2D::new(x as f64, -4.0));
457 obstacles.push(Point2D::new(x as f64, 6.0));
458 }
459 for y in -4..=6 {
460 obstacles.push(Point2D::new(10.0, y as f64));
461 obstacles.push(Point2D::new(20.0, y as f64));
462 }
463
464 let planner =
465 AStarPlanner::from_obstacle_points(&obstacles, AStarConfig::default()).unwrap();
466 let path = planner.plan_xy(12.0, -2.0, 18.0, 4.0).unwrap();
467
468 let first = path.points.first().unwrap();
469 let last = path.points.last().unwrap();
470 assert!((first.x - 12.0).abs() < 1e-6);
471 assert!((first.y + 2.0).abs() < 1e-6);
472 assert!((last.x - 18.0).abs() < 1e-6);
473 assert!((last.y - 4.0).abs() < 1e-6);
474 }
475
476 #[test]
477 #[ignore = "long-running MovingAI benchmark"]
478 fn test_a_star_matches_moving_ai_arena2_bucket_80_optimal_length() {
479 let map = MovingAiMap::parse_str(include_str!("../benchdata/moving_ai/dao/arena2.map"))
480 .expect("arena2 MovingAI map should parse");
481 let scenario =
482 MovingAiScenario::parse_str(include_str!("../benchdata/moving_ai/dao/arena2.map.scen"))
483 .expect("arena2 MovingAI scenarios should parse")
484 .into_iter()
485 .find(|row| row.bucket == 80)
486 .expect("arena2 MovingAI bucket 80 scenario should exist");
487
488 let planner = AStarPlanner::from_obstacle_points(
489 &map.to_obstacles(),
490 AStarConfig {
491 resolution: 1.0,
492 robot_radius: 0.5,
493 heuristic_weight: 1.0,
494 },
495 )
496 .expect("A* planner should build from arena2 obstacles");
497
498 let start = map
499 .planning_point(scenario.start_x, scenario.start_y)
500 .expect("arena2 start should be valid");
501 let goal = map
502 .planning_point(scenario.goal_x, scenario.goal_y)
503 .expect("arena2 goal should be valid");
504
505 let path = planner
506 .plan(start, goal)
507 .expect("A* should solve the arena2 bucket 80 scenario");
508
509 assert!(
510 (path.total_length() - scenario.optimal_length).abs() < 1e-6,
511 "A* path length {} should match MovingAI optimal {} when corner cutting is disabled",
512 path.total_length(),
513 scenario.optimal_length
514 );
515 }
516}