started sketching solution

Author: SchwartzCode

PathPlanning/TimeBasedPathPlanning/ConflictBasedSearch.py

@@ -0,0 +1,100 @@
+"""
+TODO - docstring
+
+Algorithm is defined in this paper: https://cdn.aaai.org/ojs/8140/8140-13-11667-1-2-20201228.pdf
+"""
+
+import numpy as np
+from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import (
+    Grid,
+    Interval,
+    ObstacleArrangement,
+    Position,
+)
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import MultiAgentPlanner, StartAndGoal
+from PathPlanning.TimeBasedPathPlanning.Node import NodePath
+from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner
+from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner
+from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePaths
+import time
+
+class ConflictBasedSearch(MultiAgentPlanner):
+
+    @staticmethod
+    def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]:
+        """
+        Generate a path from the start to the goal for each agent in the `start_and_goals` list.
+        Returns the re-ordered StartAndGoal combinations, and a list of path plans. The order of the plans
+        corresponds to the order of the `start_and_goals` list.
+        """
+        print(f"Using single-agent 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))
+
+        # Plan in descending order of distance from start to goal
+        # TODO: dont bother doing this
+        start_and_goals = sorted(start_and_goals,
+                    key=lambda item: item.distance_start_to_goal(),
+                    reverse=True)
+
+        # first, plan optimally for each agent
+        # now in a loop:
+        #   * 
+
+        # paths = []
+        # for start_and_goal in start_and_goals:
+        #     if verbose:
+        #         print(f"\nPlanning for agent:  {start_and_goal}" )
+
+        #     grid.clear_initial_reservation(start_and_goal.start, start_and_goal.index)
+        #     path = single_agent_planner_class.plan(grid, start_and_goal.start, start_and_goal.goal, verbose)
+
+        #     if path is None:
+        #         print(f"Failed to find path for {start_and_goal}")
+        #         return []
+
+        #     agent_index = start_and_goal.index
+        #     grid.reserve_path(path, agent_index)
+        #     paths.append(path)
+
+        return (start_and_goals, paths)
+
+    def cbs():
+
+verbose = False
+show_animation = True
+def main():
+    grid_side_length = 21
+
+    start_and_goals = [StartAndGoal(i, Position(1, i), Position(19, 19-i)) for i in range(1, 16)]
+    obstacle_avoid_points = [pos for item in start_and_goals for pos in (item.start, item.goal)]
+
+    grid = Grid(
+        np.array([grid_side_length, grid_side_length]),
+        num_obstacles=250,
+        obstacle_avoid_points=obstacle_avoid_points,
+        # 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)
+
+    runtime = time.time() - start_time
+    print(f"\nPlanning took: {runtime:.5f} seconds")
+
+    if verbose:
+        print(f"Paths:")
+        for path in paths:
+            print(f"{path}\n")
+
+    if not show_animation:
+        return
+
+    PlotNodePaths(grid, start_and_goals, paths)
+
+if __name__ == "__main__":
+    main()

PathPlanning/TimeBasedPathPlanning/ConstraintTree.py

@@ -0,0 +1,106 @@
+from dataclasses import dataclass
+from typing import Optional, TypeAlias
+import heapq
+
+from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position
+
+AgentId: TypeAlias = int
+
+@dataclass
+class Constraint:
+    position: Position
+    time: int
+
+@dataclass
+class PathConstraint:
+    constraint: Constraint
+    shorter_path_agent: AgentId
+    longer_path_agent: AgentId
+
+@dataclass
+class ConstraintTreeNode:
+    parent_idx = int
+    constraint: tuple[AgentId, Constraint]
+
+    paths: dict[AgentId, NodePath]
+    cost: int
+
+    def __lt__(self, other) -> bool:
+        # TODO - this feels jank?
+        return self.cost + self.constrained_path_cost() < other.cost + other.constrained_path_cost()
+
+    def get_constraint_point(self) -> Optional[PathConstraint]:
+        final_t = max(path.goal_reached_time() for path in self.paths)
+        positions_at_time: dict[Position, AgentId] = {}
+        for t in range(final_t + 1):
+            # TODO: need to be REALLY careful that these agent ids are consitent
+            for agent_id, path in self.paths.items():
+                position = path.get_position(t)
+                if position is None:
+                    continue
+                if position in positions_at_time:
+                    conflicting_agent_id = positions_at_time[position]
+                    this_agent_shorter = self.paths[agent_id].goal_reached_time() < self.paths[conflicting_agent_id].goal_reached_time()
+
+                    return PathConstraint(
+                        constraint=Constraint(position=position, time=t),
+                        shorter_path_agent= agent_id if this_agent_shorter else conflicting_agent_id,
+                        longer_path_agent= conflicting_agent_id if this_agent_shorter else agent_id
+                    )
+        return None
+
+    def constrained_path_cost(self) -> int:
+        constrained_path = self.paths[self.constraint[0]]
+        return constrained_path.goal_reached_time()
+
+class ConstraintTree:
+    # Child nodes have been created (Maps node_index to ConstraintTreeNode)
+    expanded_nodes: dict[int, ConstraintTreeNode]
+    # Need to solve and generate children
+    nodes_to_expand: heapq[ConstraintTreeNode]
+
+    solution: Optional[ConstraintTreeNode] = None
+
+    def __init__(self, initial_solution: dict[AgentId, NodePath]):
+        initial_cost = sum(path.goal_reached_time() for path in initial_solution.values())
+        heapq.heappush(self.nodes_to_expand, ConstraintTreeNode(constraints={}, paths=initial_solution, cost=initial_cost, parent_idx=-1))
+
+    def get_next_node_to_expand(self) -> Optional[ConstraintTreeNode]:
+        if not self.nodes_to_expand:
+            return None
+        return heapq.heappop(self.nodes_to_expand)
+    
+    def add_node_to_tree(self, node: ConstraintTreeNode) -> bool:
+        """
+        Add a node to the tree and generate children if needed. Returns true if the node is a solution, false otherwise.
+        """
+        node_index = len(self.expanded_nodes)
+        self.expanded_nodes[node_index] = node
+        constraint_point = node.get_constraint_point()
+        if constraint_point is None:
+            # Don't need to add any constraints, this is a solution!
+            self.solution = node
+            return
+        
+        child_node1 = node
+        child_node1.constraint = (constraint_point.shorter_path_agent, constraint_point.constraint)
+        child_node1.parent_idx = node_index
+
+        child_node2 = node
+        child_node2.constraint = (constraint_point.longer_path_agent, constraint_point.constraint)
+        child_node2.parent_idx = node_index
+
+        heapq.heappush(self.nodes_to_expand, child_node1)
+        heapq.heappush(self.nodes_to_expand, child_node2)
+    
+    def get_ancestor_constraints(self, parent_index: int):
+        """
+        Get the constraints that were applied to the parent node to generate this node.
+        """
+        constraints = []
+        while parent_index != -1:
+            node = self.expanded_nodes[parent_index]
+            if node.constraint is not None:
+                constraints.append(node.constraint)
+            parent_index = node.parent_idx
+        return constraints