some more debugging, fixed issues with agent id expecting in order + better constraint finding

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/BaseClasses.py

@@ -26,6 +26,7 @@ def plan(grid: Grid, start: Position, goal: Position, agent_idx: int, verbose: b
 @dataclass
 class StartAndGoal:
     # Index of this agent
+    # TODO: better name and use AgentId type
     index: int
     # Start position of the robot
     start: Position

PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py

@@ -34,12 +34,12 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
         initial_solution: dict[AgentId, NodePath] = {}
 
         # Generate initial solution (no reservations for robots)
-        for agent_idx, start_and_goal in enumerate(start_and_goals):
-            path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, agent_idx, verbose)
-            initial_solution[AgentId(agent_idx)] = path
+        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.index, verbose)
+            initial_solution[AgentId(start_and_goal.index)] = path
 
-        # for (agent_idx, path) in initial_solution.items():
-        #     print(f"\nAgent {agent_idx} path:\n {path}")
+        for (agent_idx, path) in initial_solution.items():
+            print(f"\nAgent {agent_idx} path:\n {path}")
 
         constraint_tree = ConstraintTree(initial_solution)
 
@@ -49,17 +49,17 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
             constraint_tree_node = constraint_tree.get_next_node_to_expand()
             ancestor_constraints = constraint_tree.get_ancestor_constraints(constraint_tree_node.parent_idx)
 
-            # print(f"Expanded node: {constraint_tree_node.constraint} with parent: {constraint_tree_node.parent_idx}")
-            # print(f"\tAncestor constraints: {ancestor_constraints}")
+            print(f"Expanded node: {constraint_tree_node.constraint} with parent: {constraint_tree_node.parent_idx}")
+            print(f"\tAncestor constraints: {ancestor_constraints}")
 
             if verbose:
-                print(f"Expanding node with constraint {constraint_tree_node.constraint} and parent {constraint_tree_node.parent_idx} ")
+                print(f"Expanding node with constraint {constraint_tree_node.constraint} and parent {constraint_tree_node.parent_idx}")
 
             if constraint_tree_node is None:
                 raise RuntimeError("No more nodes to expand in the constraint tree.")
             if not constraint_tree_node.constraint:
                 # This means we found a solution!
-                return (start_and_goals, [constraint_tree_node.paths[AgentId(i)] for i in range(len(start_and_goals))])
+                return (start_and_goals, [constraint_tree_node.paths[start_and_goal.index] for start_and_goal in start_and_goals])
 
             if not isinstance(constraint_tree_node.constraint, ForkingConstraint):
                 raise ValueError(f"Expected a ForkingConstraint, but got: {constraint_tree_node.constraint}")
@@ -78,19 +78,22 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
                 # Skip if we have already tried this set of constraints
                 constraint_hash = hash(frozenset(all_constraints))
                 if constraint_hash in attempted_constraint_combos:
-                    break
+                    print(f"\tSkipping already attempted constraint combination: {all_constraints}")
+                    continue
                 else:
                     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)
 
                 # Just plan for agent with new constraint
-                start_and_goal = start_and_goals[constrained_agent.agent]
+                start_and_goal = ConflictBasedSearch.find_by_index(start_and_goals, constrained_agent.agent)
                 try:
+                    print("\tplanning for: {}", start_and_goal)
                     new_path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, start_and_goal.index, verbose)
                 except Exception as e:
                     continue
@@ -99,8 +102,8 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
                 paths: dict[AgentId, NodePath] = deepcopy(applied_constraint_parent.paths) # TODO: not sure if deepcopy is actually needed
                 paths[constrained_agent.agent] = new_path
 
-                # for (agent_idx, path) in paths.items():
-                #     print(f"\nAgent {agent_idx} path:\n {path}")
+                for (agent_idx, path) in 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)
@@ -115,10 +118,18 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
                     return (start_and_goals, paths.values())
 
                 # if verbose:
-                print(f"Adding new constraint tree node with constraint: {new_constraint_tree_node.constraint}")
+                # print(f"Adding new constraint tree node with constraint: {new_constraint_tree_node.constraint}")
                 constraint_tree.add_node_to_tree(new_constraint_tree_node)
 
         raise RuntimeError("No solution found")
+    
+    # TODO: bad function name
+    def find_by_index(start_and_goal_list: list[StartAndGoal], target_index: AgentId) -> AgentId:
+        for item in start_and_goal_list:
+            if item.index == target_index:
+                return item
+        raise RuntimeError(f"Could not find agent with index {target_index} in {start_and_goal_list}")
+
 # TODO
 # * get SIPP working w CBS
 # * add checks for duplicate expansions
@@ -132,13 +143,13 @@ def main():
 
     # TODO: bug somewhere where it expects agent ids to match indices
     # start_and_goals = [StartAndGoal(i, Position(1, i), Position(19, 19-i)) for i in range(1, 12)]
