fix tests

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/BaseClasses.py

@@ -42,8 +42,8 @@ class MultiAgentPlanner(ABC):
 
     @staticmethod
     @abstractmethod
-    def plan(grid: Grid, start_and_goal_positions: list[StartAndGoal], verbose: bool = False) -> list[NodePath]:
+    def plan(grid: Grid, start_and_goal_positions: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]:
         """
-        Plan for all agents. Returned paths are in the order of the `StartAndGoal` list this object was instantiated with
+        Plan for all agents. Returned paths are in order corresponding to the returned list of `StartAndGoal` objects
         """
         pass

PathPlanning/TimeBasedPathPlanning/Node.py

@@ -1,6 +1,7 @@
 from dataclasses import dataclass
 from functools import total_ordering
 import numpy as np
+from typing import Sequence
 
 @dataclass(order=True)
 class Position:
@@ -61,11 +62,15 @@ def __hash__(self):
         return hash((self.position, self.time))
 
 class NodePath:
-    path: list[Node]
+    path: Sequence[Node]
     positions_at_time: dict[int, Position]
+    # Number of nodes expanded while finding this path
+    expanded_node_count: int
 
-    def __init__(self, path: list[Node]):
+    def __init__(self, path: Sequence[Node], expanded_node_count: int):
         self.path = path
+        self.expanded_node_count = expanded_node_count
+
         self.positions_at_time = {}
         for i, node in enumerate(path):
             reservation_finish_time = node.time + 1

PathPlanning/TimeBasedPathPlanning/Plotting.py

@@ -1,5 +1,6 @@
 import numpy as np
 import matplotlib.pyplot as plt
+from matplotlib.backend_bases import KeyEvent
 from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
     Grid,
     Position,
@@ -30,13 +31,18 @@ def PlotNodePath(grid: Grid, start: Position, goal: Position, path: NodePath):
 
     # for stopping simulation with the esc key.
     plt.gcf().canvas.mpl_connect(
-        "key_release_event", lambda event: [exit(0) if event.key == "escape" else None]
+        "key_release_event",
+        lambda event: [exit(0) if event.key == "escape" else None]
+        if isinstance(event, KeyEvent) else None
     )
 
     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)
+        if not path_position:
+            raise Exception(f"Path position not found for time {i}.")
+
         path_points.set_data([path_position.x], [path_position.y])
         plt.pause(0.2)
     plt.show()
@@ -91,7 +97,9 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N
 
     # For stopping simulation with the esc key
     plt.gcf().canvas.mpl_connect(
-        "key_release_event", lambda event: [exit(0) if event.key == "escape" else None]
+        "key_release_event",
+        lambda event: [exit(0) if event.key == "escape" else None]
+        if isinstance(event, KeyEvent) else None
     )
 
     # Find the maximum time across all paths
@@ -112,6 +120,8 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N
                     print(path)
                     print(i)
                 path_position = path.get_position(i)
+                if not path_position:
+                    raise Exception(f"Path position not found for time {i}.")
 
                 # Verify position is valid
                 assert not path_position in obs_positions

PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py

@@ -23,7 +23,7 @@
 class PriorityBasedPlanner(MultiAgentPlanner):
 
     @staticmethod
-    def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> tuple[list[StartAndGoal], list[NodePath]]:
+    def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]:
         """
         Generate a path from the start to the goal for each agent in the `start_and_goals` list.
         Returns the re-ordered StartAndGoal combinations, and a list of path plans. The order of the plans
@@ -76,8 +76,6 @@ def main():
     )
 
     start_time = time.time()
-    start_and_goals: list[StartAndGoal]
-    paths: list[NodePath]
     start_and_goals, paths = PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose)
 
     runtime = time.time() - start_time

PathPlanning/TimeBasedPathPlanning/SafeInterval.py

@@ -81,7 +81,7 @@ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) ->
 
                 # reverse path so it goes start -> goal
                 path.reverse()
-                return NodePath(path)
+                return NodePath(path, len(expanded_list))
 
             expanded_idx = len(expanded_list)
             expanded_list.append(expanded_node)

PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py

@@ -54,7 +54,7 @@ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) ->
 
                 # reverse path so it goes start -> goal
                 path.reverse()
-                return NodePath(path)
+                return NodePath(path, len(expanded_set))
 
             expanded_idx = len(expanded_list)
             expanded_list.append(expanded_node)

tests/test_safe_interval_path_planner.py

@@ -18,9 +18,7 @@ def test_1():
     )
 
     m.show_animation = False
-    planner = m.SafeIntervalPathPlanner(grid, start, goal)
-
-    path = planner.plan(False)
+    path = m.SafeIntervalPathPlanner.plan(grid, start, goal)
 
     # path should have 31 entries
     assert len(path.path) == 31

tests/test_space_time_astar.py

@@ -18,17 +18,15 @@ def test_1():
     )
 
     m.show_animation = False
-    planner = m.SpaceTimeAStar(grid, start, goal)
-
-    path = planner.plan(False)
+    path = m.SpaceTimeAStar.plan(grid, start, goal)
 
     # path should have 28 entries
     assert len(path.path) == 31
 
     # path should end at the goal
     assert path.path[-1].position == goal
 
-    assert planner.expanded_node_count < 1000
+    assert path.expanded_node_count < 1000
 
 if __name__ == "__main__":
     conftest.run_this_test(__file__)