Add path compression to A*

sea-bass · Jul 4, 2023 · 25b350e · 25b350e
1 parent 8ebdd64
commit 25b350e
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 1 deletion.
diff --git 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
@@ -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: