add unit test

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py

@@ -22,8 +22,12 @@
 
 class PriorityBasedPlanner(MultiAgentPlanner):
 
-    def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> list[NodePath]:
-        
+    def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> 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
+        corresponds to the order of the `start_and_goals` list.
+        """
         print(f"Using planner: {single_agent_planner_class}")
 
         # Reserve initial positions
@@ -51,7 +55,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
             grid.reserve_path(path, agent_index)
             paths.append(path)
 
-        return paths
+        return (start_and_goals, paths)
 
 verbose = False
 show_animation = True
@@ -71,7 +75,9 @@ def main():
     )
 
     start_time = time.time()
-    paths: list[NodePath] = PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose)
+    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
     print(f"\nPlanning took: {runtime:.5f} seconds")

tests/test_priority_based_planner.py

@@ -0,0 +1,45 @@
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+    Grid,
+    NodePath,
+    ObstacleArrangement,
+    Position,
+)
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import StartAndGoal
+from PathPlanning.TimeBasedPathPlanning import PriorityBasedPlanner as m
+from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner
+import numpy as np
+import conftest
+
+
+def test_1():
+    grid_side_length = 21
+
+    start_and_goals = [StartAndGoal(i, Position(1, i), Position(19, 19-i)) for i in range(1, 16)]
+    obstacle_avoid_points = [pos for item in start_and_goals for pos in (item.start, item.goal)]
+
+    grid = Grid(
+        np.array([grid_side_length, grid_side_length]),
+        num_obstacles=250,
+        obstacle_avoid_points=obstacle_avoid_points,
+        obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+    )
+
+    grid_side_length = 21
+    grid = Grid(
+        np.array([grid_side_length, grid_side_length]),
+        obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
+    )
+
+    m.show_animation = False
+
+    start_and_goals: list[StartAndGoal]
+    paths: list[NodePath]
+    start_and_goals, paths = m.PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, False)
+
+    # All paths should start at the specified position and reach the goal
+    for i, start_and_goal in enumerate(start_and_goals):
+        assert paths[i].path[0].position == start_and_goal.start
+        assert paths[i].path[-1].position == start_and_goal.goal
+
+if __name__ == "__main__":
+    conftest.run_this_test(__file__)