added unit test

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py

@@ -1,11 +1,17 @@
+"""
+Space-time A* Algorithm
+    This script demonstrates the Space-time A* algorithm for path planning in a grid world with moving obstacles.
+
+    Reference: https://www.davidsilver.uk/wp-content/uploads/2020/03/coop-path-AIWisdom.pdf
+"""
+
+from __future__ import annotations # For typehints of a class within itself
 import numpy as np
 import matplotlib.pyplot as plt
-import matplotlib.animation as animation
-from moving_obstacles import Grid, Position
+from PathPlanning.TimeBasedPathPlanning.moving_obstacles import Grid, ObstacleArrangement, Position
 import heapq
-from typing import Generator
+from collections.abc import Generator
 import random
-from __future__ import annotations
 
 # Seed randomness for reproducibility
 RANDOM_SEED = 50
@@ -23,7 +29,11 @@ def __init__(self, position: Position, time: int, heuristic: int, parent_index:
         self.time = time
         self.heuristic = heuristic
         self.parent_index = parent_index
-        
+
+    """
+    This is what is used to drive node expansion. The node with the lowest value is expanded next.
+    This comparison prioritizes the node with the lowest cost-to-come (self.time) + cost-to-go (self.heuristic)
+    """
     def __lt__(self, other: Node):
         return (self.time + self.heuristic) < (other.time + other.heuristic)
 
@@ -32,25 +42,25 @@ def __repr__(self):
 
 class NodePath:
     path: list[Node]
+    positions_at_time: dict[int, Position] = {}
 
     def __init__(self, path: list[Node]):
         self.path = path
-    
+        for node in path:
+            self.positions_at_time[node.time] = node.position
+
+    """
+    Get the position of the path at a given time
+    """
     def get_position(self, time: int) -> Position:
-        # TODO: this is inefficient
-        for i in range(0, len(self.path) - 2):
-            if self.path[i + 1].time > time:
-                print(f"position @ {i} is {self.path[i].position}")
-                return self.path[i].position
-        
-        if len(self.path) > 0:
-            return self.path[-1].position
-
-        return None
-    
+        return self.positions_at_time.get(time)
+
+    """
+    Time stamp of the last node in the path
+    """
     def goal_reached_time(self) -> int:
         return self.path[-1].time
-    
+
     def __repr__(self):
         repr_string = ""
         for (i, node) in enumerate(self.path):
@@ -71,7 +81,6 @@ def plan(self, verbose: bool = False) -> NodePath:
         open_set = []
         heapq.heappush(open_set, Node(self.start, 0, self.calculate_heuristic(self.start), -1))
 
-        # TODO: is vec good here?
         expanded_set = []
         while open_set:
             expanded_node: Node = heapq.heappop(open_set)
@@ -92,7 +101,7 @@ def plan(self, verbose: bool = False) -> NodePath:
                     path_walker = expanded_set[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)
@@ -102,9 +111,12 @@ def plan(self, verbose: bool = False) -> NodePath:
 
             for child in self.generate_successors(expanded_node, expanded_idx, verbose):
                 heapq.heappush(open_set, child)
-        
+
         raise Exception("No path found")
-    
+
+    """
+    Generate possible successors of the provided `parent_node`
+    """
     def generate_successors(self, parent_node: Node, parent_node_idx: int, verbose: bool) -> Generator[Node, None, None]:
         diffs = [Position(0, 1), Position(0, -1), Position(1, 0), Position(-1, 0), Position(0, 0)]
         for diff in diffs:
@@ -119,13 +131,12 @@ def calculate_heuristic(self, position) -> int:
         diff = self.goal - position
         return abs(diff.x) + abs(diff.y)
 
-import imageio.v2 as imageio
 show_animation = True
 def main():
-    start = Position(1, 1)
+    start = Position(1, 11)
     goal = Position(19, 19)
     grid_side_length = 21
-    grid = Grid(np.array([grid_side_length, grid_side_length]), num_obstacles=40, obstacle_avoid_points=[start, goal])
+    grid = Grid(np.array([grid_side_length, grid_side_length]), num_obstacles=40, obstacle_avoid_points=[start, goal], obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1)
 
     planner = TimeBasedAStar(grid, start, goal)
     verbose = False
@@ -144,7 +155,7 @@ def main():
     ax.set_xticks(np.arange(0, grid_side_length, 1))
     ax.set_yticks(np.arange(0, grid_side_length, 1))
 
-    start_and_goal, = ax.plot([], [], 'mD', ms=15, label="Start and Goal") 
+    start_and_goal, = ax.plot([], [], 'mD', ms=15, label="Start and Goal")
     start_and_goal.set_data([start.x, goal.x], [start.y, goal.y])
     obs_points, = ax.plot([], [], 'ro', ms=15, label="Obstacles")
     path_points, = ax.plot([], [], 'bo', ms=10, label="Path Found")
@@ -155,17 +166,13 @@ def main():
         lambda event: [exit(
             0) if event.key == 'escape' else None])
 
