debugging sipp, not sure. this commit is mostly garbo

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py

@@ -101,7 +101,7 @@ def main():
     grid_side_length = 21
 
     # start_and_goals = [StartAndGoal(i, Position(1, i), Position(19, 19-i)) for i in range(1, 16)]
-    start_and_goals = [StartAndGoal(i, Position(1, 8+i), Position(19, 19-i)) for i in range(5)]
+    start_and_goals = [StartAndGoal(i, Position(1, 8+i), Position(19, 19-i)) for i in range(4)]
     obstacle_avoid_points = [pos for item in start_and_goals for pos in (item.start, item.goal)]
 
     grid = Grid(
@@ -114,7 +114,8 @@ def main():
     )
 
     start_time = time.time()
-    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/GridWithDynamicObstacles.py

@@ -81,6 +81,7 @@ def __init__(
             self.obstacle_paths = self.generate_narrow_corridor_obstacles(num_obstacles)
 
         for i, path in enumerate(self.obstacle_paths):
+            # TODO: i think this is a bug. obstacle indices will overlap with robot indices
             obs_idx = i + 1  # avoid using 0 - that indicates free space in the grid
             for t, position in enumerate(path):
                 # Reserve old & new position at this time step
@@ -282,20 +283,36 @@ def get_obstacle_positions_at_time(self, t: int) -> tuple[list[int], list[int]]:
     """
     Returns safe intervals for each cell.
     """
-    def get_safe_intervals(self) -> np.ndarray:
+    def get_safe_intervals(self, agent_idx: int) -> np.ndarray:
         intervals = empty_2d_array_of_lists(self.grid_size[0], self.grid_size[1])
         for x in range(intervals.shape[0]):
             for y in range(intervals.shape[1]):
-                intervals[x, y] = self.get_safe_intervals_at_cell(Position(x, y))
+                intervals[x, y] = self.get_safe_intervals_at_cell(Position(x, y), agent_idx)
 
         return intervals
 
     """
     Generate the safe intervals for a given cell. The intervals will be in order of start time.
     ex: Interval (2, 3) will be before Interval (4, 5)
     """
-    def get_safe_intervals_at_cell(self, cell: Position) -> list[Interval]:
+    def get_safe_intervals_at_cell(self, cell: Position, agent_idx: int) -> list[Interval]:
         vals = self.reservation_matrix[cell.x, cell.y, :]
+
+        had_constraints = False
+        # ct: AppliedConstraint
+        for constraint_set in self.constraint_points[cell.x, cell.y]:
+            for constraint in constraint_set:
+                if constraint.constrained_agent != agent_idx:
+                    continue
+                if constraint.constraint.position != cell:
+                    continue
+                had_constraints = True
+                vals[constraint.constraint.time] = 99999 # TODO: no magic numbers
+                
+                # TODO: hack
+                # if constraint.constraint.time + 1 < self.time_limit:
+                #     vals[constraint.constraint.time + 1] = 99999 # TODO: no magic numbers
+
         # Find where the array is zero
         zero_mask = (vals == 0)
 
@@ -321,6 +338,10 @@ def get_safe_intervals_at_cell(self, cell: Position) -> list[Interval]:
         # move into and out of the cell each take 1 time step, and the cell is considered occupied during
         # both the time step when it is entering the cell,  and the time step when it is leaving the cell.
         intervals = [interval for interval in intervals if interval.start_time != interval.end_time]
+
+        # if had_constraints:
+            # print("\t\tconstraints: ", self.constraint_points[cell.x, cell.y])
+            # print("\t\tintervals: ", intervals)
         return intervals
 
     """

PathPlanning/TimeBasedPathPlanning/SafeInterval.py

@@ -49,7 +49,7 @@ class SafeIntervalPathPlanner(SingleAgentPlanner):
     @staticmethod
     def plan(grid: Grid, start: Position, goal: Position, agent_idx: int, verbose: bool = False) -> NodePath:
 
-        safe_intervals = grid.get_safe_intervals()
+        safe_intervals = grid.get_safe_intervals(agent_idx)
 
         open_set: list[SIPPNode] = []
         first_node_interval = safe_intervals[start.x, start.y][0]