some cleanup

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/BaseClasses.py

@@ -1,12 +1,13 @@
+from abc import ABC, abstractmethod
+from dataclasses import dataclass
+import math
 from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
     Grid,
     Position,
 )
 from PathPlanning.TimeBasedPathPlanning.Node import NodePath
 import random
 import numpy.random as numpy_random
-from abc import ABC, abstractmethod
-from dataclasses import dataclass
 
 # Seed randomness for reproducibility
 RANDOM_SEED = 50
@@ -32,6 +33,9 @@ class StartAndGoal:
     # Goal position of the robot
     goal: Position
 
+    def distance_start_to_goal(self) -> float:
+        return pow(self.goal.x - self.start.x, 2) + pow(self.goal.y - self.start.y, 2)
+
 class MultiAgentPlanner(ABC):
     """
     Base class for multi-agent planners

PathPlanning/TimeBasedPathPlanning/Plotting.py

@@ -105,12 +105,19 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N
 
         # Update each agent's position
         for (j, path) in enumerate(paths):
+            path_postitions = []
             if i <= path.goal_reached_time():
                 res = path.get_position(i)
                 if not res:
                     print(path)
                     print(i)
                 path_position = path.get_position(i)
+
+                # Verify position is valid
+                assert not path_position in obs_positions
+                assert not path_position in path_postitions
+                path_postitions.append(path_position)
+
                 path_plots[j].set_data([path_position.x], [path_position.y])
 
         plt.pause(0.2)

PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py

@@ -1,8 +1,9 @@
 """
-TODO - doc comment
+Priority Based Planner for multi agent path planning.
+The planner generates an order to plan in, and generates plans for the robots in that order. Each planned
+path is reserved in the grid, and all future plans must avoid that path.
 
-sort of outlined in this paper: https://pure.tudelft.nl/ws/portalfiles/portal/67074672/07138650.pdf
-better paper: https://people.csail.mit.edu/tlp/publications/01087401.pdf
+Algorithm outlined in section III of this paper: https://pure.tudelft.nl/ws/portalfiles/portal/67074672/07138650.pdf
 """
 
 import numpy as np
@@ -16,21 +17,24 @@
 from PathPlanning.TimeBasedPathPlanning.Node import NodePath
 from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner
 from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner
-from PathPlanning.TimeBasedPathPlanning.SpaceTimeAStar import SpaceTimeAStar
 from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePaths
 import time
 
 class PriorityBasedPlanner(MultiAgentPlanner):
 
-    def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> list[NodePath]: # TODO: list of what
+    def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> list[NodePath]:
 
         print(f"Using planner: {single_agent_planner_class}")
 
         # Reserve initial positions
         for start_and_goal in start_and_goals:
             grid.reserve_position(start_and_goal.start, start_and_goal.index, Interval(0, 10))
 
-        # TODO: smarter ordering
+        # Plan in descending order of distance from start to goal
+        start_and_goals = sorted(start_and_goals,
+                    key=lambda item: item.distance_start_to_goal(),
+                    reverse=True)
+
         paths = []
         for start_and_goal in start_and_goals:
             if True: