From 25b350ed7aa79746f9b80ee605d00d47be211016 Mon Sep 17 00:00:00 2001 From: ibrahiminfinite Date: Tue, 4 Jul 2023 08:38:04 +0530 Subject: [PATCH] Add path compression to A* --- pyrobosim/examples/demo_astar.py | 1 + pyrobosim/pyrobosim/navigation/a_star.py | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/pyrobosim/examples/demo_astar.py b/pyrobosim/examples/demo_astar.py index 9117109f..f6de8b8c 100644 --- a/pyrobosim/examples/demo_astar.py +++ b/pyrobosim/examples/demo_astar.py @@ -21,6 +21,7 @@ def demo_astar(): ), "diagonal_motion": True, "heuristic": "euclidean", + "compress_path": False, } planner = PathPlanner("astar", **planner_config) diff --git a/pyrobosim/pyrobosim/navigation/a_star.py b/pyrobosim/pyrobosim/navigation/a_star.py index 5ebf2990..a0249264 100644 --- a/pyrobosim/pyrobosim/navigation/a_star.py +++ b/pyrobosim/pyrobosim/navigation/a_star.py @@ -5,7 +5,7 @@ import warnings from astar import AStar from pyrobosim.utils.pose import Pose -from pyrobosim.utils.motion import Path +from pyrobosim.utils.motion import Path, reduce_waypoints_grid from pyrobosim.navigation.planner_base import PathPlannerBase @@ -127,6 +127,10 @@ def plan(self, start, goal): goal_grid = self.grid.world_to_grid((goal.x, goal.y)) path = self.astar(start_grid, goal_grid) + # Apply waypoint reduction if enabled. + if self.compress_path: + path = reduce_waypoints_grid(self.grid, list(path)) + world_path = [] if path is not None: for waypoint in path: