From 9fb1b7b28c43a28cf831ae96ce7e6014911c0969 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Apr 2025 19:38:13 -0400 Subject: [PATCH 01/22] consolidate Node definition --- PathPlanning/TimeBasedPathPlanning/Node.py | 65 ++++++++++++++++ .../TimeBasedPathPlanning/SafeInterval.py | 77 +++---------------- .../TimeBasedPathPlanning/SpaceTimeAStar.py | 63 +-------------- 3 files changed, 77 insertions(+), 128 deletions(-) create mode 100644 PathPlanning/TimeBasedPathPlanning/Node.py diff --git a/PathPlanning/TimeBasedPathPlanning/Node.py b/PathPlanning/TimeBasedPathPlanning/Node.py new file mode 100644 index 0000000000..648a2980f9 --- /dev/null +++ b/PathPlanning/TimeBasedPathPlanning/Node.py @@ -0,0 +1,65 @@ +from dataclasses import dataclass +from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( + Position, +) +from functools import total_ordering + +@dataclass() +# Note: Total_ordering is used instead of adding `order=True` to the @dataclass decorator because +# this class needs to override the __lt__ and __eq__ methods to ignore parent_index. Parent +# index is just used to track the path found by the algorithm, and has no effect on the quality +# of a node. +@total_ordering +class Node: + position: Position + time: int + heuristic: int + parent_index: int + + """ + This is what is used to drive node expansion. The node with the lowest value is expanded next. + This comparison prioritizes the node with the lowest cost-to-come (self.time) + cost-to-go (self.heuristic) + """ + def __lt__(self, other: object): + if not isinstance(other, Node): + return NotImplementedError(f"Cannot compare Node with object of type: {type(other)}") + return (self.time + self.heuristic) < (other.time + other.heuristic) + + """ + Note: cost and heuristic are not included in eq or hash, since they will always be the same + for a given (position, time) pair. Including either cost or heuristic would be redundant. + """ + def __eq__(self, other: object): + if not isinstance(other, Node): + return NotImplementedError(f"Cannot compare Node with object of type: {type(other)}") + return self.position == other.position and self.time == other.time + + def __hash__(self): + return hash((self.position, self.time)) + +class NodePath: + path: list[Node] + positions_at_time: dict[int, Position] = {} + + def __init__(self, path: list[Node]): + self.path = path + for node in path: + self.positions_at_time[node.time] = node.position + + """ + Get the position of the path at a given time + """ + def get_position(self, time: int) -> Position | None: + return self.positions_at_time.get(time) + + """ + Time stamp of the last node in the path + """ + def goal_reached_time(self) -> int: + return self.path[-1].time + + def __repr__(self): + repr_string = "" + for i, node in enumerate(self.path): + repr_string += f"{i}: {node}\n" + return repr_string \ No newline at end of file diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py index 7fea10c67f..ab97b14155 100644 --- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py +++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py @@ -17,6 +17,7 @@ Position, empty_2d_array_of_lists, ) +from PathPlanning.TimeBasedPathPlanning.Node import Node, NodePath import heapq import random from dataclasses import dataclass @@ -29,70 +30,14 @@ # index and interval member variables are just used to track the path found by the algorithm, # and has no effect on the quality of a node. @total_ordering -class Node: - position: Position - time: int - heuristic: int - parent_index: int +class SIPPNode(Node): interval: Interval - """ - This is what is used to drive node expansion. The node with the lowest value is expanded next. - This comparison prioritizes the node with the lowest cost-to-come (self.time) + cost-to-go (self.heuristic) - """ - def __lt__(self, other: object): - if not isinstance(other, Node): - return NotImplementedError(f"Cannot compare Node with object of type: {type(other)}") - return (self.time + self.heuristic) < (other.time + other.heuristic) - - """ - Equality only cares about position and time. Heuristic and interval will always be the same for a given - (position, time) pairing, so they are not considered in equality. - """ - def __eq__(self, other: object): - if not isinstance(other, Node): - return NotImplemented - return self.position == other.position and self.time == other.time - @dataclass class EntryTimeAndInterval: entry_time: int interval: Interval -class NodePath: - path: list[Node] - positions_at_time: dict[int, Position] = {} - - def __init__(self, path: list[Node]): - self.path = path - for (i, node) in enumerate(path): - if i > 0: - # account for waiting in interval at previous node - prev_node = path[i-1] - for t in range(prev_node.time, node.time): - self.positions_at_time[t] = prev_node.position - - self.positions_at_time[node.time] = node.position - - """ - Get the position of the path at a given time - """ - def get_position(self, time: int) -> Position | None: - return self.positions_at_time.get(time) - - """ - Time stamp of the last node in the path - """ - def goal_reached_time(self) -> int: - return self.path[-1].time - - def __repr__(self): - repr_string = "" - for i, node in enumerate(self.path): - repr_string += f"{i}: {node}\n" - return repr_string - - class SafeIntervalPathPlanner: grid: Grid start: Position @@ -117,16 +62,16 @@ def plan(self, verbose: bool = False) -> NodePath: safe_intervals = self.grid.get_safe_intervals() - open_set: list[Node] = [] + open_set: list[SIPPNode] = [] first_node_interval = safe_intervals[self.start.x, self.start.y][0] heapq.heappush( - open_set, Node(self.start, 0, self.calculate_heuristic(self.start), -1, first_node_interval) + open_set, SIPPNode(self.start, 0, self.calculate_heuristic(self.start), -1, first_node_interval) ) - expanded_list: list[Node] = [] + expanded_list: list[SIPPNode] = [] visited_intervals = empty_2d_array_of_lists(self.grid.grid_size[0], self.grid.grid_size[1]) while open_set: - expanded_node: Node = heapq.heappop(open_set) + expanded_node: SIPPNode = heapq.heappop(open_set) if verbose: print("Expanded node:", expanded_node) @@ -138,7 +83,7 @@ def plan(self, verbose: bool = False) -> NodePath: if expanded_node.position == self.goal: print(f"Found path to goal after {len(expanded_list)} expansions") path = [] - path_walker: Node = expanded_node + path_walker: SIPPNode = expanded_node while True: path.append(path_walker) if path_walker.parent_index == -1: @@ -163,8 +108,8 @@ def plan(self, verbose: bool = False) -> NodePath: Generate list of possible successors of the provided `parent_node` that are worth expanding """ def generate_successors( - self, parent_node: Node, parent_node_idx: int, intervals: np.ndarray, visited_intervals: np.ndarray - ) -> list[Node]: + self, parent_node: SIPPNode, parent_node_idx: int, intervals: np.ndarray, visited_intervals: np.ndarray + ) -> list[SIPPNode]: new_nodes = [] diffs = [ Position(0, 0), @@ -204,7 +149,7 @@ def generate_successors( # new interval can be entered for possible_t in range(max(parent_node.time + 1, interval.start_time), min(current_interval.end_time, interval.end_time)): if self.grid.valid_position(new_pos, possible_t): - new_nodes.append(Node( + new_nodes.append(SIPPNode( new_pos, # entry is max of interval start and parent node time + 1 (get there as soon as possible) max(interval.start_time, parent_node.time + 1), @@ -229,7 +174,7 @@ def calculate_heuristic(self, position) -> int: Adds a new entry to the visited intervals array. If the entry is already present, the entry time is updated if the new entry time is better. Otherwise, the entry is added to `visited_intervals` at the position of `expanded_node`. """ -def add_entry_to_visited_intervals_array(entry_time_and_interval: EntryTimeAndInterval, visited_intervals: np.ndarray, expanded_node: Node): +def add_entry_to_visited_intervals_array(entry_time_and_interval: EntryTimeAndInterval, visited_intervals: np.ndarray, expanded_node: SIPPNode): # if entry is present, update entry time if better for existing_entry_and_interval in visited_intervals[expanded_node.position.x, expanded_node.position.y]: if existing_entry_and_interval.interval == entry_time_and_interval.interval: diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py index c4e2802d37..ce7debd310 100644 --- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py +++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py @@ -15,11 +15,10 @@ ObstacleArrangement, Position, ) +from PathPlanning.TimeBasedPathPlanning.Node import Node, NodePath import heapq from collections.abc import Generator import random -from dataclasses import dataclass -from functools import total_ordering import time # Seed randomness for reproducibility @@ -27,66 +26,6 @@ random.seed(RANDOM_SEED) np.random.seed(RANDOM_SEED) -@dataclass() -# Note: Total_ordering is used instead of adding `order=True` to the @dataclass decorator because -# this class needs to override the __lt__ and __eq__ methods to ignore parent_index. Parent -# index is just used to track the path found by the algorithm, and has no effect on the quality -# of a node. -@total_ordering -class Node: - position: Position - time: int - heuristic: int - parent_index: int - - """ - This is what is used to drive node expansion. The node with the lowest value is expanded next. - This comparison prioritizes the node with the lowest cost-to-come (self.time) + cost-to-go (self.heuristic) - """ - def __lt__(self, other: object): - if not isinstance(other, Node): - return NotImplementedError(f"Cannot compare Node with object of type: {type(other)}") - return (self.time + self.heuristic) < (other.time + other.heuristic) - - """ - Note: cost and heuristic are not included in eq or hash, since they will always be the same - for a given (position, time) pair. Including either cost or heuristic would be redundant. - """ - def __eq__(self, other: object): - if not isinstance(other, Node): - return NotImplementedError(f"Cannot compare Node with object of type: {type(other)}") - return self.position == other.position and self.time == other.time - - def __hash__(self): - return hash((self.position, self.time)) - -class NodePath: - path: list[Node] - positions_at_time: dict[int, Position] = {} - - def __init__(self, path: list[Node]): - self.path = path - for node in path: - self.positions_at_time[node.time] = node.position - - """ - Get the position of the path at a given time - """ - def get_position(self, time: int) -> Position | None: - return self.positions_at_time.get(time) - - """ - Time stamp of the last node in the path - """ - def goal_reached_time(self) -> int: - return self.path[-1].time - - def __repr__(self): - repr_string = "" - for i, node in enumerate(self.path): - repr_string += f"{i}: {node}\n" - return repr_string - class SpaceTimeAStar: grid: Grid From 1c5be77b8ea9851360649304af082195890a2f6c Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Apr 2025 19:49:15 -0400 Subject: [PATCH 02/22] add base class for single agent planner --- .../TimeBasedPathPlanning/SafeInterval.py | 16 ++--------- .../SingleAgentPlannerBase.py | 27 +++++++++++++++++++ .../TimeBasedPathPlanning/SpaceTimeAStar.py | 13 ++------- 3 files changed, 31 insertions(+), 25 deletions(-) create mode 100644 PathPlanning/TimeBasedPathPlanning/SingleAgentPlannerBase.py diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py index ab97b14155..0985e37fab 100644 --- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py +++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py @@ -18,6 +18,7 @@ empty_2d_array_of_lists, ) from PathPlanning.TimeBasedPathPlanning.Node import Node, NodePath +from PathPlanning.TimeBasedPathPlanning.SingleAgentPlannerBase import SingleAgentPlanner import heapq import random from dataclasses import dataclass @@ -38,20 +39,7 @@ class EntryTimeAndInterval: entry_time: int interval: Interval -class SafeIntervalPathPlanner: - grid: Grid - start: Position - goal: Position - - def __init__(self, grid: Grid, start: Position, goal: Position): - self.grid = grid - self.start = start - self.goal = goal - - # Seed randomness for reproducibility - RANDOM_SEED = 50 - random.seed(RANDOM_SEED) - np.random.seed(RANDOM_SEED) +class SafeIntervalPathPlanner(SingleAgentPlanner): """ Generate a plan given the loaded problem statement. Raises an exception if it fails to find a path. diff --git a/PathPlanning/TimeBasedPathPlanning/SingleAgentPlannerBase.py b/PathPlanning/TimeBasedPathPlanning/SingleAgentPlannerBase.py new file mode 100644 index 0000000000..68739a1694 --- /dev/null +++ b/PathPlanning/TimeBasedPathPlanning/SingleAgentPlannerBase.py @@ -0,0 +1,27 @@ +from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( + Grid, + Position, +) +from PathPlanning.TimeBasedPathPlanning.Node import NodePath +import random +import numpy.random as numpy_random +from abc import ABC, abstractmethod + +class SingleAgentPlanner(ABC): + """ + Base class for single agent path planners. + """ + + def __init__(self, grid: Grid, start: Position, goal: Position): + self.grid = grid + self.start = start + self.goal = goal + + # Seed randomness for reproducibility + RANDOM_SEED = 50 + random.seed(RANDOM_SEED) + numpy_random.seed(RANDOM_SEED) + + @abstractmethod + def plan(self, verbose: bool = False) -> NodePath: + pass diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py index ce7debd310..43d18f4124 100644 --- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py +++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py @@ -20,6 +20,7 @@ from collections.abc import Generator import random import time +from PathPlanning.TimeBasedPathPlanning.SingleAgentPlannerBase import SingleAgentPlanner # Seed randomness for reproducibility RANDOM_SEED = 50 @@ -27,17 +28,7 @@ np.random.seed(RANDOM_SEED) -class SpaceTimeAStar: - grid: Grid - start: Position - goal: Position - # Used to evaluate solutions - expanded_node_count: int = -1 - - def __init__(self, grid: Grid, start: Position, goal: Position): - self.grid = grid - self.start = start - self.goal = goal +class SpaceTimeAStar(SingleAgentPlanner): def plan(self, verbose: bool = False) -> NodePath: open_set: list[Node] = [] From a481f61340753e1cde71b3b7cdb265f7dbf0b1c8 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Apr 2025 20:18:11 -0400 Subject: [PATCH 03/22] add base class for single agent planner --- .../TimeBasedPathPlanning/BaseClasses.py | 51 +++++++++++++++++++ .../TimeBasedPathPlanning/SafeInterval.py | 37 +++++++------- .../SingleAgentPlannerBase.py | 27 ---------- .../TimeBasedPathPlanning/SpaceTimeAStar.py | 29 ++++++----- 4 files changed, 85 insertions(+), 59 deletions(-) create mode 100644 PathPlanning/TimeBasedPathPlanning/BaseClasses.py delete mode 100644 PathPlanning/TimeBasedPathPlanning/SingleAgentPlannerBase.py diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py new file mode 100644 index 0000000000..7351660ec7 --- /dev/null +++ b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py @@ -0,0 +1,51 @@ +from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( + Grid, + Position, +) +from PathPlanning.TimeBasedPathPlanning.Node import NodePath +import random +import numpy.random as numpy_random +from abc import ABC, abstractmethod +from dataclasses import dataclass + +class SingleAgentPlanner(ABC): + """ + Base class for single agent planners + """ + + @staticmethod + @abstractmethod + def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> NodePath: + pass + +@dataclass +class StartAndGoal: + # Start position of the robot + start: Position + # Goal position of the robot + goal: Position + +class MultiAgentPlanner(ABC): + """ + Base class for multi-agent planners + """ + + def __init__(self, grid: Grid, start_and_goal_positions: list[StartAndGoal]): + self.grid = grid + self.start_and_goal_positions = start_and_goal_positions + + # Seed randomness for reproducibility + RANDOM_SEED = 50 + random.seed(RANDOM_SEED) + numpy_random.seed(RANDOM_SEED) + + def plan(self, verbose: bool = False) -> list[NodePath]: + """ + Plan for all agents. Returned paths are in the order of the `StartAndGoal` list this object was instantiated with + """ + paths = [] + for start_and_goal in self.start_and_goal_positions: + planner = self.create_planner(start_and_goal.start, start_and_goal.goal) + path = planner.plan(verbose) + paths.append(path) + return paths \ No newline at end of file diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py index 0985e37fab..d1500ee305 100644 --- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py +++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py @@ -18,9 +18,8 @@ empty_2d_array_of_lists, ) from PathPlanning.TimeBasedPathPlanning.Node import Node, NodePath -from PathPlanning.TimeBasedPathPlanning.SingleAgentPlannerBase import SingleAgentPlanner +from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner import heapq -import random from dataclasses import dataclass from functools import total_ordering import time @@ -46,29 +45,30 @@ class SafeIntervalPathPlanner(SingleAgentPlanner): Arguments: verbose (bool): set to True to print debug information """ - def plan(self, verbose: bool = False) -> NodePath: + @staticmethod + def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> NodePath: - safe_intervals = self.grid.get_safe_intervals() + safe_intervals = grid.get_safe_intervals() open_set: list[SIPPNode] = [] - first_node_interval = safe_intervals[self.start.x, self.start.y][0] + first_node_interval = safe_intervals[start.x, start.y][0] heapq.heappush( - open_set, SIPPNode(self.start, 0, self.calculate_heuristic(self.start), -1, first_node_interval) + open_set, SIPPNode(start, 0, SafeIntervalPathPlanner.calculate_heuristic(start, goal), -1, first_node_interval) ) expanded_list: list[SIPPNode] = [] - visited_intervals = empty_2d_array_of_lists(self.grid.grid_size[0], self.grid.grid_size[1]) + visited_intervals = empty_2d_array_of_lists(grid.grid_size[0], grid.grid_size[1]) while open_set: expanded_node: SIPPNode = heapq.heappop(open_set) if verbose: print("Expanded node:", expanded_node) - if expanded_node.time + 1 >= self.grid.time_limit: + if expanded_node.time + 1 >= grid.time_limit: if verbose: print(f"\tSkipping node that is past time limit: {expanded_node}") continue - if expanded_node.position == self.goal: + if expanded_node.position == goal: print(f"Found path to goal after {len(expanded_list)} expansions") path = [] path_walker: SIPPNode = expanded_node @@ -87,7 +87,7 @@ def plan(self, verbose: bool = False) -> NodePath: entry_time_and_node = EntryTimeAndInterval(expanded_node.time, expanded_node.interval) add_entry_to_visited_intervals_array(entry_time_and_node, visited_intervals, expanded_node) - for child in self.generate_successors(expanded_node, expanded_idx, safe_intervals, visited_intervals): + for child in SafeIntervalPathPlanner.generate_successors(grid, goal, expanded_node, expanded_idx, safe_intervals, visited_intervals): heapq.heappush(open_set, child) raise Exception("No path found") @@ -95,8 +95,9 @@ def plan(self, verbose: bool = False) -> NodePath: """ Generate list of possible successors of the provided `parent_node` that are worth expanding """ + @staticmethod def generate_successors( - self, parent_node: SIPPNode, parent_node_idx: int, intervals: np.ndarray, visited_intervals: np.ndarray + grid: Grid, goal: Position, parent_node: SIPPNode, parent_node_idx: int, intervals: np.ndarray, visited_intervals: np.ndarray ) -> list[SIPPNode]: new_nodes = [] diffs = [ @@ -108,7 +109,7 @@ def generate_successors( ] for diff in diffs: new_pos = parent_node.position + diff - if not self.grid.inside_grid_bounds(new_pos): + if not grid.inside_grid_bounds(new_pos): continue current_interval = parent_node.interval @@ -136,12 +137,12 @@ def generate_successors( # We know there is a node worth expanding. Generate successor at the earliest possible time the # new interval can be entered for possible_t in range(max(parent_node.time + 1, interval.start_time), min(current_interval.end_time, interval.end_time)): - if self.grid.valid_position(new_pos, possible_t): + if grid.valid_position(new_pos, possible_t): new_nodes.append(SIPPNode( new_pos, # entry is max of interval start and parent node time + 1 (get there as soon as possible) max(interval.start_time, parent_node.time + 1), - self.calculate_heuristic(new_pos), + SafeIntervalPathPlanner.calculate_heuristic(new_pos, goal), parent_node_idx, interval, )) @@ -153,8 +154,9 @@ def generate_successors( """ Calculate the heuristic for a given position - Manhattan distance to the goal """ - def calculate_heuristic(self, position) -> int: - diff = self.goal - position + @staticmethod + def calculate_heuristic(position: Position, goal: Position) -> int: + diff = goal - position return abs(diff.x) + abs(diff.y) @@ -190,8 +192,7 @@ def main(): # obstacle_arrangement=ObstacleArrangement.RANDOM, ) - planner = SafeIntervalPathPlanner(grid, start, goal) - path = planner.plan(verbose) + path = SafeIntervalPathPlanner.plan(grid, start, goal, verbose) runtime = time.time() - start_time print(f"Planning took: {runtime:.5f} seconds") diff --git a/PathPlanning/TimeBasedPathPlanning/SingleAgentPlannerBase.py b/PathPlanning/TimeBasedPathPlanning/SingleAgentPlannerBase.py deleted file mode 100644 index 68739a1694..0000000000 --- a/PathPlanning/TimeBasedPathPlanning/SingleAgentPlannerBase.py +++ /dev/null @@ -1,27 +0,0 @@ -from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( - Grid, - Position, -) -from PathPlanning.TimeBasedPathPlanning.Node import NodePath -import random -import numpy.random as numpy_random -from abc import ABC, abstractmethod - -class SingleAgentPlanner(ABC): - """ - Base class for single agent path planners. - """ - - def __init__(self, grid: Grid, start: Position, goal: Position): - self.grid = grid - self.start = start - self.goal = goal - - # Seed randomness for reproducibility - RANDOM_SEED = 50 - random.seed(RANDOM_SEED) - numpy_random.seed(RANDOM_SEED) - - @abstractmethod - def plan(self, verbose: bool = False) -> NodePath: - pass diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py index 43d18f4124..d7b93bf908 100644 --- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py +++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py @@ -20,7 +20,7 @@ from collections.abc import Generator import random import time -from PathPlanning.TimeBasedPathPlanning.SingleAgentPlannerBase import SingleAgentPlanner +from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner # Seed randomness for reproducibility RANDOM_SEED = 50 @@ -30,10 +30,11 @@ class SpaceTimeAStar(SingleAgentPlanner): - def plan(self, verbose: bool = False) -> NodePath: + @staticmethod + def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> NodePath: open_set: list[Node] = [] heapq.heappush( - open_set, Node(self.start, 0, self.calculate_heuristic(self.start), -1) + open_set, Node(start, 0, SpaceTimeAStar.calculate_heuristic(start, goal), -1) ) expanded_list: list[Node] = [] @@ -43,12 +44,12 @@ def plan(self, verbose: bool = False) -> NodePath: if verbose: print("Expanded node:", expanded_node) - if expanded_node.time + 1 >= self.grid.time_limit: + if expanded_node.time + 1 >= grid.time_limit: if verbose: print(f"\tSkipping node that is past time limit: {expanded_node}") continue - if expanded_node.position == self.goal: + if expanded_node.position == goal: print(f"Found path to goal after {len(expanded_list)} expansions") path = [] path_walker: Node = expanded_node @@ -60,14 +61,13 @@ def plan(self, verbose: bool = False) -> NodePath: # reverse path so it goes start -> goal path.reverse() - self.expanded_node_count = len(expanded_set) return NodePath(path) expanded_idx = len(expanded_list) expanded_list.append(expanded_node) expanded_set.add(expanded_node) - for child in self.generate_successors(expanded_node, expanded_idx, verbose, expanded_set): + for child in SpaceTimeAStar.generate_successors(grid, goal, expanded_node, expanded_idx, verbose, expanded_set): heapq.heappush(open_set, child) raise Exception("No path found") @@ -75,8 +75,9 @@ def plan(self, verbose: bool = False) -> NodePath: """ Generate possible successors of the provided `parent_node` """ + @staticmethod def generate_successors( - self, parent_node: Node, parent_node_idx: int, verbose: bool, expanded_set: set[Node] + grid: Grid, goal: Position, parent_node: Node, parent_node_idx: int, verbose: bool, expanded_set: set[Node] ) -> Generator[Node, None, None]: diffs = [ Position(0, 0), @@ -90,20 +91,21 @@ def generate_successors( new_node = Node( new_pos, parent_node.time + 1, - self.calculate_heuristic(new_pos), + SpaceTimeAStar.calculate_heuristic(new_pos, goal), parent_node_idx, ) if new_node in expanded_set: continue - if self.grid.valid_position(new_pos, parent_node.time + 1): + if grid.valid_position(new_pos, parent_node.time + 1): if verbose: print("\tNew successor node: ", new_node) yield new_node - def calculate_heuristic(self, position) -> int: - diff = self.goal - position + @staticmethod + def calculate_heuristic(position: Position, goal: Position) -> int: + diff = goal - position return abs(diff.x) + abs(diff.y) @@ -124,8 +126,7 @@ def main(): obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, ) - planner = SpaceTimeAStar(grid, start, goal) - path = planner.plan(verbose) + path = SpaceTimeAStar.plan(grid, start, goal, verbose) runtime = time.time() - start_time print(f"Planning took: {runtime:.5f} seconds") From 20ccb3e154ecf8db728b05422000a5a8277f5206 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Apr 2025 21:37:17 -0400 Subject: [PATCH 04/22] its working --- .../TimeBasedPathPlanning/BaseClasses.py | 30 ++--- .../CollaborativeAStar.py | 90 +++++++++++++ .../GridWithDynamicObstacles.py | 91 ++++++++++---- PathPlanning/TimeBasedPathPlanning/Node.py | 41 +++++- .../TimeBasedPathPlanning/Plotting.py | 118 ++++++++++++++++++ 5 files changed, 321 insertions(+), 49 deletions(-) create mode 100644 PathPlanning/TimeBasedPathPlanning/CollaborativeAStar.py create mode 100644 PathPlanning/TimeBasedPathPlanning/Plotting.py diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py index 7351660ec7..8e5fbf3045 100644 --- a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py +++ b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py @@ -8,6 +8,12 @@ from abc import ABC, abstractmethod from dataclasses import dataclass +# TODO: have planners call this somehow? +# Seed randomness for reproducibility +RANDOM_SEED = 50 +random.seed(RANDOM_SEED) +numpy_random.seed(RANDOM_SEED) + class SingleAgentPlanner(ABC): """ Base class for single agent planners @@ -20,6 +26,8 @@ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> @dataclass class StartAndGoal: + # Index of this agent + index: int # Start position of the robot start: Position # Goal position of the robot @@ -28,24 +36,12 @@ class StartAndGoal: class MultiAgentPlanner(ABC): """ Base class for multi-agent planners - """ - - def __init__(self, grid: Grid, start_and_goal_positions: list[StartAndGoal]): - self.grid = grid - self.start_and_goal_positions = start_and_goal_positions - - # Seed randomness for reproducibility - RANDOM_SEED = 50 - random.seed(RANDOM_SEED) - numpy_random.seed(RANDOM_SEED) + """ - def plan(self, verbose: bool = False) -> list[NodePath]: + @staticmethod + @abstractmethod + def plan(grid: Grid, start_and_goal_positions: list[StartAndGoal], verbose: bool = False) -> list[NodePath]: """ Plan for all agents. Returned paths are in the order of the `StartAndGoal` list this object was instantiated with """ - paths = [] - for start_and_goal in self.start_and_goal_positions: - planner = self.create_planner(start_and_goal.start, start_and_goal.goal) - path = planner.plan(verbose) - paths.append(path) - return paths \ No newline at end of file + pass \ No newline at end of file diff --git a/PathPlanning/TimeBasedPathPlanning/CollaborativeAStar.py b/PathPlanning/TimeBasedPathPlanning/CollaborativeAStar.py new file mode 100644 index 0000000000..9572dd8dcf --- /dev/null +++ b/PathPlanning/TimeBasedPathPlanning/CollaborativeAStar.py @@ -0,0 +1,90 @@ +""" +TODO - doc comment +""" + +import numpy as np +import matplotlib.pyplot as plt +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.SpaceTimeAStar import SpaceTimeAStar +from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePaths +import heapq +import random +from dataclasses import dataclass +from functools import total_ordering +import time +from enum import Enum +# TODO: audit imports + +class CollaborativeAStar(MultiAgentPlanner): + + def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> list[NodePath]: # TODO: list of what + + print(f"Using 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)) + + # TODO: smarter ordering + paths = [] + for start_and_goal in start_and_goals: + 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) + + 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 paths + +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, + # TODO: some static obstacle arrangements + obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR, + # obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, + # obstacle_arrangement=ObstacleArrangement.RANDOM, + ) + + start_time = time.time() + paths: list[NodePath] = CollaborativeAStar.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() \ No newline at end of file diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index 2a47023f8c..a1c8cc5122 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -7,31 +7,7 @@ import matplotlib.pyplot as plt from enum import Enum from dataclasses import dataclass - -@dataclass(order=True) -class Position: - x: int - y: int - - def as_ndarray(self) -> np.ndarray: - return np.array([self.x, self.y]) - - def __add__(self, other): - if isinstance(other, Position): - return Position(self.x + other.x, self.y + other.y) - raise NotImplementedError( - f"Addition not supported for Position and {type(other)}" - ) - - def __sub__(self, other): - if isinstance(other, Position): - return Position(self.x - other.x, self.y - other.y) - raise NotImplementedError( - f"Subtraction not supported for Position and {type(other)}" - ) - - def __hash__(self): - return hash((self.x, self.y)) +from PathPlanning.TimeBasedPathPlanning.Node import NodePath, Position @dataclass class Interval: @@ -43,6 +19,8 @@ class ObstacleArrangement(Enum): RANDOM = 0 # Obstacles start in a line in y at center of grid and move side-to-side in x ARRANGEMENT1 = 1 + # Static obstacle arrangement + NARROW_CORRIDOR = 2 """ Generates a 2d numpy array with lists for elements. @@ -87,6 +65,8 @@ def __init__( self.obstacle_paths = self.generate_dynamic_obstacles(num_obstacles) elif obstacle_arrangement == ObstacleArrangement.ARRANGEMENT1: self.obstacle_paths = self.obstacle_arrangement_1(num_obstacles) + elif obstacle_arrangement == ObstacleArrangement.NARROW_CORRIDOR: + self.obstacle_paths = self.generate_narrow_corridor_obstacles(num_obstacles) for i, path in enumerate(self.obstacle_paths): obs_idx = i + 1 # avoid using 0 - that indicates free space in the grid @@ -184,6 +164,26 @@ def obstacle_arrangement_1(self, obs_count: int) -> list[list[Position]]: obstacle_paths.append(path) return obstacle_paths + + def generate_narrow_corridor_obstacles(self, obs_count: int) -> list[list[Position]]: + obstacle_paths = [] + + for y in range(0, self.grid_size[1]): + if y > obs_count: + break + + if y == self.grid_size[1] // 2: + # Skip the middle row + continue + + obstacle_path = [] + x = 10 # middle of the grid + for t in range(0, self.time_limit - 1): + obstacle_path.append(Position(x, y)) + + obstacle_paths.append(obstacle_path) + + return obstacle_paths """ Check if the given position is valid at time t @@ -289,9 +289,48 @@ def get_safe_intervals_at_cell(self, cell: Position) -> list[Interval]: # 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] return intervals + + """ + Reserve an agent's path in the grid. Raises an exception if the agent's index is 0, or if a position is + already reserved by a different agent. + """ + def reserve_path(self, node_path: NodePath, agent_index: int): + if agent_index == 0: + raise Exception("Agent index cannot be 0") + + for i, node in enumerate(node_path.path): + reservation_finish_time = node.time + 1 + if i < len(node_path.path) - 1: + reservation_finish_time = node_path.path[i + 1].time -show_animation = True + self.reserve_position(node.position, agent_index, Interval(node.time, reservation_finish_time)) + + """ + Reserve a position for the provided agent during the provided time interval. + Raises an exception if the agent's index is 0, or if the position is already reserved by a different agent during the interval. + """ + def reserve_position(self, position: Position, agent_index: int, interval: Interval): + if agent_index == 0: + raise Exception("Agent index cannot be 0") + + for t in range(interval.start_time, interval.end_time + 1): + current_reserver = self.reservation_matrix[position.x, position.y, t] + if current_reserver not in [0, agent_index]: + raise Exception( + f"Agent {agent_index} tried to reserve a position already reserved by another agent: {position} at time {t}, reserved by {current_reserver}" + ) + self.reservation_matrix[position.x, position.y, t] = agent_index + + """ + Clears the initial reservation for an agent by clearing reservations at its start position with its index for + from time 0 to the time limit. + """ + def clear_initial_reservation(self, position: Position, agent_index: int): + for t in range(self.time_limit): + if self.reservation_matrix[position.x, position.y, t] == agent_index: + self.reservation_matrix[position.x, position.y, t] = 0 +show_animation = True def main(): grid = Grid( diff --git a/PathPlanning/TimeBasedPathPlanning/Node.py b/PathPlanning/TimeBasedPathPlanning/Node.py index 648a2980f9..ee3ecf2cf7 100644 --- a/PathPlanning/TimeBasedPathPlanning/Node.py +++ b/PathPlanning/TimeBasedPathPlanning/Node.py @@ -1,8 +1,31 @@ from dataclasses import dataclass -from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( - Position, -) from functools import total_ordering +import numpy as np + +@dataclass(order=True) +class Position: + x: int + y: int + + def as_ndarray(self) -> np.ndarray: + return np.array([self.x, self.y]) + + def __add__(self, other): + if isinstance(other, Position): + return Position(self.x + other.x, self.y + other.y) + raise NotImplementedError( + f"Addition not supported for Position and {type(other)}" + ) + + def __sub__(self, other): + if isinstance(other, Position): + return Position(self.x - other.x, self.y - other.y) + raise NotImplementedError( + f"Subtraction not supported for Position and {type(other)}" + ) + + def __hash__(self): + return hash((self.x, self.y)) @dataclass() # Note: Total_ordering is used instead of adding `order=True` to the @dataclass decorator because @@ -39,12 +62,18 @@ def __hash__(self): class NodePath: path: list[Node] - positions_at_time: dict[int, Position] = {} + positions_at_time: dict[int, Position] def __init__(self, path: list[Node]): self.path = path - for node in path: - self.positions_at_time[node.time] = node.position + self.positions_at_time = {} + for i, node in enumerate(path): + reservation_finish_time = node.time + 1 + if i < len(path) - 1: + reservation_finish_time = path[i + 1].time + + for t in range(node.time, reservation_finish_time): + self.positions_at_time[t] = node.position """ Get the position of the path at a given time diff --git a/PathPlanning/TimeBasedPathPlanning/Plotting.py b/PathPlanning/TimeBasedPathPlanning/Plotting.py new file mode 100644 index 0000000000..92f9cd40be --- /dev/null +++ b/PathPlanning/TimeBasedPathPlanning/Plotting.py @@ -0,0 +1,118 @@ +import numpy as np +import matplotlib.pyplot as plt +from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( + Grid, + Position, +) +from PathPlanning.TimeBasedPathPlanning.BaseClasses import StartAndGoal +from PathPlanning.TimeBasedPathPlanning.Node import NodePath + +''' +Plot a single agent path. +''' +def PlotNodePath(grid: Grid, start: Position, goal: Position, path: NodePath): + fig = plt.figure(figsize=(10, 7)) + ax = fig.add_subplot( + autoscale_on=False, + xlim=(0, grid.grid_size[0] - 1), + ylim=(0, grid.grid_size[1] - 1), + ) + ax.set_aspect("equal") + ax.grid() + ax.set_xticks(np.arange(0, grid.grid_size[0], 1)) + ax.set_yticks(np.arange(0, grid.grid_size[1], 1)) + + (start_and_goal,) = ax.plot([], [], "mD", ms=15, label="Start and Goal") + start_and_goal.set_data([start.x, goal.x], [start.y, goal.y]) + (obs_points,) = ax.plot([], [], "ro", ms=15, label="Obstacles") + (path_points,) = ax.plot([], [], "bo", ms=10, label="Path Found") + ax.legend(bbox_to_anchor=(1.05, 1)) + + # for stopping simulation with the esc key. + plt.gcf().canvas.mpl_connect( + "key_release_event", lambda event: [exit(0) if event.key == "escape" else None] + ) + + for i in range(0, path.goal_reached_time()): + obs_positions = grid.get_obstacle_positions_at_time(i) + obs_points.set_data(obs_positions[0], obs_positions[1]) + path_position = path.get_position(i) + path_points.set_data([path_position.x], [path_position.y]) + plt.pause(0.2) + plt.show() + +''' +Plot a series of agent paths. +''' +def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[NodePath]): + fig = plt.figure(figsize=(10, 7)) + + ax = fig.add_subplot( + autoscale_on=False, + xlim=(0, grid.grid_size[0] - 1), + ylim=(0, grid.grid_size[1] - 1), + ) + ax.set_aspect("equal") + ax.grid() + ax.set_xticks(np.arange(0, grid.grid_size[0], 1)) + ax.set_yticks(np.arange(0, grid.grid_size[1], 1)) + + # Plot start and goal positions for each agent + colors = [] # generated randomly in loop + markers = ['D', 's', '^', 'o', 'p'] # Different markers for visual distinction + + # Create plots for start and goal positions + start_and_goal_plots = [] + for i, path in enumerate(paths): + marker_idx = i % len(markers) + agent_id = start_and_goals[i].index + start = start_and_goals[i].start + goal = start_and_goals[i].goal + + color = np.random.rand(3,) + colors.append(color) + sg_plot, = ax.plot([], [], markers[marker_idx], c=color, ms=15, + label=f"Agent {agent_id} Start/Goal") + sg_plot.set_data([start.x, goal.x], [start.y, goal.y]) + start_and_goal_plots.append(sg_plot) + + # Plot for obstacles + (obs_points,) = ax.plot([], [], "ro", ms=15, label="Obstacles") + + # Create plots for each agent's path + path_plots = [] + for i, path in enumerate(paths): + agent_id = start_and_goals[i].index + path_plot, = ax.plot([], [], "o", c=colors[i], ms=10, + label=f"Agent {agent_id} Path") + path_plots.append(path_plot) + + ax.legend(bbox_to_anchor=(1.05, 1)) + + # For stopping simulation with the esc key + plt.gcf().canvas.mpl_connect( + "key_release_event", lambda event: [exit(0) if event.key == "escape" else None] + ) + + # Find the maximum time across all paths + max_time = max(path.goal_reached_time() for path in paths) + + # Animation loop + for i in range(0, max_time + 1): + # Update obstacle positions + obs_positions = grid.get_obstacle_positions_at_time(i) + obs_points.set_data(obs_positions[0], obs_positions[1]) + + # Update each agent's position + for (j, path) in enumerate(paths): + if i <= path.goal_reached_time(): + res = path.get_position(i) + if not res: + print(path) + print(i) + path_position = path.get_position(i) + path_plots[j].set_data([path_position.x], [path_position.y]) + + plt.pause(0.5) + + plt.show() \ No newline at end of file From e93116c806697a6baf91cb5786389f9a8d30b8b9 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Apr 2025 21:39:05 -0400 Subject: [PATCH 05/22] use single agent plotting util --- .../TimeBasedPathPlanning/SafeInterval.py | 34 +++---------------- .../TimeBasedPathPlanning/SpaceTimeAStar.py | 32 ++--------------- 2 files changed, 6 insertions(+), 60 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py index d1500ee305..cdbcbee2a9 100644 --- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py +++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py @@ -17,8 +17,10 @@ Position, empty_2d_array_of_lists, ) -from PathPlanning.TimeBasedPathPlanning.Node import Node, NodePath from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner +from PathPlanning.TimeBasedPathPlanning.Node import Node, NodePath +from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePath + import heapq from dataclasses import dataclass from functools import total_ordering @@ -202,35 +204,7 @@ def main(): if not show_animation: return - fig = plt.figure(figsize=(10, 7)) - ax = fig.add_subplot( - autoscale_on=False, - xlim=(0, grid.grid_size[0] - 1), - ylim=(0, grid.grid_size[1] - 1), - ) - ax.set_aspect("equal") - ax.grid() - ax.set_xticks(np.arange(0, grid_side_length, 1)) - ax.set_yticks(np.arange(0, grid_side_length, 1)) - - (start_and_goal,) = ax.plot([], [], "mD", ms=15, label="Start and Goal") - start_and_goal.set_data([start.x, goal.x], [start.y, goal.y]) - (obs_points,) = ax.plot([], [], "ro", ms=15, label="Obstacles") - (path_points,) = ax.plot([], [], "bo", ms=10, label="Path Found") - ax.legend(bbox_to_anchor=(1.05, 1)) - - # for stopping simulation with the esc key. - plt.gcf().canvas.mpl_connect( - "key_release_event", lambda event: [exit(0) if event.key == "escape" else None] - ) - - for i in range(0, path.goal_reached_time() + 1): - obs_positions = grid.get_obstacle_positions_at_time(i) - obs_points.set_data(obs_positions[0], obs_positions[1]) - path_position = path.get_position(i) - path_points.set_data([path_position.x], [path_position.y]) - plt.pause(0.2) - plt.show() + PlotNodePath(grid, start, goal, path) if __name__ == "__main__": diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py index d7b93bf908..b7035fbf61 100644 --- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py +++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py @@ -21,6 +21,7 @@ import random import time from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner +from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePath # Seed randomness for reproducibility RANDOM_SEED = 50 @@ -137,36 +138,7 @@ def main(): if not show_animation: return - fig = plt.figure(figsize=(10, 7)) - ax = fig.add_subplot( - autoscale_on=False, - xlim=(0, grid.grid_size[0] - 1), - ylim=(0, grid.grid_size[1] - 1), - ) - ax.set_aspect("equal") - ax.grid() - ax.set_xticks(np.arange(0, grid_side_length, 1)) - ax.set_yticks(np.arange(0, grid_side_length, 1)) - - (start_and_goal,) = ax.plot([], [], "mD", ms=15, label="Start and Goal") - start_and_goal.set_data([start.x, goal.x], [start.y, goal.y]) - (obs_points,) = ax.plot([], [], "ro", ms=15, label="Obstacles") - (path_points,) = ax.plot([], [], "bo", ms=10, label="Path Found") - ax.legend(bbox_to_anchor=(1.05, 1)) - - # for stopping simulation with the esc key. - plt.gcf().canvas.mpl_connect( - "key_release_event", lambda event: [exit(0) if event.key == "escape" else None] - ) - - for i in range(0, path.goal_reached_time()): - obs_positions = grid.get_obstacle_positions_at_time(i) - obs_points.set_data(obs_positions[0], obs_positions[1]) - path_position = path.get_position(i) - path_points.set_data([path_position.x], [path_position.y]) - plt.pause(0.2) - plt.show() - + PlotNodePath(grid, start, goal, path) if __name__ == "__main__": main() From 7f4cf58d4f7b88bea78a192b4294ea53f1d36d65 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Apr 2025 22:08:26 -0400 Subject: [PATCH 06/22] cleanup, bug fix, add some results to docs --- .../CollaborativeAStar.py | 13 +-- .../GridWithDynamicObstacles.py | 4 +- .../TimeBasedPathPlanning/SafeInterval.py | 1 - .../TimeBasedPathPlanning/SpaceTimeAStar.py | 4 +- .../time_based_grid_search_main.rst | 99 +++++++++++++++++++ 5 files changed, 106 insertions(+), 15 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/CollaborativeAStar.py b/PathPlanning/TimeBasedPathPlanning/CollaborativeAStar.py index 9572dd8dcf..37f2ea9b4c 100644 --- a/PathPlanning/TimeBasedPathPlanning/CollaborativeAStar.py +++ b/PathPlanning/TimeBasedPathPlanning/CollaborativeAStar.py @@ -3,7 +3,6 @@ """ import numpy as np -import matplotlib.pyplot as plt from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( Grid, Interval, @@ -16,13 +15,7 @@ from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner from PathPlanning.TimeBasedPathPlanning.SpaceTimeAStar import SpaceTimeAStar from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePaths -import heapq -import random -from dataclasses import dataclass -from functools import total_ordering import time -from enum import Enum -# TODO: audit imports class CollaborativeAStar(MultiAgentPlanner): @@ -37,10 +30,11 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c # TODO: smarter ordering paths = [] for start_and_goal in start_and_goals: - print(f"\nPlanning for agent: {start_and_goal}" ) + if True: + 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) + 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}") @@ -64,7 +58,6 @@ def main(): np.array([grid_side_length, grid_side_length]), num_obstacles=250, obstacle_avoid_points=obstacle_avoid_points, - # TODO: some static obstacle arrangements obstacle_arrangement=ObstacleArrangement.NARROW_CORRIDOR, # obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, # obstacle_arrangement=ObstacleArrangement.RANDOM, diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index a1c8cc5122..337830dbb9 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -196,11 +196,11 @@ def generate_narrow_corridor_obstacles(self, obs_count: int) -> list[list[Positi bool: True if position/time combination is valid, False otherwise """ def valid_position(self, position: Position, t: int) -> bool: - # Check if new position is in grid + # Check if position is in grid if not self.inside_grid_bounds(position): return False - # Check if new position is not occupied at time t + # Check if position is not occupied at time t return self.reservation_matrix[position.x, position.y, t] == 0 """ diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py index cdbcbee2a9..e7b2d27fed 100644 --- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py +++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py @@ -9,7 +9,6 @@ """ import numpy as np -import matplotlib.pyplot as plt from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( Grid, Interval, diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py index b7035fbf61..ccb6200d50 100644 --- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py +++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py @@ -9,7 +9,6 @@ """ import numpy as np -import matplotlib.pyplot as plt from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( Grid, ObstacleArrangement, @@ -99,7 +98,8 @@ def generate_successors( if new_node in expanded_set: continue - if grid.valid_position(new_pos, parent_node.time + 1): + # Check if the new node is valid for the next 2 time steps - one step to enter, and another to leave + if all([grid.valid_position(new_pos, parent_node.time + dt) for dt in [1, 2]]): if verbose: print("\tNew successor node: ", new_node) yield new_node diff --git a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst index 9517e95b31..937697144a 100644 --- a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst +++ b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst @@ -81,6 +81,105 @@ Code Link ^^^^^^^^^^^^^ .. autoclass:: PathPlanning.TimeBasedPathPlanning.SafeInterval.SafeIntervalPathPlanner +Multi-Agent Path Planning +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Priority Based Planning +~~~~~~~~~~~~~~~~~~~~ + +TODO: explain the algorithm + +TODO: explain why spacetime a* is so much slower + +Using SpaceTimeAstar for the single agent planner:: + Using planner: + + Planning for agent: StartAndGoal(index=1, start=Position(x=1, y=1), goal=Position(x=19, y=18)) + Found path to goal after 300 expansions + + Planning for agent: StartAndGoal(index=2, start=Position(x=1, y=2), goal=Position(x=19, y=17)) + Found path to goal after 645 expansions + + Planning for agent: StartAndGoal(index=3, start=Position(x=1, y=3), goal=Position(x=19, y=16)) + Found path to goal after 273 expansions + + Planning for agent: StartAndGoal(index=4, start=Position(x=1, y=4), goal=Position(x=19, y=15)) + Found path to goal after 7798 expansions + + Planning for agent: StartAndGoal(index=5, start=Position(x=1, y=5), goal=Position(x=19, y=14)) + Found path to goal after 186 expansions + + Planning for agent: StartAndGoal(index=6, start=Position(x=1, y=6), goal=Position(x=19, y=13)) + Found path to goal after 216790 expansions + + Planning for agent: StartAndGoal(index=7, start=Position(x=1, y=7), goal=Position(x=19, y=12)) + Found path to goal after 472128 expansions + + Planning for agent: StartAndGoal(index=8, start=Position(x=1, y=8), goal=Position(x=19, y=11)) + Found path to goal after 916485 expansions + + Planning for agent: StartAndGoal(index=9, start=Position(x=1, y=9), goal=Position(x=19, y=10)) + Found path to goal after 2098980 expansions + + Planning for agent: StartAndGoal(index=10, start=Position(x=1, y=10), goal=Position(x=19, y=9)) + Found path to goal after 50 expansions + + Planning for agent: StartAndGoal(index=11, start=Position(x=1, y=11), goal=Position(x=19, y=8)) + Found path to goal after 62 expansions + + Planning for agent: StartAndGoal(index=12, start=Position(x=1, y=12), goal=Position(x=19, y=7)) + ... + (timed out) + +Using the safe interval path planner:: + + Planning for agent: StartAndGoal(index=1, start=Position(x=1, y=1), goal=Position(x=19, y=18)) + Found path to goal after 256 expansions + + Planning for agent: StartAndGoal(index=2, start=Position(x=1, y=2), goal=Position(x=19, y=17)) + Found path to goal after 336 expansions + + Planning for agent: StartAndGoal(index=3, start=Position(x=1, y=3), goal=Position(x=19, y=16)) + Found path to goal after 526 expansions + + Planning for agent: StartAndGoal(index=4, start=Position(x=1, y=4), goal=Position(x=19, y=15)) + Found path to goal after 619 expansions + + Planning for agent: StartAndGoal(index=5, start=Position(x=1, y=5), goal=Position(x=19, y=14)) + Found path to goal after 534 expansions + + Planning for agent: StartAndGoal(index=6, start=Position(x=1, y=6), goal=Position(x=19, y=13)) + Found path to goal after 596 expansions + + Planning for agent: StartAndGoal(index=7, start=Position(x=1, y=7), goal=Position(x=19, y=12)) + Found path to goal after 149 expansions + + Planning for agent: StartAndGoal(index=8, start=Position(x=1, y=8), goal=Position(x=19, y=11)) + Found path to goal after 551 expansions + + Planning for agent: StartAndGoal(index=9, start=Position(x=1, y=9), goal=Position(x=19, y=10)) + Found path to goal after 92 expansions + + Planning for agent: StartAndGoal(index=10, start=Position(x=1, y=10), goal=Position(x=19, y=9)) + Found path to goal after 1042 expansions + + Planning for agent: StartAndGoal(index=11, start=Position(x=1, y=11), goal=Position(x=19, y=8)) + Found path to goal after 1062 expansions + + Planning for agent: StartAndGoal(index=12, start=Position(x=1, y=12), goal=Position(x=19, y=7)) + Found path to goal after 1000 expansions + + Planning for agent: StartAndGoal(index=13, start=Position(x=1, y=13), goal=Position(x=19, y=6)) + Found path to goal after 867 expansions + + Planning for agent: StartAndGoal(index=14, start=Position(x=1, y=14), goal=Position(x=19, y=5)) + Found path to goal after 964 expansions + + Planning for agent: StartAndGoal(index=15, start=Position(x=1, y=15), goal=Position(x=19, y=4)) + Found path to goal after 1146 expansions + + Planning took: 0.11520 seconds + References ~~~~~~~~~~~ From 43bc927340cec63dd95f88be4090f53c6b94d139 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Apr 2025 22:10:04 -0400 Subject: [PATCH 07/22] remove seeding from sta* - it happens in Node --- PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py | 7 ------- 1 file changed, 7 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py index ccb6200d50..a413fb7329 100644 --- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py +++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py @@ -17,17 +17,10 @@ from PathPlanning.TimeBasedPathPlanning.Node import Node, NodePath import heapq from collections.abc import Generator -import random import time from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePath -# Seed randomness for reproducibility -RANDOM_SEED = 50 -random.seed(RANDOM_SEED) -np.random.seed(RANDOM_SEED) - - class SpaceTimeAStar(SingleAgentPlanner): @staticmethod From 777b5ce96bf02fb481fe3dc415744d7ba6ea0afb Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Apr 2025 22:10:23 -0400 Subject: [PATCH 08/22] remove stale todo --- PathPlanning/TimeBasedPathPlanning/BaseClasses.py | 1 - 1 file changed, 1 deletion(-) diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py index 8e5fbf3045..1d3fd9ff94 100644 --- a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py +++ b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py @@ -8,7 +8,6 @@ from abc import ABC, abstractmethod from dataclasses import dataclass -# TODO: have planners call this somehow? # Seed randomness for reproducibility RANDOM_SEED = 50 random.seed(RANDOM_SEED) From 7495fe64526ad63ab28d012e978d682691f01a8c Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Apr 2025 22:12:11 -0400 Subject: [PATCH 09/22] rename CA* and speed up plotting --- PathPlanning/TimeBasedPathPlanning/Plotting.py | 2 +- .../{CollaborativeAStar.py => PriorityBasedPlanner.py} | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) rename PathPlanning/TimeBasedPathPlanning/{CollaborativeAStar.py => PriorityBasedPlanner.py} (89%) diff --git a/PathPlanning/TimeBasedPathPlanning/Plotting.py b/PathPlanning/TimeBasedPathPlanning/Plotting.py index 92f9cd40be..cdb1131581 100644 --- a/PathPlanning/TimeBasedPathPlanning/Plotting.py +++ b/PathPlanning/TimeBasedPathPlanning/Plotting.py @@ -113,6 +113,6 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N path_position = path.get_position(i) path_plots[j].set_data([path_position.x], [path_position.y]) - plt.pause(0.5) + plt.pause(0.2) plt.show() \ No newline at end of file diff --git a/PathPlanning/TimeBasedPathPlanning/CollaborativeAStar.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py similarity index 89% rename from PathPlanning/TimeBasedPathPlanning/CollaborativeAStar.py rename to PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py index 37f2ea9b4c..124c240f81 100644 --- a/PathPlanning/TimeBasedPathPlanning/CollaborativeAStar.py +++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py @@ -17,7 +17,7 @@ from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePaths import time -class CollaborativeAStar(MultiAgentPlanner): +class PriorityBasedPlanner(MultiAgentPlanner): def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> list[NodePath]: # TODO: list of what @@ -58,13 +58,13 @@ def main(): 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.NARROW_CORRIDOR, + obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, # obstacle_arrangement=ObstacleArrangement.RANDOM, ) start_time = time.time() - paths: list[NodePath] = CollaborativeAStar.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose) + paths: list[NodePath] = PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose) runtime = time.time() - start_time print(f"\nPlanning took: {runtime:.5f} seconds") From 63aef91e7f42a4ba2e9414e5e1d985c019341d3a Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Apr 2025 22:12:53 -0400 Subject: [PATCH 10/22] paper --- PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py index 124c240f81..a5938bfbc4 100644 --- a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py +++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py @@ -1,5 +1,7 @@ """ TODO - doc comment + +sort of outlined in this paper: https://pure.tudelft.nl/ws/portalfiles/portal/67074672/07138650.pdf """ import numpy as np From d722adc73c2a0c56f5e89b4e3db4f37ebcecfc62 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Apr 2025 22:14:10 -0400 Subject: [PATCH 11/22] proper paper (ofc its csail) --- PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py | 1 + 1 file changed, 1 insertion(+) diff --git a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py index a5938bfbc4..858a81522a 100644 --- a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py +++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py @@ -2,6 +2,7 @@ TODO - doc comment sort of outlined in this paper: https://pure.tudelft.nl/ws/portalfiles/portal/67074672/07138650.pdf +better paper: https://people.csail.mit.edu/tlp/publications/01087401.pdf """ import numpy as np From 224dd3be222654fab45d8d1188a7d3930f745a67 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 9 Jul 2025 22:33:09 -0400 Subject: [PATCH 12/22] some cleanup --- .../TimeBasedPathPlanning/BaseClasses.py | 8 ++++++-- PathPlanning/TimeBasedPathPlanning/Plotting.py | 7 +++++++ .../PriorityBasedPlanner.py | 16 ++++++++++------ 3 files changed, 23 insertions(+), 8 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py index 1d3fd9ff94..be14441d95 100644 --- a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py +++ b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py @@ -1,3 +1,6 @@ +from abc import ABC, abstractmethod +from dataclasses import dataclass +import math from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( Grid, Position, @@ -5,8 +8,6 @@ from PathPlanning.TimeBasedPathPlanning.Node import NodePath import random import numpy.random as numpy_random -from abc import ABC, abstractmethod -from dataclasses import dataclass # Seed randomness for reproducibility RANDOM_SEED = 50 @@ -32,6 +33,9 @@ class StartAndGoal: # Goal position of the robot goal: Position + def distance_start_to_goal(self) -> float: + return pow(self.goal.x - self.start.x, 2) + pow(self.goal.y - self.start.y, 2) + class MultiAgentPlanner(ABC): """ Base class for multi-agent planners diff --git a/PathPlanning/TimeBasedPathPlanning/Plotting.py b/PathPlanning/TimeBasedPathPlanning/Plotting.py index cdb1131581..44d6f7ce71 100644 --- a/PathPlanning/TimeBasedPathPlanning/Plotting.py +++ b/PathPlanning/TimeBasedPathPlanning/Plotting.py @@ -105,12 +105,19 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N # Update each agent's position for (j, path) in enumerate(paths): + path_postitions = [] if i <= path.goal_reached_time(): res = path.get_position(i) if not res: print(path) print(i) path_position = path.get_position(i) + + # Verify position is valid + assert not path_position in obs_positions + assert not path_position in path_postitions + path_postitions.append(path_position) + path_plots[j].set_data([path_position.x], [path_position.y]) plt.pause(0.2) diff --git a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py index 858a81522a..decd80c53e 100644 --- a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py +++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py @@ -1,8 +1,9 @@ """ -TODO - doc comment +Priority Based Planner for multi agent path planning. +The planner generates an order to plan in, and generates plans for the robots in that order. Each planned +path is reserved in the grid, and all future plans must avoid that path. -sort of outlined in this paper: https://pure.tudelft.nl/ws/portalfiles/portal/67074672/07138650.pdf -better paper: https://people.csail.mit.edu/tlp/publications/01087401.pdf +Algorithm outlined in section III of this paper: https://pure.tudelft.nl/ws/portalfiles/portal/67074672/07138650.pdf """ import numpy as np @@ -16,13 +17,12 @@ from PathPlanning.TimeBasedPathPlanning.Node import NodePath from PathPlanning.TimeBasedPathPlanning.BaseClasses import SingleAgentPlanner from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner -from PathPlanning.TimeBasedPathPlanning.SpaceTimeAStar import SpaceTimeAStar from PathPlanning.TimeBasedPathPlanning.Plotting import PlotNodePaths import time class PriorityBasedPlanner(MultiAgentPlanner): - def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> list[NodePath]: # TODO: list of what + def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> list[NodePath]: print(f"Using planner: {single_agent_planner_class}") @@ -30,7 +30,11 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c for start_and_goal in start_and_goals: grid.reserve_position(start_and_goal.start, start_and_goal.index, Interval(0, 10)) - # TODO: smarter ordering + # Plan in descending order of distance from start to goal + start_and_goals = sorted(start_and_goals, + key=lambda item: item.distance_start_to_goal(), + reverse=True) + paths = [] for start_and_goal in start_and_goals: if True: From ede80e431e10228f06bd0e1d538b017101fed451 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 9 Jul 2025 22:39:51 -0400 Subject: [PATCH 13/22] update docs --- .../time_based_grid_search_main.rst | 94 +------------------ 1 file changed, 4 insertions(+), 90 deletions(-) diff --git a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst index 937697144a..5dfaf6875b 100644 --- a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst +++ b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst @@ -87,98 +87,11 @@ Multi-Agent Path Planning Priority Based Planning ~~~~~~~~~~~~~~~~~~~~ -TODO: explain the algorithm +The planner generates an order to plan in, and generates plans for the robots in that order. Each planned path is reserved in the grid, and all future plans must avoid that path. The robots are planned for in descending order of distance from start to goal. -TODO: explain why spacetime a* is so much slower +This is a sub-optimal algorithm because no replanning happens once paths are found. It does not guarantee the shortest path is found for any particular robot, nor that the final set of paths found contains the lowest possible amount of time or movement. -Using SpaceTimeAstar for the single agent planner:: - Using planner: - - Planning for agent: StartAndGoal(index=1, start=Position(x=1, y=1), goal=Position(x=19, y=18)) - Found path to goal after 300 expansions - - Planning for agent: StartAndGoal(index=2, start=Position(x=1, y=2), goal=Position(x=19, y=17)) - Found path to goal after 645 expansions - - Planning for agent: StartAndGoal(index=3, start=Position(x=1, y=3), goal=Position(x=19, y=16)) - Found path to goal after 273 expansions - - Planning for agent: StartAndGoal(index=4, start=Position(x=1, y=4), goal=Position(x=19, y=15)) - Found path to goal after 7798 expansions - - Planning for agent: StartAndGoal(index=5, start=Position(x=1, y=5), goal=Position(x=19, y=14)) - Found path to goal after 186 expansions - - Planning for agent: StartAndGoal(index=6, start=Position(x=1, y=6), goal=Position(x=19, y=13)) - Found path to goal after 216790 expansions - - Planning for agent: StartAndGoal(index=7, start=Position(x=1, y=7), goal=Position(x=19, y=12)) - Found path to goal after 472128 expansions - - Planning for agent: StartAndGoal(index=8, start=Position(x=1, y=8), goal=Position(x=19, y=11)) - Found path to goal after 916485 expansions - - Planning for agent: StartAndGoal(index=9, start=Position(x=1, y=9), goal=Position(x=19, y=10)) - Found path to goal after 2098980 expansions - - Planning for agent: StartAndGoal(index=10, start=Position(x=1, y=10), goal=Position(x=19, y=9)) - Found path to goal after 50 expansions - - Planning for agent: StartAndGoal(index=11, start=Position(x=1, y=11), goal=Position(x=19, y=8)) - Found path to goal after 62 expansions - - Planning for agent: StartAndGoal(index=12, start=Position(x=1, y=12), goal=Position(x=19, y=7)) - ... - (timed out) - -Using the safe interval path planner:: - - Planning for agent: StartAndGoal(index=1, start=Position(x=1, y=1), goal=Position(x=19, y=18)) - Found path to goal after 256 expansions - - Planning for agent: StartAndGoal(index=2, start=Position(x=1, y=2), goal=Position(x=19, y=17)) - Found path to goal after 336 expansions - - Planning for agent: StartAndGoal(index=3, start=Position(x=1, y=3), goal=Position(x=19, y=16)) - Found path to goal after 526 expansions - - Planning for agent: StartAndGoal(index=4, start=Position(x=1, y=4), goal=Position(x=19, y=15)) - Found path to goal after 619 expansions - - Planning for agent: StartAndGoal(index=5, start=Position(x=1, y=5), goal=Position(x=19, y=14)) - Found path to goal after 534 expansions - - Planning for agent: StartAndGoal(index=6, start=Position(x=1, y=6), goal=Position(x=19, y=13)) - Found path to goal after 596 expansions - - Planning for agent: StartAndGoal(index=7, start=Position(x=1, y=7), goal=Position(x=19, y=12)) - Found path to goal after 149 expansions - - Planning for agent: StartAndGoal(index=8, start=Position(x=1, y=8), goal=Position(x=19, y=11)) - Found path to goal after 551 expansions - - Planning for agent: StartAndGoal(index=9, start=Position(x=1, y=9), goal=Position(x=19, y=10)) - Found path to goal after 92 expansions - - Planning for agent: StartAndGoal(index=10, start=Position(x=1, y=10), goal=Position(x=19, y=9)) - Found path to goal after 1042 expansions - - Planning for agent: StartAndGoal(index=11, start=Position(x=1, y=11), goal=Position(x=19, y=8)) - Found path to goal after 1062 expansions - - Planning for agent: StartAndGoal(index=12, start=Position(x=1, y=12), goal=Position(x=19, y=7)) - Found path to goal after 1000 expansions - - Planning for agent: StartAndGoal(index=13, start=Position(x=1, y=13), goal=Position(x=19, y=6)) - Found path to goal after 867 expansions - - Planning for agent: StartAndGoal(index=14, start=Position(x=1, y=14), goal=Position(x=19, y=5)) - Found path to goal after 964 expansions - - Planning for agent: StartAndGoal(index=15, start=Position(x=1, y=15), goal=Position(x=19, y=4)) - Found path to goal after 1146 expansions - - Planning took: 0.11520 seconds +TODO: include a gif References @@ -186,3 +99,4 @@ References - `Cooperative Pathfinding `__ - `SIPP: Safe Interval Path Planning for Dynamic Environments `__ +- `Prioritized Planning Algorithms for Trajectory Coordination of Multiple Mobile Robots `__ \ No newline at end of file From c89f47f663aa22c3997cec6a8ef33e9f3c7749e1 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 9 Jul 2025 22:54:58 -0400 Subject: [PATCH 14/22] add unit test --- .../PriorityBasedPlanner.py | 14 ++++-- tests/test_priority_based_planner.py | 45 +++++++++++++++++++ 2 files changed, 55 insertions(+), 4 deletions(-) create mode 100644 tests/test_priority_based_planner.py diff --git a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py index decd80c53e..fb54507b9b 100644 --- a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py +++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py @@ -22,8 +22,12 @@ class PriorityBasedPlanner(MultiAgentPlanner): - def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> list[NodePath]: - + def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> 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 planner: {single_agent_planner_class}") # Reserve initial positions @@ -51,7 +55,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c grid.reserve_path(path, agent_index) paths.append(path) - return paths + return (start_and_goals, paths) verbose = False show_animation = True @@ -71,7 +75,9 @@ def main(): ) start_time = time.time() - paths: list[NodePath] = PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose) + start_and_goals: list[StartAndGoal] + paths: list[NodePath] + start_and_goals, paths = PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose) runtime = time.time() - start_time print(f"\nPlanning took: {runtime:.5f} seconds") diff --git a/tests/test_priority_based_planner.py b/tests/test_priority_based_planner.py new file mode 100644 index 0000000000..23fae46153 --- /dev/null +++ b/tests/test_priority_based_planner.py @@ -0,0 +1,45 @@ +from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( + Grid, + NodePath, + ObstacleArrangement, + Position, +) +from PathPlanning.TimeBasedPathPlanning.BaseClasses import StartAndGoal +from PathPlanning.TimeBasedPathPlanning import PriorityBasedPlanner as m +from PathPlanning.TimeBasedPathPlanning.SafeInterval import SafeIntervalPathPlanner +import numpy as np +import conftest + + +def test_1(): + 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.ARRANGEMENT1, + ) + + grid_side_length = 21 + grid = Grid( + np.array([grid_side_length, grid_side_length]), + obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, + ) + + m.show_animation = False + + start_and_goals: list[StartAndGoal] + paths: list[NodePath] + start_and_goals, paths = m.PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, False) + + # All paths should start at the specified position and reach the goal + for i, start_and_goal in enumerate(start_and_goals): + assert paths[i].path[0].position == start_and_goal.start + assert paths[i].path[-1].position == start_and_goal.goal + +if __name__ == "__main__": + conftest.run_this_test(__file__) From 639167795c80a76a1dbd40a6fbf1f1ba70c14d54 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 9 Jul 2025 22:58:01 -0400 Subject: [PATCH 15/22] add logic for saving animation as gif --- PathPlanning/TimeBasedPathPlanning/Plotting.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/PathPlanning/TimeBasedPathPlanning/Plotting.py b/PathPlanning/TimeBasedPathPlanning/Plotting.py index 44d6f7ce71..75f0a294e8 100644 --- a/PathPlanning/TimeBasedPathPlanning/Plotting.py +++ b/PathPlanning/TimeBasedPathPlanning/Plotting.py @@ -1,3 +1,4 @@ +import imageio.v2 as imageio import numpy as np import matplotlib.pyplot as plt from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( @@ -44,7 +45,7 @@ def PlotNodePath(grid: Grid, start: Position, goal: Position, path: NodePath): ''' Plot a series of agent paths. ''' -def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[NodePath]): +def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[NodePath], save_gif: bool = False): fig = plt.figure(figsize=(10, 7)) ax = fig.add_subplot( @@ -98,7 +99,12 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N max_time = max(path.goal_reached_time() for path in paths) # Animation loop + frames = [] for i in range(0, max_time + 1): + if save_gif: + plt.savefig(f"frame_{i:03d}.png") # Save each frame as an image + frames.append(imageio.imread(f"frame_{i:03d}.png")) + # Update obstacle positions obs_positions = grid.get_obstacle_positions_at_time(i) obs_points.set_data(obs_positions[0], obs_positions[1]) @@ -122,4 +128,7 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N plt.pause(0.2) + if save_gif: + imageio.mimsave("path_animation2.gif", frames, fps=5) # Convert images to GIF + plt.show() \ No newline at end of file From e49b6cfb8c0b7f60e50cfdc04dc4c0ad544f5e19 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 9 Jul 2025 23:06:17 -0400 Subject: [PATCH 16/22] address github bot --- PathPlanning/TimeBasedPathPlanning/BaseClasses.py | 1 - PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py | 3 ++- tests/test_priority_based_planner.py | 6 ------ 3 files changed, 2 insertions(+), 8 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py index be14441d95..416bafe736 100644 --- a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py +++ b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py @@ -1,6 +1,5 @@ from abc import ABC, abstractmethod from dataclasses import dataclass -import math from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( Grid, Position, diff --git a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py index fb54507b9b..1d3d67d05d 100644 --- a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py +++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py @@ -22,6 +22,7 @@ class PriorityBasedPlanner(MultiAgentPlanner): + @staticmethod def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> tuple[list[StartAndGoal], list[NodePath]]: """ Generate a path from the start to the goal for each agent in the `start_and_goals` list. @@ -41,7 +42,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c paths = [] for start_and_goal in start_and_goals: - if True: + if verbose: print(f"\nPlanning for agent: {start_and_goal}" ) grid.clear_initial_reservation(start_and_goal.start, start_and_goal.index) diff --git a/tests/test_priority_based_planner.py b/tests/test_priority_based_planner.py index 23fae46153..e2ff602d88 100644 --- a/tests/test_priority_based_planner.py +++ b/tests/test_priority_based_planner.py @@ -24,12 +24,6 @@ def test_1(): obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, ) - grid_side_length = 21 - grid = Grid( - np.array([grid_side_length, grid_side_length]), - obstacle_arrangement=ObstacleArrangement.ARRANGEMENT1, - ) - m.show_animation = False start_and_goals: list[StartAndGoal] From 8f63c88525721ae1fb372b09d381436e9f384025 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 9 Jul 2025 23:08:04 -0400 Subject: [PATCH 17/22] Revert "add logic for saving animation as gif" This reverts commit 639167795c80a76a1dbd40a6fbf1f1ba70c14d54. --- PathPlanning/TimeBasedPathPlanning/Plotting.py | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/Plotting.py b/PathPlanning/TimeBasedPathPlanning/Plotting.py index 75f0a294e8..44d6f7ce71 100644 --- a/PathPlanning/TimeBasedPathPlanning/Plotting.py +++ b/PathPlanning/TimeBasedPathPlanning/Plotting.py @@ -1,4 +1,3 @@ -import imageio.v2 as imageio import numpy as np import matplotlib.pyplot as plt from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( @@ -45,7 +44,7 @@ def PlotNodePath(grid: Grid, start: Position, goal: Position, path: NodePath): ''' Plot a series of agent paths. ''' -def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[NodePath], save_gif: bool = False): +def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[NodePath]): fig = plt.figure(figsize=(10, 7)) ax = fig.add_subplot( @@ -99,12 +98,7 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N max_time = max(path.goal_reached_time() for path in paths) # Animation loop - frames = [] for i in range(0, max_time + 1): - if save_gif: - plt.savefig(f"frame_{i:03d}.png") # Save each frame as an image - frames.append(imageio.imread(f"frame_{i:03d}.png")) - # Update obstacle positions obs_positions = grid.get_obstacle_positions_at_time(i) obs_points.set_data(obs_positions[0], obs_positions[1]) @@ -128,7 +122,4 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N plt.pause(0.2) - if save_gif: - imageio.mimsave("path_animation2.gif", frames, fps=5) # Convert images to GIF - plt.show() \ No newline at end of file From 92689edfb2c3b5c014a7079d842df3df35169d6c Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 9 Jul 2025 23:31:54 -0400 Subject: [PATCH 18/22] fix tests --- PathPlanning/TimeBasedPathPlanning/BaseClasses.py | 4 ++-- PathPlanning/TimeBasedPathPlanning/Node.py | 9 +++++++-- PathPlanning/TimeBasedPathPlanning/Plotting.py | 14 ++++++++++++-- .../TimeBasedPathPlanning/PriorityBasedPlanner.py | 4 +--- PathPlanning/TimeBasedPathPlanning/SafeInterval.py | 2 +- .../TimeBasedPathPlanning/SpaceTimeAStar.py | 2 +- tests/test_safe_interval_path_planner.py | 4 +--- tests/test_space_time_astar.py | 6 ++---- 8 files changed, 27 insertions(+), 18 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py index 416bafe736..745cde45fb 100644 --- a/PathPlanning/TimeBasedPathPlanning/BaseClasses.py +++ b/PathPlanning/TimeBasedPathPlanning/BaseClasses.py @@ -42,8 +42,8 @@ class MultiAgentPlanner(ABC): @staticmethod @abstractmethod - def plan(grid: Grid, start_and_goal_positions: list[StartAndGoal], verbose: bool = False) -> list[NodePath]: + def plan(grid: Grid, start_and_goal_positions: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool = False) -> tuple[list[StartAndGoal], list[NodePath]]: """ - Plan for all agents. Returned paths are in the order of the `StartAndGoal` list this object was instantiated with + Plan for all agents. Returned paths are in order corresponding to the returned list of `StartAndGoal` objects """ pass \ No newline at end of file diff --git a/PathPlanning/TimeBasedPathPlanning/Node.py b/PathPlanning/TimeBasedPathPlanning/Node.py index ee3ecf2cf7..728eebb676 100644 --- a/PathPlanning/TimeBasedPathPlanning/Node.py +++ b/PathPlanning/TimeBasedPathPlanning/Node.py @@ -1,6 +1,7 @@ from dataclasses import dataclass from functools import total_ordering import numpy as np +from typing import Sequence @dataclass(order=True) class Position: @@ -61,11 +62,15 @@ def __hash__(self): return hash((self.position, self.time)) class NodePath: - path: list[Node] + path: Sequence[Node] positions_at_time: dict[int, Position] + # Number of nodes expanded while finding this path + expanded_node_count: int - def __init__(self, path: list[Node]): + def __init__(self, path: Sequence[Node], expanded_node_count: int): self.path = path + self.expanded_node_count = expanded_node_count + self.positions_at_time = {} for i, node in enumerate(path): reservation_finish_time = node.time + 1 diff --git a/PathPlanning/TimeBasedPathPlanning/Plotting.py b/PathPlanning/TimeBasedPathPlanning/Plotting.py index 44d6f7ce71..57d6625eaf 100644 --- a/PathPlanning/TimeBasedPathPlanning/Plotting.py +++ b/PathPlanning/TimeBasedPathPlanning/Plotting.py @@ -1,5 +1,6 @@ import numpy as np import matplotlib.pyplot as plt +from matplotlib.backend_bases import KeyEvent from PathPlanning.TimeBasedPathPlanning.GridWithDynamicObstacles import ( Grid, Position, @@ -30,13 +31,18 @@ def PlotNodePath(grid: Grid, start: Position, goal: Position, path: NodePath): # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect( - "key_release_event", lambda event: [exit(0) if event.key == "escape" else None] + "key_release_event", + lambda event: [exit(0) if event.key == "escape" else None] + if isinstance(event, KeyEvent) else None ) for i in range(0, path.goal_reached_time()): obs_positions = grid.get_obstacle_positions_at_time(i) obs_points.set_data(obs_positions[0], obs_positions[1]) path_position = path.get_position(i) + if not path_position: + raise Exception(f"Path position not found for time {i}.") + path_points.set_data([path_position.x], [path_position.y]) plt.pause(0.2) plt.show() @@ -91,7 +97,9 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N # For stopping simulation with the esc key plt.gcf().canvas.mpl_connect( - "key_release_event", lambda event: [exit(0) if event.key == "escape" else None] + "key_release_event", + lambda event: [exit(0) if event.key == "escape" else None] + if isinstance(event, KeyEvent) else None ) # Find the maximum time across all paths @@ -112,6 +120,8 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N print(path) print(i) path_position = path.get_position(i) + if not path_position: + raise Exception(f"Path position not found for time {i}.") # Verify position is valid assert not path_position in obs_positions diff --git a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py index 1d3d67d05d..810fd64b4c 100644 --- a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py +++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py @@ -23,7 +23,7 @@ class PriorityBasedPlanner(MultiAgentPlanner): @staticmethod - def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_class: SingleAgentPlanner, verbose: bool) -> tuple[list[StartAndGoal], list[NodePath]]: + 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 @@ -76,8 +76,6 @@ def main(): ) start_time = time.time() - start_and_goals: list[StartAndGoal] - paths: list[NodePath] start_and_goals, paths = PriorityBasedPlanner.plan(grid, start_and_goals, SafeIntervalPathPlanner, verbose) runtime = time.time() - start_time diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py index e7b2d27fed..e60ada45db 100644 --- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py +++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py @@ -81,7 +81,7 @@ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> # reverse path so it goes start -> goal path.reverse() - return NodePath(path) + return NodePath(path, len(expanded_list)) expanded_idx = len(expanded_list) expanded_list.append(expanded_node) diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py index a413fb7329..c3f633a32f 100644 --- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py +++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py @@ -54,7 +54,7 @@ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> # reverse path so it goes start -> goal path.reverse() - return NodePath(path) + return NodePath(path, len(expanded_set)) expanded_idx = len(expanded_list) expanded_list.append(expanded_node) diff --git a/tests/test_safe_interval_path_planner.py b/tests/test_safe_interval_path_planner.py index f1fbf90d2a..bbcd4e427c 100644 --- a/tests/test_safe_interval_path_planner.py +++ b/tests/test_safe_interval_path_planner.py @@ -18,9 +18,7 @@ def test_1(): ) m.show_animation = False - planner = m.SafeIntervalPathPlanner(grid, start, goal) - - path = planner.plan(False) + path = m.SafeIntervalPathPlanner.plan(grid, start, goal) # path should have 31 entries assert len(path.path) == 31 diff --git a/tests/test_space_time_astar.py b/tests/test_space_time_astar.py index 390c7732dc..1194c61d2e 100644 --- a/tests/test_space_time_astar.py +++ b/tests/test_space_time_astar.py @@ -18,9 +18,7 @@ def test_1(): ) m.show_animation = False - planner = m.SpaceTimeAStar(grid, start, goal) - - path = planner.plan(False) + path = m.SpaceTimeAStar.plan(grid, start, goal) # path should have 28 entries assert len(path.path) == 31 @@ -28,7 +26,7 @@ def test_1(): # path should end at the goal assert path.path[-1].position == goal - assert planner.expanded_node_count < 1000 + assert path.expanded_node_count < 1000 if __name__ == "__main__": conftest.run_this_test(__file__) From d150de517ae5545d69ece8d76dc5dfbb0e3e1b74 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Wed, 9 Jul 2025 23:34:56 -0400 Subject: [PATCH 19/22] docs lint --- .../time_based_grid_search/time_based_grid_search_main.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst index 5dfaf6875b..c4788817e8 100644 --- a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst +++ b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst @@ -85,7 +85,7 @@ Multi-Agent Path Planning ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Priority Based Planning -~~~~~~~~~~~~~~~~~~~~ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ The planner generates an order to plan in, and generates plans for the robots in that order. Each planned path is reserved in the grid, and all future plans must avoid that path. The robots are planned for in descending order of distance from start to goal. From 46646f44625ed3945becf7dec9ca9ef08a214a36 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Jul 2025 17:16:48 -0400 Subject: [PATCH 20/22] add gifs --- .../time_based_grid_search_main.rst | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst index c4788817e8..a44dd20a97 100644 --- a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst +++ b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst @@ -91,7 +91,13 @@ The planner generates an order to plan in, and generates plans for the robots in This is a sub-optimal algorithm because no replanning happens once paths are found. It does not guarantee the shortest path is found for any particular robot, nor that the final set of paths found contains the lowest possible amount of time or movement. -TODO: include a gif +Static Obstacles: + +.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/refs/heads/master/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner/priority_planner2.gif + +Dynamic Obstacles: + +.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/refs/heads/master/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner/priority_planner.gif References From 5bd8d4223d6fc8d4e103694c4b3ed23eeb7e5c06 Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Jul 2025 17:20:33 -0400 Subject: [PATCH 21/22] copilot review --- .../TimeBasedPathPlanning/GridWithDynamicObstacles.py | 2 +- PathPlanning/TimeBasedPathPlanning/Plotting.py | 6 +++--- PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py | 2 +- PathPlanning/TimeBasedPathPlanning/SafeInterval.py | 4 +++- PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py | 4 +++- 5 files changed, 11 insertions(+), 7 deletions(-) diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index 337830dbb9..ccc2989001 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -177,7 +177,7 @@ def generate_narrow_corridor_obstacles(self, obs_count: int) -> list[list[Positi continue obstacle_path = [] - x = 10 # middle of the grid + x = self.grid_size[0] // 2 # middle of the grid for t in range(0, self.time_limit - 1): obstacle_path.append(Position(x, y)) diff --git a/PathPlanning/TimeBasedPathPlanning/Plotting.py b/PathPlanning/TimeBasedPathPlanning/Plotting.py index 57d6625eaf..7cd1f615d8 100644 --- a/PathPlanning/TimeBasedPathPlanning/Plotting.py +++ b/PathPlanning/TimeBasedPathPlanning/Plotting.py @@ -113,7 +113,7 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N # Update each agent's position for (j, path) in enumerate(paths): - path_postitions = [] + path_positions = [] if i <= path.goal_reached_time(): res = path.get_position(i) if not res: @@ -125,8 +125,8 @@ def PlotNodePaths(grid: Grid, start_and_goals: list[StartAndGoal], paths: list[N # Verify position is valid assert not path_position in obs_positions - assert not path_position in path_postitions - path_postitions.append(path_position) + assert not path_position in path_positions + path_positions.append(path_position) path_plots[j].set_data([path_position.x], [path_position.y]) diff --git a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py index 810fd64b4c..9d2eddf788 100644 --- a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py +++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py @@ -29,7 +29,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c 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 planner: {single_agent_planner_class}") + print(f"Using single-agent planner: {single_agent_planner_class.__name__}") # Reserve initial positions for start_and_goal in start_and_goals: diff --git a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py index e60ada45db..446847ac6d 100644 --- a/PathPlanning/TimeBasedPathPlanning/SafeInterval.py +++ b/PathPlanning/TimeBasedPathPlanning/SafeInterval.py @@ -70,7 +70,9 @@ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> continue if expanded_node.position == goal: - print(f"Found path to goal after {len(expanded_list)} expansions") + if verbose: + print(f"Found path to goal after {len(expanded_list)} expansions") + path = [] path_walker: SIPPNode = expanded_node while True: diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py index c3f633a32f..b85569f5dc 100644 --- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py +++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py @@ -43,7 +43,9 @@ def plan(grid: Grid, start: Position, goal: Position, verbose: bool = False) -> continue if expanded_node.position == goal: - print(f"Found path to goal after {len(expanded_list)} expansions") + if verbose: + print(f"Found path to goal after {len(expanded_list)} expansions") + path = [] path_walker: Node = expanded_node while True: From e97236abd0923ffdf3885101b8ec8ad3cffb12ea Mon Sep 17 00:00:00 2001 From: SchwartzCode Date: Sat, 12 Jul 2025 17:32:38 -0400 Subject: [PATCH 22/22] appease mypy --- PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py index 9d2eddf788..2573965cf6 100644 --- a/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py +++ b/PathPlanning/TimeBasedPathPlanning/PriorityBasedPlanner.py @@ -29,7 +29,7 @@ def plan(grid: Grid, start_and_goals: list[StartAndGoal], single_agent_planner_c 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.__name__}") + print(f"Using single-agent planner: {single_agent_planner_class}") # Reserve initial positions for start_and_goal in start_and_goals: