Add missing path compression option in A* planner

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)

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:

test/navigation/test_astar.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python3
+
+"""Unit tests for A-star planner"""
+
+import os
+
+from pyrobosim.core import WorldYamlLoader
+from pyrobosim.navigation import PathPlanner, occupancy_grid_from_world
+from pyrobosim.utils.general import get_data_folder
+from pyrobosim.utils.pose import Pose
+
+
+def test_astar():
+    """Test A* planner with and without path compression"""
+
+    world = WorldYamlLoader().from_yaml(
+        os.path.join(get_data_folder(), "test_world.yaml")
+    )
+
+    start = Pose(x=-1.6, y=2.8)
+    goal = Pose(x=2.5, y=3.0)
+
+    robot = world.robots[0]
+    planner_config = {
+        "grid": occupancy_grid_from_world(
+            world, resolution=0.05, inflation_radius=1.5 * robot.radius
+        ),
+        "diagonal_motion": True,
+        "heuristic": "euclidean",
+        "compress_path": False,
+    }
+    astar_planner = PathPlanner("astar", **planner_config)
+
+    full_path = astar_planner.plan(start, goal).poses
+
+    assert len(full_path) >= 2
+    assert full_path[0] == start
+    assert full_path[-1] == goal
+
+    # Plan for same start and goal with path compression enabled
+    planner_config = {
+        "grid": occupancy_grid_from_world(
+            world, resolution=0.05, inflation_radius=1.5 * robot.radius
+        ),
+        "diagonal_motion": True,
+        "heuristic": "euclidean",
+        "compress_path": True,
+    }
+    astar_planner = PathPlanner("astar", **planner_config)
+
+    compressed_path = astar_planner.plan(start, goal).poses
+
+    assert len(compressed_path) >= 2
+    assert len(compressed_path) <= len(full_path)
+    assert compressed_path[0] == start
+    assert compressed_path[-1] == goal