rust_robotics_planning/
time_based_path_planning.rs

1//! Time-Based Path Planning with dynamic obstacles
2//!
3//! Implements Space-Time A\* and Safe Interval Path Planning (SIPP) on a 2-D
4//! grid where obstacles move over discrete time steps. The module also
5//! includes a priority-based multi-agent planner that sequences single-agent
6//! plans to avoid inter-agent collisions.
7//!
8//! References:
9//! - Silver (2005), "Cooperative Pathfinding"
10//! - Phillips & Likhachev (2011), "SIPP: Safe Interval Path Planning for
11//!   Dynamic Environments", ICRA 2011
12
13use std::cmp::Ordering;
14use std::collections::{BinaryHeap, HashSet};
15
16use rust_robotics_core::{RoboticsError, RoboticsResult};
17
18// ---------------------------------------------------------------------------
19// Position & Interval
20// ---------------------------------------------------------------------------
21
22/// A discrete 2-D grid position.
23#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, PartialOrd, Ord)]
24pub struct Position {
25    pub x: i32,
26    pub y: i32,
27}
28
29impl Position {
30    pub fn new(x: i32, y: i32) -> Self {
31        Self { x, y }
32    }
33
34    fn add(self, other: Self) -> Self {
35        Self {
36            x: self.x + other.x,
37            y: self.y + other.y,
38        }
39    }
40
41    fn manhattan_distance(self, other: Self) -> i32 {
42        (self.x - other.x).abs() + (self.y - other.y).abs()
43    }
44}
45
46/// A closed time interval `[start, end]`.
47#[derive(Debug, Clone, Copy, PartialEq, Eq)]
48pub struct Interval {
49    pub start_time: i32,
50    pub end_time: i32,
51}
52
53impl Interval {
54    pub fn new(start: i32, end: i32) -> Self {
55        Self {
56            start_time: start,
57            end_time: end,
58        }
59    }
60}
61
62// ---------------------------------------------------------------------------
63// Grid with dynamic obstacles
64// ---------------------------------------------------------------------------
65
66/// Describes how obstacles are arranged in the grid.
67#[derive(Debug, Clone, Copy, PartialEq, Eq)]
68pub enum ObstacleArrangement {
69    /// Obstacles start in a vertical line at the grid centre and bounce horizontally.
70    Arrangement1,
71    /// Static wall with a single corridor in the middle row.
72    NarrowCorridor,
73}
74
75/// A 2-D grid with a time dimension that records obstacle occupancy.
76///
77/// `reservation_matrix[x][y][t]` holds 0 if free, otherwise an obstacle/agent
78/// identifier.
79#[derive(Debug, Clone)]
80pub struct Grid {
81    pub width: i32,
82    pub height: i32,
83    pub time_limit: i32,
84    /// `[x][y][t]` -> occupying agent id (0 = free).
85    reservation: Vec<Vec<Vec<i32>>>,
86    /// Paths of every dynamic obstacle (list of positions per time step).
87    pub obstacle_paths: Vec<Vec<Position>>,
88}
89
90impl Grid {
91    /// Create a grid and populate it with dynamic obstacles.
92    pub fn new(
93        width: i32,
94        height: i32,
95        num_obstacles: usize,
96        arrangement: ObstacleArrangement,
97        avoid_points: &[Position],
98        time_limit: i32,
99    ) -> RoboticsResult<Self> {
100        if width <= 0 || height <= 0 || time_limit <= 0 {
101            return Err(RoboticsError::InvalidParameter(
102                "Grid dimensions and time_limit must be positive".into(),
103            ));
104        }
105
106        let mut reservation =
107            vec![vec![vec![0i32; time_limit as usize]; height as usize]; width as usize];
108
109        let obstacle_paths = match arrangement {
110            ObstacleArrangement::Arrangement1 => {
111                Self::arrangement_1(width, height, num_obstacles, time_limit, avoid_points)
112            }
113            ObstacleArrangement::NarrowCorridor => {
114                Self::narrow_corridor(width, height, num_obstacles, time_limit)
115            }
116        };
117
118        // Record obstacle paths into the reservation matrix.
119        for (i, path) in obstacle_paths.iter().enumerate() {
120            let obs_id = (i + 1) as i32;
121            for (t, &pos) in path.iter().enumerate() {
122                reservation[pos.x as usize][pos.y as usize][t] = obs_id;
123                if t > 0 {
124                    let prev = path[t - 1];
125                    reservation[prev.x as usize][prev.y as usize][t] = obs_id;
126                }
127            }
128        }
129
130        Ok(Self {
131            width,
132            height,
133            time_limit,
134            reservation,
135            obstacle_paths,
136        })
137    }
138
139    /// Create an empty grid (no obstacles).
140    pub fn empty(width: i32, height: i32, time_limit: i32) -> RoboticsResult<Self> {
141        if width <= 0 || height <= 0 || time_limit <= 0 {
142            return Err(RoboticsError::InvalidParameter(
143                "Grid dimensions and time_limit must be positive".into(),
144            ));
145        }
146        Ok(Self {
147            width,
148            height,
149            time_limit,
150            reservation: vec![
151                vec![vec![0i32; time_limit as usize]; height as usize];
152                width as usize
153            ],
154            obstacle_paths: vec![],
155        })
156    }
157
158    pub fn inside_bounds(&self, pos: Position) -> bool {
159        pos.x >= 0 && pos.x < self.width && pos.y >= 0 && pos.y < self.height
160    }
161
162    pub fn is_free(&self, pos: Position, t: i32) -> bool {
163        if !self.inside_bounds(pos) {
164            return false;
165        }
166        if t < 0 || t >= self.time_limit {
167            return false;
168        }
169        self.reservation[pos.x as usize][pos.y as usize][t as usize] == 0
170    }
171
172    /// Reserve a cell for a given agent during a time interval.
173    pub fn reserve(&mut self, pos: Position, agent_id: i32, interval: Interval) {
174        for t in interval.start_time..=interval.end_time.min(self.time_limit - 1) {
175            self.reservation[pos.x as usize][pos.y as usize][t as usize] = agent_id;
176        }
177    }
178
179    /// Reserve the entire path of an agent.
180    pub fn reserve_path(&mut self, path: &NodePath, agent_id: i32) {
181        for (i, node) in path.nodes.iter().enumerate() {
182            let end = if i + 1 < path.nodes.len() {
183                path.nodes[i + 1].time
184            } else {
185                node.time + 1
186            };
187            self.reserve(node.position, agent_id, Interval::new(node.time, end));
188        }
189    }
190
191    /// Clear all reservations for a given agent at a specific position.
192    pub fn clear_reservation(&mut self, pos: Position, agent_id: i32) {
193        for t in 0..self.time_limit as usize {
194            if self.reservation[pos.x as usize][pos.y as usize][t] == agent_id {
195                self.reservation[pos.x as usize][pos.y as usize][t] = 0;
196            }
197        }
198    }
199
200    /// Compute safe intervals for a single cell.
201    pub fn safe_intervals_at(&self, pos: Position) -> Vec<Interval> {
202        let tl = self.time_limit as usize;
203        let col = &self.reservation[pos.x as usize][pos.y as usize];
204        let mut intervals = Vec::new();
205        let mut start: Option<usize> = None;
206        for (t, &cell) in col.iter().enumerate().take(tl) {
207            if cell == 0 {
208                if start.is_none() {
209                    start = Some(t);
210                }
211            } else if let Some(s) = start {
212                if t - 1 > s {
213                    // interval with at least 2 time steps
214                    intervals.push(Interval::new(s as i32, (t - 1) as i32));
215                }
216                start = None;
217            }
218        }
219        if let Some(s) = start {
220            if tl - 1 > s {
221                intervals.push(Interval::new(s as i32, (tl - 1) as i32));
222            }
223        }
224        intervals
225    }
226
227    // -- obstacle generators -------------------------------------------------
228
229    fn arrangement_1(
230        width: i32,
231        height: i32,
232        num_obstacles: usize,
233        time_limit: i32,
234        _avoid: &[Position],
235    ) -> Vec<Vec<Position>> {
236        let half_x = width / 2;
237        let half_y = height / 2;
238        let count = num_obstacles.min(height as usize);
239        let mut paths = Vec::with_capacity(count);
240        for y_idx in 0..count as i32 {
241            let mut moving_right = y_idx < half_y;
242            let mut pos = Position::new(half_x, y_idx);
243            let mut path = vec![pos];
244            for t in 1..time_limit - 1 {
245                if t % 2 == 0 {
246                    path.push(pos);
247                    continue;
248                }
249                if (moving_right && pos.x == width - 1) || (!moving_right && pos.x == 0) {
250                    moving_right = !moving_right;
251                }
252                pos = Position::new(pos.x + if moving_right { 1 } else { -1 }, pos.y);
253                path.push(pos);
254            }
255            paths.push(path);
256        }
257        paths
258    }
259
260    fn narrow_corridor(
261        width: i32,
262        height: i32,
263        num_obstacles: usize,
264        time_limit: i32,
265    ) -> Vec<Vec<Position>> {
266        let mid_y = height / 2;
267        let x = width / 2;
268        let mut paths = Vec::new();
269        for y in 0..height.min(num_obstacles as i32) {
270            if y == mid_y {
271                continue;
272            }
273            let pos = Position::new(x, y);
274            let path = vec![pos; (time_limit - 1) as usize];
275            paths.push(path);
276        }
277        paths
278    }
279}
280
281// ---------------------------------------------------------------------------
282// Node & NodePath
283// ---------------------------------------------------------------------------
284
285/// A search node in the space-time graph.
286#[derive(Debug, Clone, Copy, Eq, PartialEq)]
287pub struct Node {
288    pub position: Position,
289    pub time: i32,
290    pub heuristic: i32,
291    pub parent_index: i32,
292}
293
294impl Node {
295    pub fn priority(&self) -> i32 {
296        self.time + self.heuristic
297    }
298}
299
300impl Ord for Node {
301    fn cmp(&self, other: &Self) -> Ordering {
302        // Reverse for min-heap in BinaryHeap
303        other.priority().cmp(&self.priority())
304    }
305}
306
307impl PartialOrd for Node {
308    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
309        Some(self.cmp(other))
310    }
311}
312
313impl std::hash::Hash for Node {
314    fn hash<H: std::hash::Hasher>(&self, state: &mut H) {
315        self.position.hash(state);
316        self.time.hash(state);
317    }
318}
319
320/// A planned path as a sequence of nodes with position-at-time lookup.
321#[derive(Debug, Clone)]
322pub struct NodePath {
323    pub nodes: Vec<Node>,
324    pub expanded_count: usize,
325}
326
327impl NodePath {
328    pub fn new(nodes: Vec<Node>, expanded_count: usize) -> Self {
329        Self {
330            nodes,
331            expanded_count,
332        }
333    }
334
335    /// Get the position of the agent at a given time (interpolated between nodes).
336    pub fn position_at(&self, time: i32) -> Option<Position> {
337        if self.nodes.is_empty() {
338            return None;
339        }
340        for (i, node) in self.nodes.iter().enumerate() {
341            let next_time = if i + 1 < self.nodes.len() {
342                self.nodes[i + 1].time
343            } else {
344                node.time + 1
345            };
346            if time >= node.time && time < next_time {
347                return Some(node.position);
348            }
349        }
350        // After the last node the agent stays at the goal
351        self.nodes.last().map(|n| n.position)
352    }
353
354    /// Time at which the goal is reached.
355    pub fn goal_reached_time(&self) -> i32 {
356        self.nodes.last().map_or(0, |n| n.time)
357    }
358}
359
360// ---------------------------------------------------------------------------
361// 4-connected + wait neighbourhood
362// ---------------------------------------------------------------------------
363
364const DIFFS: [Position; 5] = [
365    Position { x: 0, y: 0 }, // wait
366    Position { x: 1, y: 0 },
367    Position { x: -1, y: 0 },
368    Position { x: 0, y: 1 },
369    Position { x: 0, y: -1 },
370];
371
372// ---------------------------------------------------------------------------
373// Space-Time A*
374// ---------------------------------------------------------------------------
375
376/// Space-Time A\* planner for a single agent on a grid with dynamic obstacles.
377///
378/// The cost `g(n)` is the number of time steps taken to reach a node, and the
379/// heuristic is the Manhattan distance to the goal.
380pub struct SpaceTimeAStar;
381
382impl SpaceTimeAStar {
383    /// Plan a path from `start` to `goal` on the given grid.
384    pub fn plan(grid: &Grid, start: Position, goal: Position) -> RoboticsResult<NodePath> {
385        let mut open: BinaryHeap<Node> = BinaryHeap::new();
386        open.push(Node {
387            position: start,
388            time: 0,
389            heuristic: start.manhattan_distance(goal),
390            parent_index: -1,
391        });
392
393        let mut expanded_list: Vec<Node> = Vec::new();
394        let mut expanded_set: HashSet<(Position, i32)> = HashSet::new();
395
396        while let Some(node) = open.pop() {
397            if node.time + 1 >= grid.time_limit {
398                continue;
399            }
400            if node.position == goal {
401                let path = Self::reconstruct(&expanded_list, node);
402                return Ok(NodePath::new(path, expanded_set.len()));
403            }
404
405            let parent_idx = expanded_list.len() as i32;
406            expanded_list.push(node);
407            expanded_set.insert((node.position, node.time));
408
409            for &diff in &DIFFS {
410                let new_pos = node.position.add(diff);
411                let new_time = node.time + 1;
412                let new_node = Node {
413                    position: new_pos,
414                    time: new_time,
415                    heuristic: new_pos.manhattan_distance(goal),
416                    parent_index: parent_idx,
417                };
418
419                if expanded_set.contains(&(new_pos, new_time)) {
420                    continue;
421                }
422
423                // Valid for the next 2 time steps (enter + leave)
424                if grid.is_free(new_pos, new_time) && grid.is_free(new_pos, new_time + 1) {
425                    open.push(new_node);
426                }
427            }
428        }
429
430        Err(RoboticsError::PlanningError("No path found".into()))
431    }
432
433    fn reconstruct(expanded: &[Node], goal_node: Node) -> Vec<Node> {
434        let mut path = vec![goal_node];
435        let mut walker = goal_node;
436        while walker.parent_index >= 0 {
437            walker = expanded[walker.parent_index as usize];
438            path.push(walker);
439        }
440        path.reverse();
441        path
442    }
443}
444
445// ---------------------------------------------------------------------------
446// SIPP node
447// ---------------------------------------------------------------------------
448
449#[derive(Debug, Clone, Copy, Eq, PartialEq)]
450struct SippNode {
451    position: Position,
452    time: i32,
453    heuristic: i32,
454    parent_index: i32,
455    interval: Interval,
456}
457
458impl SippNode {
459    fn priority(&self) -> i32 {
460        self.time + self.heuristic
461    }
462}
463
464impl Ord for SippNode {
465    fn cmp(&self, other: &Self) -> Ordering {
466        other.priority().cmp(&self.priority())
467    }
468}
469
470impl PartialOrd for SippNode {
471    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
472        Some(self.cmp(other))
473    }
474}
475
476// ---------------------------------------------------------------------------
477// Safe Interval Path Planner
478// ---------------------------------------------------------------------------
479
480/// Safe Interval Path Planner (SIPP).
481///
482/// Reduces redundant node expansions compared to Space-Time A\* by
483/// pre-computing safe intervals at each cell.
484pub struct SafeIntervalPlanner;
485
486impl SafeIntervalPlanner {
487    /// Plan a path from `start` to `goal`.
488    pub fn plan(grid: &Grid, start: Position, goal: Position) -> RoboticsResult<NodePath> {
489        // Pre-compute safe intervals for every cell.
490        let intervals = Self::compute_all_intervals(grid);
491
492        let first_interval = intervals[start.x as usize][start.y as usize]
493            .first()
494            .copied()
495            .ok_or_else(|| {
496                RoboticsError::PlanningError("Start position has no safe interval".into())
497            })?;
498
499        let mut open: BinaryHeap<SippNode> = BinaryHeap::new();
500        open.push(SippNode {
501            position: start,
502            time: 0,
503            heuristic: start.manhattan_distance(goal),
504            parent_index: -1,
505            interval: first_interval,
506        });
507
508        let mut expanded_list: Vec<SippNode> = Vec::new();
509        // visited_intervals[x][y] -> Vec<(entry_time, interval)>
510        let mut visited: Vec<Vec<Vec<(i32, Interval)>>> =
511            vec![vec![vec![]; grid.height as usize]; grid.width as usize];
512
513        while let Some(node) = open.pop() {
514            if node.time + 1 >= grid.time_limit {
515                continue;
516            }
517            if node.position == goal {
518                let path = Self::reconstruct(&expanded_list, &node);
519                return Ok(NodePath::new(path, expanded_list.len()));
520            }
521
522            let parent_idx = expanded_list.len() as i32;
523            expanded_list.push(node);
524
525            // Mark visited
526            let vlist = &mut visited[node.position.x as usize][node.position.y as usize];
527            let already = vlist.iter_mut().find(|(_, iv)| *iv == node.interval);
528            match already {
529                Some(entry) => {
530                    entry.0 = entry.0.min(node.time);
531                }
532                None => {
533                    vlist.push((node.time, node.interval));
534                }
535            }
536
537            for child in Self::successors(grid, goal, &node, parent_idx, &intervals, &visited) {
538                open.push(child);
539            }
540        }
541
542        Err(RoboticsError::PlanningError("No path found".into()))
543    }
544
545    fn successors(
546        grid: &Grid,
547        goal: Position,
548        parent: &SippNode,
549        parent_idx: i32,
550        intervals: &[Vec<Vec<Interval>>],
551        visited: &[Vec<Vec<(i32, Interval)>>],
552    ) -> Vec<SippNode> {
553        let mut result = Vec::new();
554        for &diff in &DIFFS {
555            let new_pos = parent.position.add(diff);
556            if !grid.inside_bounds(new_pos) {
557                continue;
558            }
559            let cell_intervals = &intervals[new_pos.x as usize][new_pos.y as usize];
560            for &iv in cell_intervals {
561                if iv.start_time > parent.interval.end_time {
562                    break;
563                }
564                if iv.end_time < parent.interval.start_time {
565                    continue;
566                }
567                // Check if already visited with equal or better entry time
568                let dominated = visited[new_pos.x as usize][new_pos.y as usize]
569                    .iter()
570                    .any(|&(et, ref vis_iv)| *vis_iv == iv && et <= parent.time + 1);
571                if dominated {
572                    continue;
573                }
574                // Find earliest valid entry time
575                let t_lo = (parent.time + 1).max(iv.start_time);
576                let t_hi = parent.interval.end_time.min(iv.end_time);
577                for t in t_lo..t_hi {
578                    if grid.is_free(new_pos, t) {
579                        result.push(SippNode {
580                            position: new_pos,
581                            time: (parent.time + 1).max(iv.start_time),
582                            heuristic: new_pos.manhattan_distance(goal),
583                            parent_index: parent_idx,
584                            interval: iv,
585                        });
586                        break;
587                    }
588                }
589            }
590        }
591        result
592    }
593
594    fn compute_all_intervals(grid: &Grid) -> Vec<Vec<Vec<Interval>>> {
595        let mut all = vec![vec![vec![]; grid.height as usize]; grid.width as usize];
596        for x in 0..grid.width {
597            for y in 0..grid.height {
598                all[x as usize][y as usize] = grid.safe_intervals_at(Position::new(x, y));
599            }
600        }
601        all
602    }
603
604    fn reconstruct(expanded: &[SippNode], goal_node: &SippNode) -> Vec<Node> {
605        let mut path = vec![Node {
606            position: goal_node.position,
607            time: goal_node.time,
608            heuristic: goal_node.heuristic,
609            parent_index: goal_node.parent_index,
610        }];
611        let mut walker = *goal_node;
612        while walker.parent_index >= 0 {
613            walker = expanded[walker.parent_index as usize];
614            path.push(Node {
615                position: walker.position,
616                time: walker.time,
617                heuristic: walker.heuristic,
618                parent_index: walker.parent_index,
619            });
620        }
621        path.reverse();
622        path
623    }
624}
625
626// ---------------------------------------------------------------------------
627// Priority-based multi-agent planner
628// ---------------------------------------------------------------------------
629
630/// Start and goal for a single agent.
631#[derive(Debug, Clone)]
632pub struct AgentTask {
633    pub id: i32,
634    pub start: Position,
635    pub goal: Position,
636}
637
638impl AgentTask {
639    pub fn new(id: i32, start: Position, goal: Position) -> Self {
640        Self { id, start, goal }
641    }
642
643    fn distance_sq(&self) -> i32 {
644        let dx = self.goal.x - self.start.x;
645        let dy = self.goal.y - self.start.y;
646        dx * dx + dy * dy
647    }
648}
649
650/// Which single-agent algorithm to use inside the multi-agent planner.
651#[derive(Debug, Clone, Copy, PartialEq, Eq)]
652pub enum SingleAgentAlgorithm {
653    SpaceTimeAStar,
654    SafeInterval,
655}
656
657/// Priority-based multi-agent planner.
658///
659/// Plans agents one by one (longest distance first), reserving each
660/// agent's path in the grid so that subsequent agents avoid it.
661pub struct PriorityBasedPlanner;
662
663impl PriorityBasedPlanner {
664    /// Plan paths for all agents. Returns the ordered agent list and their
665    /// paths. Agents are planned in descending order of start-to-goal distance.
666    pub fn plan(
667        grid: &mut Grid,
668        tasks: &mut [AgentTask],
669        algorithm: SingleAgentAlgorithm,
670    ) -> RoboticsResult<Vec<NodePath>> {
671        // Sort by descending distance
672        tasks.sort_by_key(|b| std::cmp::Reverse(b.distance_sq()));
673
674        // Reserve initial positions
675        for task in tasks.iter() {
676            grid.reserve(
677                task.start,
678                task.id,
679                Interval::new(0, 10.min(grid.time_limit - 1)),
680            );
681        }
682
683        let mut paths = Vec::with_capacity(tasks.len());
684        for task in tasks.iter() {
685            grid.clear_reservation(task.start, task.id);
686            let path = match algorithm {
687                SingleAgentAlgorithm::SpaceTimeAStar => {
688                    SpaceTimeAStar::plan(grid, task.start, task.goal)?
689                }
690                SingleAgentAlgorithm::SafeInterval => {
691                    SafeIntervalPlanner::plan(grid, task.start, task.goal)?
692                }
693            };
694            grid.reserve_path(&path, task.id);
695            paths.push(path);
696        }
697        Ok(paths)
698    }
699}
700
701// ===========================================================================
702// Tests
703// ===========================================================================
704
705#[cfg(test)]
706mod tests {
707    use super::*;
708
709    // Helper: empty grid where every cell is free at all times.
710    fn empty_grid(size: i32) -> Grid {
711        Grid::empty(size, size, 100).unwrap()
712    }
713
714    // -----------------------------------------------------------------------
715    // Space-Time A*
716    // -----------------------------------------------------------------------
717
718    #[test]
719    fn test_sta_star_straight_line() {
720        let grid = empty_grid(10);
721        let start = Position::new(0, 0);
722        let goal = Position::new(5, 0);
723        let path = SpaceTimeAStar::plan(&grid, start, goal).unwrap();
724        assert_eq!(path.nodes.first().unwrap().position, start);
725        assert_eq!(path.nodes.last().unwrap().position, goal);
726        assert_eq!(path.goal_reached_time(), 5);
727    }
728
729    #[test]
730    fn test_sta_star_diagonal() {
731        let grid = empty_grid(10);
732        let start = Position::new(0, 0);
733        let goal = Position::new(3, 3);
734        let path = SpaceTimeAStar::plan(&grid, start, goal).unwrap();
735        assert_eq!(path.nodes.last().unwrap().position, goal);
736        // Manhattan distance = 6
737        assert_eq!(path.goal_reached_time(), 6);
738    }
739
740    #[test]
741    fn test_sta_star_same_start_goal() {
742        let grid = empty_grid(10);
743        let pos = Position::new(5, 5);
744        let path = SpaceTimeAStar::plan(&grid, pos, pos).unwrap();
745        assert_eq!(path.nodes.last().unwrap().position, pos);
746    }
747
748    #[test]
749    fn test_sta_star_with_obstacles() {
750        let grid = Grid::new(
751            11,
752            11,
753            5,
754            ObstacleArrangement::Arrangement1,
755            &[Position::new(0, 0), Position::new(10, 10)],
756            100,
757        )
758        .unwrap();
759        let result = SpaceTimeAStar::plan(&grid, Position::new(0, 0), Position::new(10, 10));
760        // May or may not find a path depending on obstacle layout; we just
761        // verify it does not panic.
762        assert!(result.is_ok() || result.is_err());
763    }
764
765    // -----------------------------------------------------------------------
766    // Safe Interval Planner
767    // -----------------------------------------------------------------------
768
769    #[test]
770    fn test_sipp_straight_line() {
771        let grid = empty_grid(10);
772        let start = Position::new(0, 0);
773        let goal = Position::new(5, 0);
774        let path = SafeIntervalPlanner::plan(&grid, start, goal).unwrap();
775        assert_eq!(path.nodes.first().unwrap().position, start);
776        assert_eq!(path.nodes.last().unwrap().position, goal);
777        assert_eq!(path.goal_reached_time(), 5);
778    }
779
780    #[test]
781    fn test_sipp_same_start_goal() {
782        let grid = empty_grid(10);
783        let pos = Position::new(3, 3);
784        let path = SafeIntervalPlanner::plan(&grid, pos, pos).unwrap();
785        assert_eq!(path.nodes.last().unwrap().position, pos);
786    }
787
788    // -----------------------------------------------------------------------
789    // Grid & intervals
790    // -----------------------------------------------------------------------
791
792    #[test]
793    fn test_grid_empty_all_free() {
794        let grid = empty_grid(5);
795        for x in 0..5 {
796            for y in 0..5 {
797                assert!(grid.is_free(Position::new(x, y), 0));
798            }
799        }
800    }
801
802    #[test]
803    fn test_grid_bounds_check() {
804        let grid = empty_grid(5);
805        assert!(!grid.inside_bounds(Position::new(-1, 0)));
806        assert!(!grid.inside_bounds(Position::new(0, 5)));
807        assert!(grid.inside_bounds(Position::new(0, 0)));
808        assert!(grid.inside_bounds(Position::new(4, 4)));
809    }
810
811    #[test]
812    fn test_safe_intervals_empty_grid() {
813        let grid = empty_grid(5);
814        let ivs = grid.safe_intervals_at(Position::new(2, 2));
815        // Single interval covering (almost) the whole time range
816        assert!(!ivs.is_empty());
817        assert_eq!(ivs[0].start_time, 0);
818    }
819
820    #[test]
821    fn test_reserve_and_check() {
822        let mut grid = empty_grid(5);
823        let pos = Position::new(2, 2);
824        assert!(grid.is_free(pos, 5));
825        grid.reserve(pos, 1, Interval::new(5, 10));
826        assert!(!grid.is_free(pos, 5));
827        assert!(!grid.is_free(pos, 10));
828        assert!(grid.is_free(pos, 11));
829    }
830
831    #[test]
832    fn test_clear_reservation() {
833        let mut grid = empty_grid(5);
834        let pos = Position::new(1, 1);
835        grid.reserve(pos, 42, Interval::new(0, 20));
836        assert!(!grid.is_free(pos, 10));
837        grid.clear_reservation(pos, 42);
838        assert!(grid.is_free(pos, 10));
839    }
840
841    // -----------------------------------------------------------------------
842    // NodePath
843    // -----------------------------------------------------------------------
844
845    #[test]
846    fn test_node_path_position_at() {
847        let nodes = vec![
848            Node {
849                position: Position::new(0, 0),
850                time: 0,
851                heuristic: 3,
852                parent_index: -1,
853            },
854            Node {
855                position: Position::new(1, 0),
856                time: 1,
857                heuristic: 2,
858                parent_index: 0,
859            },
860            Node {
861                position: Position::new(2, 0),
862                time: 2,
863                heuristic: 1,
864                parent_index: 1,
865            },
866        ];
867        let np = NodePath::new(nodes, 3);
868        assert_eq!(np.position_at(0), Some(Position::new(0, 0)));
869        assert_eq!(np.position_at(1), Some(Position::new(1, 0)));
870        assert_eq!(np.position_at(2), Some(Position::new(2, 0)));
871        // After goal: stays at last position
872        assert_eq!(np.position_at(10), Some(Position::new(2, 0)));
873    }
874
875    // -----------------------------------------------------------------------
876    // Priority-based multi-agent planner
877    // -----------------------------------------------------------------------
878
879    #[test]
880    fn test_priority_planner_two_agents() {
881        let mut grid = empty_grid(10);
882        let mut tasks = vec![
883            AgentTask::new(1, Position::new(0, 0), Position::new(5, 0)),
884            AgentTask::new(2, Position::new(0, 1), Position::new(5, 1)),
885        ];
886        let paths =
887            PriorityBasedPlanner::plan(&mut grid, &mut tasks, SingleAgentAlgorithm::SpaceTimeAStar)
888                .unwrap();
889        assert_eq!(paths.len(), 2);
890        for (task, path) in tasks.iter().zip(paths.iter()) {
891            assert_eq!(path.nodes.last().unwrap().position, task.goal);
892        }
893    }
894
895    // -----------------------------------------------------------------------
896    // Obstacle arrangement
897    // -----------------------------------------------------------------------
898
899    #[test]
900    fn test_arrangement1_creates_obstacles() {
901        let grid = Grid::new(11, 11, 5, ObstacleArrangement::Arrangement1, &[], 50).unwrap();
902        assert_eq!(grid.obstacle_paths.len(), 5);
903    }
904
905    #[test]
906    fn test_narrow_corridor_creates_obstacles() {
907        let grid = Grid::new(11, 11, 20, ObstacleArrangement::NarrowCorridor, &[], 50).unwrap();
908        // Skips the middle row, so 10 out of 11 rows
909        assert_eq!(grid.obstacle_paths.len(), 10);
910    }
911
912    #[test]
913    fn test_invalid_grid_params() {
914        assert!(Grid::empty(0, 5, 100).is_err());
915        assert!(Grid::empty(5, -1, 100).is_err());
916        assert!(Grid::empty(5, 5, 0).is_err());
917    }
918}
rust_robotics_planning/time_based_path_planning.rs

rust_robotics_planning/
time_based_path_planning.rs
