From ccd88323bdf9d7bf67202805b8e7b4be2997b01a Mon Sep 17 00:00:00 2001 From: christiefhyang Date: Thu, 4 Jun 2026 21:13:15 +0800 Subject: [PATCH] Add optional Weighted A* heuristic settings --- PathPlanning/AStar/a_star.py | 42 ++++++++++++++----- tests/test_a_star.py | 80 ++++++++++++++++++++++++++++++++++++ 2 files changed, 112 insertions(+), 10 deletions(-) diff --git a/PathPlanning/AStar/a_star.py b/PathPlanning/AStar/a_star.py index 6d20350432..a1e6fad058 100644 --- a/PathPlanning/AStar/a_star.py +++ b/PathPlanning/AStar/a_star.py @@ -15,10 +15,13 @@ 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 @@ -26,10 +29,24 @@ def __init__(self, ox, oy, resolution, rr): 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): diff --git a/tests/test_a_star.py b/tests/test_a_star.py index 82f76401ad..5ff007c89e 100644 --- a/tests/test_a_star.py +++ b/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__)