add random positions start_and_goals

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py

@@ -68,9 +68,6 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
 
             # TODO: contents of this loop should probably be in a helper?
             for constrained_agent in constraint_tree_node.constraint.constrained_agents:
-                num_expansions = constraint_tree.expanded_node_count()
-                if num_expansions % 50 == 0:
-                    print(f"Expanded {num_expansions} nodes so far...")
                 if verbose:
                     print(f"\nOuter loop step for agent {constrained_agent}")
 
@@ -80,6 +77,11 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c
                 all_constraints = deepcopy(ancestor_constraints) # TODO - no deepcopy pls
                 all_constraints.append(applied_constraint)
 
+                num_expansions = constraint_tree.expanded_node_count()
+                if num_expansions % 50 == 0:
+                    print(f"Expanded {num_expansions} nodes so far...")
+                    print(f"\tlen of constraints {len(all_constraints)}")
+
                 # Skip if we have already tried this set of constraints
                 constraint_hash = hash(frozenset(all_constraints))
                 if constraint_hash in attempted_constraint_combos:
@@ -143,14 +145,26 @@ def find_by_index(start_and_goal_list: list[StartAndGoal], target_index: AgentId
 # * SIPP stinks at 3 robots in the hallway case
 verbose = False
 show_animation = True
+np.random.seed(42)  # For reproducibility
 def main():
     grid_side_length = 21
 
     # 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)]
 
+    # generate random start and goals
+    start_and_goals: list[StartAndGoal] = []
+    for i in range(40):
+        start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length))
+        goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length))
+        while any([start_and_goal.start == start for start_and_goal in start_and_goals]):
+            start = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length))
+            goal = Position(np.random.randint(0, grid_side_length), np.random.randint(0, grid_side_length))
+
+        start_and_goals.append(StartAndGoal(i, start, goal))
+
     # hallway cross
     # start_and_goals = [StartAndGoal(0, Position(6, 10), Position(13, 10)),
     #                    StartAndGoal(1, Position(11, 10), Position(6, 10)),
@@ -169,14 +183,15 @@ def main():
         obstacle_avoid_points=obstacle_avoid_points,
         # obstacle_arrangement=ObstacleArrangement.TEMPORARY_OBSTACLE,
         # obstacle_arrangement=ObstacleArrangement.HALLWAY,
-        obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR,
+        # obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR,
+        obstacle_arrangement=ObstacleArrangement.NONE,
         # 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

@@ -142,7 +142,7 @@ def get_ancestor_constraints(self, parent_index: int) -> list[AppliedConstraint]
             if node.constraint and isinstance(node.constraint, AppliedConstraint):
                 constraints.append(node.constraint)
             else:
-                print(f"Aha!!! {node.constraint}")
+                raise RuntimeError(f"Expected AppliedConstraint, but got: {node.constraint}")
             parent_index = node.parent_idx
         return constraints
 

PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py

@@ -16,16 +16,18 @@ class Interval:
     end_time: int
 
 class ObstacleArrangement(Enum):
+    # No obstacles
+    NONE = 0
     # Random obstacle positions and movements
-    RANDOM = 0
+    RANDOM = 1
     # Obstacles start in a line in y at center of grid and move side-to-side in x
-    ARRANGEMENT1 = 1
+    ARRANGEMENT1 = 2
     # Static obstacle arrangement
-    NARROW_CORRIDOR = 2
+    NARROW_CORRIDOR = 3
     # A hallway surrounded by obstacles with a 2-cell opening in the middle
-    HALLWAY = 3
+    HALLWAY = 4
     # A temporary obstacle that vanishes after some time
-    TEMPORARY_OBSTACLE = 4
+    TEMPORARY_OBSTACLE = 5
 
 """
 Generates a 2d numpy array with lists for elements.
@@ -91,6 +93,8 @@ def __init__(
             self.obstacle_paths = self.generate_hallway_obstacles()
         elif obstacle_arrangement == ObstacleArrangement.TEMPORARY_OBSTACLE:
             self.obstacle_paths = self.generate_temporary_obstacle()
+        elif obstacle_arrangement == ObstacleArrangement.NONE:
+            self.obstacle_paths = []
 
         for i, path in enumerate(self.obstacle_paths):
             # TODO: i think this is a bug. obstacle indices will overlap with robot indices