diff --git a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py index 7b0190d023..7416ff2f08 100644 --- a/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py +++ b/PathPlanning/TimeBasedPathPlanning/GridWithDynamicObstacles.py @@ -30,6 +30,9 @@ def __sub__(self, other): f"Subtraction not supported for Position and {type(other)}" ) + def __hash__(self): + return hash((self.x, self.y)) + class ObstacleArrangement(Enum): # Random obstacle positions and movements diff --git a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py index a7aed41869..c4e2802d37 100644 --- a/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py +++ b/PathPlanning/TimeBasedPathPlanning/SpaceTimeAStar.py @@ -20,7 +20,7 @@ import random from dataclasses import dataclass from functools import total_ordering - +import time # Seed randomness for reproducibility RANDOM_SEED = 50 @@ -48,11 +48,17 @@ def __lt__(self, other: object): 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] @@ -86,6 +92,8 @@ 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 @@ -98,7 +106,8 @@ def plan(self, verbose: bool = False) -> NodePath: open_set, Node(self.start, 0, self.calculate_heuristic(self.start), -1) ) - expanded_set: list[Node] = [] + expanded_list: list[Node] = [] + expanded_set: set[Node] = set() while open_set: expanded_node: Node = heapq.heappop(open_set) if verbose: @@ -110,23 +119,25 @@ def plan(self, verbose: bool = False) -> NodePath: continue if expanded_node.position == self.goal: - print(f"Found path to goal after {len(expanded_set)} expansions") + print(f"Found path to goal after {len(expanded_list)} expansions") path = [] path_walker: Node = expanded_node while True: path.append(path_walker) if path_walker.parent_index == -1: break - path_walker = expanded_set[path_walker.parent_index] + path_walker = expanded_list[path_walker.parent_index] # reverse path so it goes start -> goal path.reverse() + self.expanded_node_count = len(expanded_set) return NodePath(path) - expanded_idx = len(expanded_set) - expanded_set.append(expanded_node) + 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): + for child in self.generate_successors(expanded_node, expanded_idx, verbose, expanded_set): heapq.heappush(open_set, child) raise Exception("No path found") @@ -135,7 +146,7 @@ def plan(self, verbose: bool = False) -> NodePath: Generate possible successors of the provided `parent_node` """ def generate_successors( - self, parent_node: Node, parent_node_idx: int, verbose: bool + self, parent_node: Node, parent_node_idx: int, verbose: bool, expanded_set: set[Node] ) -> Generator[Node, None, None]: diffs = [ Position(0, 0), @@ -146,13 +157,17 @@ def generate_successors( ] for diff in diffs: new_pos = parent_node.position + diff + new_node = Node( + new_pos, + parent_node.time + 1, + self.calculate_heuristic(new_pos), + parent_node_idx, + ) + + if new_node in expanded_set: + continue + if self.grid.valid_position(new_pos, parent_node.time + 1): - new_node = Node( - new_pos, - parent_node.time + 1, - self.calculate_heuristic(new_pos), - parent_node_idx, - ) if verbose: print("\tNew successor node: ", new_node) yield new_node @@ -166,9 +181,12 @@ def calculate_heuristic(self, position) -> int: verbose = False def main(): - start = Position(1, 11) + start = Position(1, 5) goal = Position(19, 19) grid_side_length = 21 + + start_time = time.time() + grid = Grid( np.array([grid_side_length, grid_side_length]), num_obstacles=40, @@ -179,6 +197,9 @@ def main(): planner = SpaceTimeAStar(grid, start, goal) path = planner.plan(verbose) + runtime = time.time() - start_time + print(f"Planning took: {runtime:.5f} seconds") + if verbose: print(f"Path: {path}") 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 0c26badec7..48dc1289c2 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 @@ -16,6 +16,23 @@ Using a time-based cost and heuristic ensures the path found is optimal in terms The cost is the amount of time it takes to reach a given node, and the heuristic is the minimum amount of time it could take to reach the goal from that node, disregarding all obstacles. For a simple scenario where the robot can move 1 cell per time step and stop and go as it pleases, the heuristic for time is equivalent to the heuristic for distance. +One optimization that was added in `this PR `__ was to add an expanded set to the algorithm. The algorithm will not expand nodes that are already in that set. This greatly reduces the number of node expansions needed to find a path, since no duplicates are expanded. It also helps to reduce the amount of memory the algorithm uses. + +Before:: + + Found path to goal after 204490 expansions + Planning took: 1.72464 seconds + Memory usage (RSS): 68.19 MB + + +After:: + + Found path to goal after 2348 expansions + Planning took: 0.01550 seconds + Memory usage (RSS): 64.85 MB + +When starting at (1, 11) in the structured obstacle arrangement (second of the two gifs above). + References: ~~~~~~~~~~~ diff --git a/tests/test_space_time_astar.py b/tests/test_space_time_astar.py index 5290738eb4..390c7732dc 100644 --- a/tests/test_space_time_astar.py +++ b/tests/test_space_time_astar.py @@ -28,6 +28,7 @@ def test_1(): # path should end at the goal assert path.path[-1].position == goal + assert planner.expanded_node_count < 1000 if __name__ == "__main__": conftest.run_this_test(__file__)