-    frames = []
     for i in range(0, path.goal_reached_time()):
         obs_positions = grid.get_obstacle_positions_at_time(i)
         obs_points.set_data(obs_positions[0], obs_positions[1])
         path_position = path.get_position(i)
         path_points.set_data([path_position.x], [path_position.y])
         plt.pause(0.2)
-        plt.savefig(f"frame_{i:03d}.png")  # Save each frame as an image
-        frames.append(imageio.imread(f"frame_{i:03d}.png"))
-    imageio.mimsave("path_animation.gif", frames, fps=5)  # Convert images to GIF
     plt.show()
 
 if __name__ == '__main__':
-    main()
+    main()

PathPlanning/TimeBasedPathPlanning/__init__.py

@@ -11,12 +11,12 @@ def __init__(self, x: int, y: int):
 
     def as_ndarray(self) -> np.ndarray[int, int]:
         return np.array([self.x, self.y])
-    
+
     def __add__(self, other):
         if isinstance(other, Position):
             return Position(self.x + other.x, self.y + other.y)
         raise NotImplementedError(f"Addition not supported for Position and {type(other)}")
-    
+
     def __sub__(self, other):
         if isinstance(other, Position):
             return Position(self.x - other.x, self.y - other.y)
@@ -36,24 +36,21 @@ class ObstacleArrangement(Enum):
     # Obstacles start in a line in y at center of grid and move side-to-side in x
     ARRANGEMENT1 = 1
 
-class Grid():
-    
+class Grid:
     # Set in constructor
     grid_size = None
     grid = None
     obstacle_paths: list[list[Position]] = []
     # Obstacles will never occupy these points. Useful to avoid impossible scenarios
     obstacle_avoid_points = []
 
-    # Problem definition
     # Number of time steps in the simulation
     time_limit: int
 
     # Logging control
     verbose = False
 
-    def __init__(self, grid_size: np.ndarray[int, int], num_obstacles: int = 2, obstacle_avoid_points: list[Position] = [], obstacle_arrangement: ObstacleArrangement = ObstacleArrangement.RANDOM,  time_limit: int = 100):
-        num_obstacles
+    def __init__(self, grid_size: np.ndarray[int, int], num_obstacles: int = 40, obstacle_avoid_points: list[Position] = [], obstacle_arrangement: ObstacleArrangement = ObstacleArrangement.RANDOM,  time_limit: int = 100):
         self.obstacle_avoid_points = obstacle_avoid_points
         self.time_limit = time_limit
         self.grid_size = grid_size
