Add optional heuristic weight and tie-break support to A*

PathPlanning/AStar/a_star.py

@@ -15,21 +15,38 @@
 
 show_animation = True
 
+TIE_BREAKER_OPTIONS = (None, "larger_g")
+
 
 class AStarPlanner:
 
-    def __init__(self, ox, oy, resolution, rr):
+    def __init__(self, ox, oy, resolution, rr, heuristic_weight=1.0,
+                 tie_breaker=None):
         """
         Initialize grid map for a star planning
 
         ox: x position list of Obstacles [m]
         oy: y position list of Obstacles [m]
         resolution: grid resolution [m]
         rr: robot radius[m]
+        heuristic_weight: multiplier for the Euclidean heuristic. A value of
+            1.0 keeps the default A* behavior.
+        tie_breaker: optional tie-break strategy. None keeps the default
+            behavior, and "larger_g" prefers nodes farther from the start when
+            priorities are equal.
         """
 
+        if heuristic_weight <= 0.0:
+            raise ValueError("heuristic_weight must be positive")
+        if tie_breaker not in TIE_BREAKER_OPTIONS:
+            raise ValueError(
+                f"tie_breaker must be one of {TIE_BREAKER_OPTIONS}")
+
         self.resolution = resolution
         self.rr = rr
+        self.heuristic_weight = heuristic_weight
+        self.tie_breaker = tie_breaker
+        self.last_expanded_node_count = 0
         self.min_x, self.min_y = 0, 0
         self.max_x, self.max_y = 0, 0
         self.obstacle_map = None
@@ -70,17 +87,16 @@ def planning(self, sx, sy, gx, gy):
 
         open_set, closed_set = dict(), dict()
         open_set[self.calc_grid_index(start_node)] = start_node
+        self.last_expanded_node_count = 0
 
         while True:
             if len(open_set) == 0:
                 print("Open set is empty..")
                 break
 
-            c_id = min(
-                open_set,
-                key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node,
-                                                                     open_set[
-                                                                         o]))
+            c_id = min(open_set,
+                       key=lambda o: self.calc_node_priority(
+                           goal_node, open_set[o]))
             current = open_set[c_id]
 
             # show graph
@@ -105,6 +121,7 @@ def planning(self, sx, sy, gx, gy):
 
             # Add it to the closed set
             closed_set[c_id] = current
+            self.last_expanded_node_count += 1
 
             # expand_grid search grid based on motion model
             for i, _ in enumerate(self.motion):
@@ -144,10 +161,15 @@ def calc_final_path(self, goal_node, closed_set):
 
         return rx, ry
 
-    @staticmethod
-    def calc_heuristic(n1, n2):
-        w = 1.0  # weight of heuristic
-        d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
+    def calc_node_priority(self, goal_node, node):
+        priority = node.cost + self.calc_heuristic(goal_node, node)
+        if self.tie_breaker == "larger_g":
+            return priority, -node.cost
+
+        return priority
+
+    def calc_heuristic(self, n1, n2):
+        d = self.heuristic_weight * math.hypot(n1.x - n2.x, n1.y - n2.y)
         return d
 
     def calc_grid_position(self, index, min_position):

tests/test_a_star.py

@@ -1,3 +1,9 @@
+import contextlib
+import io
+import math
+
+import pytest
+
 import conftest
 from PathPlanning.AStar import a_star as m
 
@@ -7,5 +13,79 @@ def test_1():
     m.main()
 
 
+def create_test_map():
+    ox, oy = [], []
+    for i in range(-10, 60):
+        ox.append(i)
+        oy.append(-10.0)
+    for i in range(-10, 60):
+        ox.append(60.0)
+        oy.append(i)
+    for i in range(-10, 61):
+        ox.append(i)
+        oy.append(60.0)
+    for i in range(-10, 61):
+        ox.append(-10.0)
+        oy.append(i)
+    for i in range(-10, 40):
+        ox.append(20.0)
+        oy.append(i)
+    for i in range(0, 40):
+        ox.append(40.0)
+        oy.append(60.0 - i)
+
+    return ox, oy
+
+
+def plan_path(**planner_options):
+    ox, oy = create_test_map()
+    planner = m.AStarPlanner(ox, oy, 2.0, 1.0, **planner_options)
+    with contextlib.redirect_stdout(io.StringIO()):
+        rx, ry = planner.planning(10.0, 10.0, 50.0, 50.0)
+
+    return planner, rx, ry
+
+
+def calc_path_length(rx, ry):
+    return sum(math.hypot(rx[i] - rx[i - 1], ry[i] - ry[i - 1])
+               for i in range(1, len(rx)))
+
+
+def test_weight_one_keeps_default_path():
+    m.show_animation = False
+
+    _, default_rx, default_ry = plan_path()
+    _, weighted_rx, weighted_ry = plan_path(heuristic_weight=1.0)
+
+    assert weighted_rx == default_rx
+    assert weighted_ry == default_ry
+    assert calc_path_length(weighted_rx, weighted_ry) == pytest.approx(
+        calc_path_length(default_rx, default_ry))
+
+
+def test_weighted_a_star_returns_valid_path():
+    m.show_animation = False
+
+    planner, rx, ry = plan_path(heuristic_weight=1.5,
+                                tie_breaker="larger_g")
+
+    assert rx[0] == pytest.approx(50.0)
+    assert ry[0] == pytest.approx(50.0)
+    assert rx[-1] == pytest.approx(10.0)
+    assert ry[-1] == pytest.approx(10.0)
+    assert calc_path_length(rx, ry) > 0.0
+    assert planner.last_expanded_node_count > 0
+
+
+def test_invalid_a_star_options():
+    ox, oy = create_test_map()
+
+    with pytest.raises(ValueError):
+        m.AStarPlanner(ox, oy, 2.0, 1.0, heuristic_weight=0.0)
+
+    with pytest.raises(ValueError):
+        m.AStarPlanner(ox, oy, 2.0, 1.0, tie_breaker="unknown")
+
+
 if __name__ == '__main__':
     conftest.run_this_test(__file__)