more cleanup

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py

@@ -40,19 +40,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
         """
         print(f"Using single-agent planner: {single_agent_planner_class}")
 
-        initial_solution: dict[AgentId, NodePath] = {}
-
-        # Generate initial solution (no constraints)
-        for start_and_goal in start_and_goals:
-            path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.agent_id, verbose)
-            initial_solution[start_and_goal.agent_id] = path
-
-        if verbose:
-            print("Initial solution:")
-            for (agent_idx, path) in initial_solution.items():
-                print(f"\nAgent {agent_idx} path:\n {path}")
-
-        constraint_tree = ConstraintTree(initial_solution)
+        constraint_tree: ConstraintTree = ConflictBasedSearch.initialize_constraint_tree(grid, start_and_goals, single_agent_planner_class, verbose)
         attempted_constraint_combos: set = set()
 
         while constraint_tree.nodes_to_expand:
@@ -100,11 +88,6 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
                 # Deepcopy to update with applied constraint and new paths
                 applied_constraint_parent = deepcopy(constraint_tree_node)
                 applied_constraint_parent.paths[constrained_agent.agent] = new_path
-
-                if verbose:
-                    for (agent_idx, path) in applied_constraint_parent.paths.items():
-                        print(f"\nAgent {agent_idx} path:\n {path}")
-
                 applied_constraint_parent.constraint = applied_constraint
                 parent_idx = constraint_tree.add_expanded_node(applied_constraint_parent)
 
@@ -152,10 +135,10 @@ def plan_for_agent(constrained_agent: ConstrainedAgent,
             attempted_constraint_combos.add(constraint_hash)
 
         if verbose:
-            print(f"\tall constraints: {all_constraints}")
+            print(f"\tAll constraints: {all_constraints}")
 
-        grid.clear_constraint_points()
-        grid.apply_constraint_points(all_constraints)
+        grid.clear_constraint_reservations()
+        grid.apply_constraint_reservations(all_constraints)
 
         # Just plan for agent with new constraint
         start_and_goal: StartAndGoal = ConflictBasedSearch.get_agents_start_and_goal(start_and_goals, constrained_agent.agent)
@@ -165,10 +148,31 @@ def plan_for_agent(constrained_agent: ConstrainedAgent,
             new_path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.agent_id, verbose)
             return new_path
         except Exception as e:
+            # Note: single agent planners can fail if constraints make planning for an agent impossible. The upper level search does not
+            # consider if a constraint will make it impossible for an agent to plan when adding a new constraint.
             if verbose:
                 print(f"Error: {e}")
             return None
 
+    @staticmethod
+    def initialize_constraint_tree(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> ConstraintTree:
+        """
+        Generate an initial solution by planning for each agent with no obstacles
+        """
+        initial_solution: dict[AgentId, NodePath] = {}
+
+        # Generate initial solution (no constraints)
+        for start_and_goal in start_and_goals:
+            path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.agent_id, verbose)
+            initial_solution[start_and_goal.agent_id] = path
+
+        if verbose:
+            print("Initial solution:")
+            for (agent_idx, path) in initial_solution.items():
+                print(f"\nAgent {agent_idx} path:\n {path}")
+
+        return ConstraintTree(initial_solution)
+
 class Scenario(Enum):
     # Five robots all trying to get through a single cell choke point to reach their goals
     NARROW_CORRIDOR = 0

PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py

@@ -59,7 +59,7 @@ class Grid:
     applied_constraints: list[AppliedConstraint] = []
     # Note - this is separate from reservation_matrix because multiple robots can have the same
     # constraints at the same (x, y, t) tuple
-    constraint_points: np.ndarray
+    constraint_reservations: np.ndarray
 
     # Number of time steps in the simulation
     time_limit: int
@@ -80,7 +80,7 @@ def __init__(
         self.grid_size = grid_size
         self.reservation_matrix = np.zeros((grid_size[0], grid_size[1], self.time_limit))
 
-        self.constraint_points = empty_3d_array_of_sets(grid_size[0], grid_size[1], self.time_limit)
+        self.constraint_reservations = empty_3d_array_of_sets(grid_size[0], grid_size[1], self.time_limit)
 
         if num_obstacles > self.grid_size[0] * self.grid_size[1]:
             raise Exception("Number of obstacles is greater than grid size!")
@@ -253,7 +253,7 @@ def generate_hallway_obstacles(self) -> list[list[Position]]:
 
         return obstacle_paths
 
-    def generate_temporary_obstacle(self, hallway_length: int = 3) -> list[list[Position]]:
+    def generate_temporary_obstacle(self) -> list[list[Position]]:
         """
         Generates a temporary obstacle at (10, 10) that disappears at t=30
         """
@@ -263,21 +263,27 @@ def generate_temporary_obstacle(self, hallway_length: int = 3) -> list[list[Posi
 
         return [obstacle_path]
 
-    def apply_constraint_points(self, constraints: list[AppliedConstraint], verbose = False):
+    def apply_constraint_reservations(self, constraints: list[AppliedConstraint], verbose = False):
+        """
+        Add new constraints and reserve them in the constraint_reservations map
+        """
         self.applied_constraints.extend(constraints)
         for constraint in constraints:
             if verbose:
                 print(f"Applying {constraint=}")
-            if constraint not in self.constraint_points[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time]:
-                self.constraint_points[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time].add(constraint)
+            if constraint not in self.constraint_reservations[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time]:
+                self.constraint_reservations[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time].add(constraint)
 
             if verbose:
-                print(f"\tExisting constraints: {self.constraint_points[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time]}")
+                print(f"\tExisting constraints: {self.constraint_reservations[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time]}")
 
-    def clear_constraint_points(self):
+    def clear_constraint_reservations(self):
+        """
+        Clear all applied constraints and their associated reservations
+        """
         for constraint in self.applied_constraints:
-            if constraint in self.constraint_points[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time]:
-                self.constraint_points[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time].remove(constraint)
+            if constraint in self.constraint_reservations[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time]:
+                self.constraint_reservations[constraint.constraint.position.x, constraint.constraint.position.y, constraint.constraint.time].remove(constraint)
         self.applied_constraints.clear()
 
     """
@@ -295,7 +301,7 @@ def valid_position(self, position: Position, t: int, agent_idx: int) -> bool:
         if not self.inside_grid_bounds(position):
             return False
 
-        constraints = self.constraint_points[position.x, position.y, t]
+        constraints = self.constraint_reservations[position.x, position.y, t]
         for constraint in constraints:
             if constraint.constrained_agent == agent_idx:
                 return False
@@ -366,7 +372,7 @@ def get_safe_intervals_at_cell(self, cell: Position, agent_idx: int) -> list[Int
         # Find where the array is zero
         zero_mask = (vals == 0)
 
-        for constraint_set in self.constraint_points[cell.x, cell.y]:
+        for constraint_set in self.constraint_reservations[cell.x, cell.y]:
             for constraint in constraint_set:
                 if constraint.constrained_agent != agent_idx:
                     continue