add some docs & address todos

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py

@@ -30,6 +30,9 @@ def __sub__(self, other):
             f"Subtraction not supported for Position and {type(other)}"
         )
 
+    def __hash__(self):
+        return hash((self.x, self.y))
+
 @dataclass
 class Interval:
     start_time: int

PathPlanning/TimeBasedPathPlanning/SafeInterval.py

@@ -13,11 +13,10 @@
     Position,
 )
 import heapq
-from collections.abc import Generator
 import random
 from dataclasses import dataclass
 from functools import total_ordering
-
+import time
 
 # Seed randomness for reproducibility
 RANDOM_SEED = 50
@@ -55,6 +54,10 @@ def __eq__(self, other: object):
             return NotImplementedError(f"Cannot compare Node with object of type: {type(other)}")
         return self.position == other.position and self.time == other.time and self.interval == other.interval
 
+@dataclass
+class EntryTimeAndInterval:
+    entry_time: int
+    interval: Interval
 
 class NodePath:
     path: list[Node]
@@ -130,24 +133,22 @@ def plan(self, verbose: bool = False) -> NodePath:
                 print(f"Found path to goal after {len(expanded_list)} expansions")
                 path = []
                 path_walker: Node = expanded_node
-                while path_walker.parent_index != -1:
+                while True:
                     path.append(path_walker)
+                    if path_walker.parent_index == -1:
+                        break
                     path_walker = expanded_list[path_walker.parent_index]
-                # TODO: fix hack around bad while condiiotn
-                path.append(path_walker)
 
                 # reverse path so it goes start -> goal
                 path.reverse()
                 return NodePath(path)
 
             expanded_idx = len(expanded_list)
             expanded_list.append(expanded_node)
-            visited_intervals[expanded_node.position.x, expanded_node.position.y].append((expanded_node.time, expanded_node.interval))
+            entry_time_and_node = EntryTimeAndInterval(expanded_node.time, expanded_node.interval)
+            add_entry_to_visited_intervals_array(entry_time_and_node, visited_intervals, expanded_node)
 
-            # if len(expanded_set) > 100:
-            #     blarg
-
-            for child in self.generate_successors(expanded_node, expanded_idx, verbose, safe_intervals, visited_intervals):
+            for child in self.generate_successors(expanded_node, expanded_idx, safe_intervals, visited_intervals):
                 heapq.heappush(open_set, child)
 
         raise Exception("No path found")
@@ -157,7 +158,7 @@ def plan(self, verbose: bool = False) -> NodePath:
     """
     # TODO: is intervals being passed by ref? (i think so?)
     def generate_successors(
-        self, parent_node: Node, parent_node_idx: int, verbose: bool, intervals: np.ndarray, visited_intervals: np.ndarray
+        self, parent_node: Node, parent_node_idx: int, intervals: np.ndarray, visited_intervals: np.ndarray
     ) -> list[Node]:
         new_nodes = []
         diffs = [
@@ -185,60 +186,69 @@ def generate_successors(
                 if interval.start_time > current_interval.end_time:
                     break
 
-                # TODO: this bit feels wonky
                 # if we have already expanded a node in this interval with a <= starting time, continue
                 better_node_expanded = False
                 for visited in visited_intervals[new_pos.x, new_pos.y]:
-                    if interval == visited[1] and visited[0] <= parent_node.time + 1:
+                    if interval == visited.interval and visited.entry_time <= parent_node.time + 1:
                         better_node_expanded = True
                         break
                 if better_node_expanded:
                     continue
 
                 # We know there is some overlap. Generate successor at the earliest possible time the
                 # new interval can be entered
-                # TODO: dont love the optionl usage here
-                new_node_t = None
                 for possible_t in range(max(parent_node.time + 1, interval.start_time), min(current_interval.end_time, interval.end_time)):
                     if self.grid.valid_position(new_pos, possible_t):
-                        new_node_t = possible_t
+                        new_nodes.append(Node(
+                            new_pos,
+                            # entry is max of interval start and parent node start time (get there as soon as possible)
+                            max(parent_node.time + 1, interval.start_time),
+                            self.calculate_heuristic(new_pos),
+                            parent_node_idx,
+                            interval,
+                        ))
+                        # break because all t's after this will make nodes with a higher cost, the same heuristic, and are in the same interval
                         break
 
-                if new_node_t:
-                    # TODO: should be able to break here?
-                    new_nodes.append(Node(
-                        new_pos,
-                        # entry is max of interval start and parent node start time (get there as soon as possible)
-                        max(parent_node.time + 1, interval.start_time),
-                        self.calculate_heuristic(new_pos),
-                        parent_node_idx,
-                        interval,
-                    ))
-
         return new_nodes
 
     def calculate_heuristic(self, position) -> int:
         diff = self.goal - position
         return abs(diff.x) + abs(diff.y)
 
 
+def add_entry_to_visited_intervals_array(entry_time_and_interval: EntryTimeAndInterval, visited_intervals: np.ndarray, expanded_node: Node):
+    # if entry is present, update entry time if better
+    for existing_entry_and_interval in visited_intervals[expanded_node.position.x, expanded_node.position.y]:
+        if existing_entry_and_interval.interval == entry_time_and_interval.interval:
+            existing_entry_and_interval.entry_time = min(existing_entry_and_interval.entry_time, entry_time_and_interval.entry_time)
+
+    # Otherwise, append
+    visited_intervals[expanded_node.position.x, expanded_node.position.y].append(entry_time_and_interval)
+
+
 show_animation = True
 verbose = False
 
 def main():
-    start = Position(1, 1)
+    start = Position(1, 18)
     goal = Position(19, 19)
     grid_side_length = 21
+
+    start_time = time.time()
+
     grid = Grid(
         np.array([grid_side_length, grid_side_length]),
         num_obstacles=250,
         obstacle_avoid_points=[start, goal],
-        # obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
-        obstacle_arrangement=ObstacleArrangement.RANDOM,
+        obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+        # obstacle_arrangement=ObstacleArrangement.RANDOM,
     )
 
     planner = SafeIntervalPathPlanner(grid, start, goal)
     path = planner.plan(verbose)
+    runtime = time.time() - start_time
+    print(f"Planning took: {runtime:.5f} seconds")
 
     if verbose:
         print(f"Path: {path}")

PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py

@@ -20,7 +20,7 @@
 import random
 from dataclasses import dataclass
 from functools import total_ordering
-
+import time
 
 # Seed randomness for reproducibility
 RANDOM_SEED = 50
@@ -53,6 +53,8 @@ def __eq__(self, other: object):
             return NotImplementedError(f"Cannot compare Node with object of type: {type(other)}")
         return self.position == other.position and self.time == other.time
 
+    def __hash__(self):
+        return hash((self.position, self.time, self.heuristic))
 
 class NodePath:
     path: list[Node]
@@ -98,7 +100,8 @@ def plan(self, verbose: bool = False) -> NodePath:
             open_set, Node(self.start, 0, self.calculate_heuristic(self.start), -1)
         )
 
-        expanded_set: list[Node] = []
+        expanded_list: list[Node] = []
+        expanded_set: set[Node] = set()
         while open_set:
             expanded_node: Node = heapq.heappop(open_set)
             if verbose:
@@ -110,23 +113,24 @@ def plan(self, verbose: bool = False) -> NodePath:
                 continue
 
             if expanded_node.position == self.goal:
-                print(f"Found path to goal after {len(expanded_set)} expansions")
+                print(f"Found path to goal after {len(expanded_list)} expansions")
                 path = []
                 path_walker: Node = expanded_node
                 while True:
                     path.append(path_walker)
                     if path_walker.parent_index == -1:
                         break
-                    path_walker = expanded_set[path_walker.parent_index]
+                    path_walker = expanded_list[path_walker.parent_index]
 
                 # reverse path so it goes start -> goal
                 path.reverse()
                 return NodePath(path)
 
-            expanded_idx = len(expanded_set)
-            expanded_set.append(expanded_node)
+            expanded_idx = len(expanded_list)
+            expanded_list.append(expanded_node)
+            expanded_set.add(expanded_node)
 
-            for child in self.generate_successors(expanded_node, expanded_idx, verbose):
+            for child in self.generate_successors(expanded_node, expanded_idx, verbose, expanded_set):
                 heapq.heappush(open_set, child)
 
         raise Exception("No path found")
@@ -135,7 +139,7 @@ def plan(self, verbose: bool = False) -> NodePath:
     Generate possible successors of the provided `parent_node`
     """
     def generate_successors(
-        self, parent_node: Node, parent_node_idx: int, verbose: bool
+        self, parent_node: Node, parent_node_idx: int, verbose: bool, expanded_set: set[Node]
     ) -> Generator[Node, None, None]:
         diffs = [
             Position(0, 0),
@@ -146,13 +150,17 @@ def generate_successors(
         ]
         for diff in diffs:
             new_pos = parent_node.position + diff
+            new_node = Node(
+                new_pos,
+                parent_node.time + 1,
+                self.calculate_heuristic(new_pos),
+                parent_node_idx,
+            )
+
+            if new_node in expanded_set:
+                continue
+
             if self.grid.valid_position(new_pos, parent_node.time + 1):
-                new_node = Node(
-                    new_pos,
-                    parent_node.time + 1,
-                    self.calculate_heuristic(new_pos),
-                    parent_node_idx,
-                )
                 if verbose:
                     print("\tNew successor node: ", new_node)
                 yield new_node
@@ -166,9 +174,12 @@ def calculate_heuristic(self, position) -> int:
 verbose = False
 
 def main():
-    start = Position(1, 11)
+    start = Position(1, 5)
     goal = Position(19, 19)
     grid_side_length = 21
+
+    start_time = time.time()
+
     grid = Grid(
         np.array([grid_side_length, grid_side_length]),
         num_obstacles=40,
@@ -179,6 +190,9 @@ def main():
     planner = SpaceTimeAStar(grid, start, goal)
     path = planner.plan(verbose)
 
+    runtime = time.time() - start_time
+    print(f"Planning took: {runtime:.5f} seconds")
+
     if verbose:
         print(f"Path: {path}")
 

docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst

@@ -16,7 +16,33 @@ Using a time-based cost and heuristic ensures the path found is optimal in terms
 The cost is the amount of time it takes to reach a given node, and the heuristic is the minimum amount of time it could take to reach the goal from that node, disregarding all obstacles.
 For a simple scenario where the robot can move 1 cell per time step and stop and go as it pleases, the heuristic for time is equivalent to the heuristic for distance.
 
+Safe Interval Path Planning
+~~~~~~~~~~~~~~~~~~~~~~
+
+The safe interval path planning algorithm is described in this paper: https://www.cs.cmu.edu/~maxim/files/sipp_icra11.pdf
+It is faster than space-time A* because it pre-computes the intervals of time that are unoccupied in each cell. This allows it to reduce the number of successor node it generates by avoiding nodes within the same interval.
+
+## Comparison with Space-time A*:
+Arrangement1 startings at (1, 18)
+
+SafeInterval planner:
+```
+Found path to goal after 322 expansions
+Planning took: 0.00730 seconds
+```
+
+SpaceTimeAStar:
+```
+Found path to goal after 2717154 expansions
+Planning took: 20.51330 seconds
+```
+
+.. image:: <gif-1>
+
+.. image:: <gif-2>
+
 References:
 ~~~~~~~~~~~
 
 -  `Cooperative Pathfinding <https://www.davidsilver.uk/wp-content/uploads/2020/03/coop-path-AIWisdom.pdf>`__
+-  `SIPP: Safe Interval Path Planning for Dynamic Environments <https://www.cs.cmu.edu/~maxim/files/sipp_icra11.pdf>`__