@@ -66,15 +63,18 @@ def __init__(self, grid_size: np.ndarray[int, int], num_obstacles: int = 2, obst
             self.obstacle_paths = self.generate_dynamic_obstacles(num_obstacles)
         elif obstacle_arrangement == ObstacleArrangement.ARRANGEMENT1:
             self.obstacle_paths = self.obstacle_arrangement_1(num_obstacles)
-        
+
         for (i, path) in enumerate(self.obstacle_paths):
-            obs_idx = i + 1 # avoid using 0 - that indicates free space
+            obs_idx = i + 1 # avoid using 0 - that indicates free space in the grid
             for (t, position) in enumerate(path):
                 # Reserve old & new position at this time step
                 if t > 0:
                     self.grid[path[t-1].x, path[t-1].y, t] = obs_idx
                 self.grid[position.x, position.y, t] = obs_idx
 
+    """
+    Generate dynamic obstacles that move around the grid. Initial positions and movements are random
+    """
     def generate_dynamic_obstacles(self, obs_count: int) -> list[list[Position]]:
         obstacle_paths = []
         for _obs_idx in (0, obs_count):
@@ -112,31 +112,39 @@ def generate_dynamic_obstacles(self, obs_count: int) -> list[list[Position]]:
                     valid_position = positions[-1]
 
                 positions.append(valid_position)
-    
+
             obstacle_paths.append(positions)
 
         return obstacle_paths
-    
+
+    """
+    Generate a line of obstacles in y at the center of the grid that move side-to-side in x
+    Bottom half start moving right, top half start moving left. If `obs_count` is less than the length of
+    the grid, only the first `obs_count` obstacles will be generated.
+    """
     def obstacle_arrangement_1(self, obs_count: int) -> list[list[Position]]:
-        # bottom half of y values start left -> right
-        # top half of y values start right -> left
         obstacle_paths = []
         half_grid_x = self.grid_size[0] // 2
         half_grid_y = self.grid_size[1] // 2
-        
-        for y_idx in range(0, min(obs_count, self.grid_size[1] - 1)):
+
+        for y_idx in range(0, min(obs_count, self.grid_size[1])):
             moving_right = y_idx < half_grid_y
             position = Position(half_grid_x, y_idx)
             path = [position]
 
-            for _t in range(1, self.time_limit-1):
+            for t in range(1, self.time_limit-1):
+                # sit in place every other time step
+                if t % 2 == 0:
+                    path.append(position)
+                    continue
+
                 # first check if we should switch direction (at edge of grid)
                 if (moving_right and position.x == self.grid_size[0] - 1) or (not moving_right and position.x == 0):
                     moving_right = not moving_right
                 # step in direction
                 position = Position(position.x + (1 if moving_right else -1), position.y)
                 path.append(position)
-            
+
             obstacle_paths.append(path)
 
         return obstacle_paths
@@ -159,13 +167,13 @@ def valid_position(self, position: Position, t: int) -> bool:
 
         # Check if new position is not occupied at time t
         return self.grid[position.x, position.y, t] == 0
-    
+
     """
     Returns True if the given position is valid at time t and is not in the set of obstacle_avoid_points
     """
     def valid_obstacle_position(self, position: Position, t: int) -> bool:
         return self.valid_position(position, t) and position not in self.obstacle_avoid_points
-    
+
     """
     Returns True if the given position is within the grid's boundaries
     """
@@ -180,12 +188,12 @@ def inside_grid_bounds(self, position: Position) -> bool:
     """
     def sample_random_position(self) -> Position:
         return Position(np.random.randint(0, self.grid_size[0]), np.random.randint(0, self.grid_size[1]))
-    
+
     """
     Returns a tuple of (x_positions, y_positions) of the obstacles at time t
     """
     def get_obstacle_positions_at_time(self, t: int) -> tuple[list[int], list[int]]:
-        
+
         x_positions = []
         y_positions = []
         for obs_path in self.obstacle_paths:
@@ -221,4 +229,4 @@ def main():
     plt.show()
 
 if __name__ == '__main__':
-    main()
+    main()

PathPlanning/TimeBasedPathPlanning/moving_obstacles.py

@@ -0,0 +1,24 @@
+from PathPlanning.TimeBasedPathPlanning.moving_obstacles import Grid, ObstacleArrangement, Position
+from PathPlanning.TimeBasedPathPlanning import SpaceTimeAStar as m
+import numpy as np
+import conftest
+
+def test_1():
+    start = Position(1, 11)
+    goal = Position(19, 19)
+    grid_side_length = 21
+    grid = Grid(np.array([grid_side_length, grid_side_length]), obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1)
+
+    m.show_animation = False
+    planner = m.TimeBasedAStar(grid, start, goal)
+
+    path = planner.plan(False)
+
+    # path should have 28 entries
+    assert len(path.path) == 31
+
+    # path should end at the goal
+    assert path.path[-1].position == goal
+
+if __name__ == '__main__':
+    conftest.run_this_test(__file__)