-    start_and_goals = [StartAndGoal(i, Position(1, 8+i), Position(19, 19-i)) for i in range(6)]
+    # start_and_goals = [StartAndGoal(i, Position(1, 8+i), Position(19, 19-i)) for i in range(6)]
     # start_and_goals = [StartAndGoal(i, Position(1, 2*i), Position(19, 19-i)) for i in range(4)]
 
     # hallway cross
-    # start_and_goals = [StartAndGoal(0, Position(6, 10), Position(13, 10)),
-    #                    StartAndGoal(1, Position(13, 10), Position(7, 10)),
-    #                    StartAndGoal(2, Position(11, 10), Position(6, 10))]
+    start_and_goals = [StartAndGoal(0, Position(6, 10), Position(13, 10)),
+                       StartAndGoal(1, Position(11, 10), Position(6, 10)),
+                       StartAndGoal(2, Position(13, 10), Position(7, 10))]
 
     # temporary obstacle
     # start_and_goals = [StartAndGoal(0, Position(15, 14), Position(15, 16))]
@@ -152,15 +163,15 @@ def main():
         num_obstacles=250,
         obstacle_avoid_points=obstacle_avoid_points,
         # obstacle_arrangement=ObstacleArrangement.TEMPORARY_OBSTACLE,
-        # obstacle_arrangement=ObstacleArrangement.HALLWAY,
-        obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR,
+        obstacle_arrangement=ObstacleArrangement.HALLWAY,
+        # obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR,
         # obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1,
         # obstacle_arrangement=ObstacleArrangement.RANDOM,
     )
 
     start_time = time.time()
-    start_and_goals, paths = ConflictBasedSearch.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose)
-    # start_and_goals, paths = ConflictBasedSearch.plan(grid, start_and_goals, SpaceTimeAStar, verbose)
+    # start_and_goals, paths = ConflictBasedSearch.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose)
+    start_and_goals, paths = ConflictBasedSearch.plan(grid, start_and_goals, SpaceTimeAStar, verbose)
 
     runtime = time.time() - start_time
     print(f"\nPlanning took: {runtime:.5f} seconds")

PathPlanning/TimeBasedPathPlanning/ConstraintTree.py

@@ -51,6 +51,7 @@ def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]:
         positions_at_time: dict[PositionAtTime, AgentId] = {}
         for t in range(final_t + 1):
             # TODO: need to be REALLY careful that these agent ids are consitent
+            possible_constraints: list[ForkingConstraint] = []
             for agent_id, path in self.paths.items():
                 # Check for edge conflicts
                 last_position = None
@@ -62,19 +63,10 @@ def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]:
                     continue
                 # print(f"\treserving pos/t for {agent_id}: {position} @ {t}")
                 position_at_time = PositionAtTime(position, t)
-                if position_at_time in positions_at_time:
-                    conflicting_agent_id = positions_at_time[position_at_time]
-
-                    if verbose:
-                    # if True:
-                        print(f"found constraint: {position_at_time} for agents {agent_id} & {conflicting_agent_id}")
-                    constraint = Constraint(position=position, time=t)
-                    return ForkingConstraint((
-                        ConstrainedAgent(agent_id, constraint), ConstrainedAgent(conflicting_agent_id, constraint)
-                    ))
-                else:
+                if position_at_time not in positions_at_time:
                     positions_at_time[position_at_time] = AgentId(agent_id)
 
+                # edge conflict
                 if last_position:
                     new_position_at_last_time = PositionAtTime(position, t-1)
                     old_position_at_new_time = PositionAtTime(last_position, t)
@@ -89,8 +81,29 @@ def get_constraint_point(self, verbose = False) -> Optional[ForkingConstraint]:
                                 ConstrainedAgent(agent_id, position_at_time),
                                 ConstrainedAgent(conflicting_agent_id1, Constraint(position=last_position, time=t))
                             ))
-                            print(f"new constraint: {new_constraint}")
-                            return new_constraint
+                            possible_constraints.append(new_constraint)
+                            continue
+                
+                # double reservation at a (cell, time) combination
+                if positions_at_time[position_at_time] != agent_id:
+                    conflicting_agent_id = positions_at_time[position_at_time]
+
+                    constraint = Constraint(position=position, time=t)
+                    possible_constraints.append(ForkingConstraint((
+                        ConstrainedAgent(agent_id, constraint), ConstrainedAgent(conflicting_agent_id, constraint)
+                    )))
+                    continue
+            if possible_constraints:
+                print(f"choosing best constraint of {possible_constraints}")
+                # first check for edge constraints
+                for constraint in possible_constraints:
+                    if constraint.constrained_agents[0].constraint.position != constraint.constrained_agents[1].constraint.position:
+                        print(f"\tfound edge conflict constraint: {constraint}")
+                        return constraint
+                # if none, then return first normal constraint
+                print("\treturning normal constraint")
+                return possible_constraints[0]
+
         return None