From f03f0c4d644a19904899f93b722b8e7457404e9d Mon Sep 17 00:00:00 2001 From: huxiaoyi-ovo Date: Fri, 9 Oct 2026 17:22:28 +0800 Subject: [PATCH] Fix wrapped heading comparisons in Dubins and closed-loop RRT --- .../closed_loop_rrt_star_car.py | 5 +- PathPlanning/RRTDubins/rrt_dubins.py | 3 +- PathPlanning/RRTStarDubins/rrt_star_dubins.py | 3 +- tests/test_rrt_goal_heading.py | 105 ++++++++++++++++++ 4 files changed, 112 insertions(+), 4 deletions(-) create mode 100644 tests/test_rrt_goal_heading.py diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index 01ab8349a9..996f4a325f 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -15,6 +15,7 @@ from ClosedLoopRRTStar import pure_pursuit from ClosedLoopRRTStar import unicycle_model from ReedsSheppPath import reeds_shepp_path_planning +from utils.angle import angle_mod from RRTStarReedsShepp.rrt_star_reeds_shepp import RRTStarReedsShepp show_animation = True @@ -108,7 +109,7 @@ def check_tracking_path_is_feasible(self, path): if not find_goal: print("cannot reach goal") - if abs(yaw[-1] - goal[2]) >= self.yaw_th * 10.0: + if abs(angle_mod(yaw[-1] - goal[2])) >= self.yaw_th * 10.0: print("final angle is bad") find_goal = False @@ -140,7 +141,7 @@ def get_goal_indexes(self): # angle check fgoalinds = [] for i in goalinds: - if abs(self.node_list[i].yaw - self.end.yaw) <= self.yaw_th: + if abs(angle_mod(self.node_list[i].yaw - self.end.yaw)) <= self.yaw_th: fgoalinds.append(i) print("OK YAW TH num is") print(len(fgoalinds)) diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py index f938419f35..f8c0b3c2a6 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -16,6 +16,7 @@ from RRT.rrt import RRT from DubinsPath import dubins_path_planner +from utils.angle import angle_mod from utils.plot import plot_arrow show_animation = True @@ -180,7 +181,7 @@ def search_best_goal_node(self): # angle check final_goal_indexes = [] for i in goal_indexes: - if abs(self.node_list[i].yaw - self.end.yaw) <= self.goal_yaw_th: + if abs(angle_mod(self.node_list[i].yaw - self.end.yaw)) <= self.goal_yaw_th: final_goal_indexes.append(i) if not final_goal_indexes: diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py index 7c52879b7c..da80cdf296 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -16,6 +16,7 @@ from DubinsPath import dubins_path_planner from RRTStar.rrt_star import RRTStar +from utils.angle import angle_mod from utils.plot import plot_arrow show_animation = True @@ -187,7 +188,7 @@ def search_best_goal_node(self): # angle check final_goal_indexes = [] for i in goal_indexes: - if abs(self.node_list[i].yaw - self.end.yaw) <= self.goal_yaw_th: + if abs(angle_mod(self.node_list[i].yaw - self.end.yaw)) <= self.goal_yaw_th: final_goal_indexes.append(i) if not final_goal_indexes: diff --git a/tests/test_rrt_goal_heading.py b/tests/test_rrt_goal_heading.py new file mode 100644 index 0000000000..37941253c8 --- /dev/null +++ b/tests/test_rrt_goal_heading.py @@ -0,0 +1,105 @@ +import math + +import pytest + +import conftest # Add root path to sys.path +from PathPlanning.RRTDubins.rrt_dubins import RRTDubins +from PathPlanning.RRTStarDubins.rrt_star_dubins import RRTStarDubins +from PathPlanning.ClosedLoopRRTStar import closed_loop_rrt_star_car as closed_loop + + +@pytest.mark.parametrize("planner_class", [RRTDubins, RRTStarDubins]) +@pytest.mark.parametrize( + "candidate, goal", + [(math.pi - 0.005, -math.pi + 0.005), + (-math.pi + 0.005, math.pi - 0.005), + (0.2 + 2 * math.pi, 0.2), + (-0.2 - 2 * math.pi, -0.2)], +) +def test_dubins_goal_accepts_equivalent_headings(planner_class, candidate, goal): + planner = planner_class( + start=[-1.0, 0.0, 0.0], goal=[0.0, 0.0, goal], + obstacle_list=[], rand_area=[-2.0, 2.0], max_iter=0) + planner.node_list = [planner.Node(0.0, 0.0, goal + 0.2), + planner.Node(0.0, 0.0, candidate)] + + assert planner.search_best_goal_node() == 1 + + +@pytest.mark.parametrize("planner_class", [RRTDubins, RRTStarDubins]) +@pytest.mark.parametrize( + "candidate, goal", + [(math.pi - 0.05, -math.pi + 0.05), (0.2, 0.0)], +) +def test_dubins_goal_rejects_wrong_heading(planner_class, candidate, goal): + planner = planner_class( + start=[-1.0, 0.0, 0.0], goal=[0.0, 0.0, goal], + obstacle_list=[], rand_area=[-2.0, 2.0], max_iter=0) + planner.node_list = [planner.Node(0.0, 0.0, candidate)] + + assert planner.search_best_goal_node() is None + + +@pytest.mark.parametrize( + "candidate, goal", + [(math.pi - 0.01, -math.pi + 0.01), + (-math.pi + 0.01, math.pi - 0.01), + (0.2 + 2 * math.pi, 0.2)], +) +def test_closed_loop_goal_accepts_equivalent_headings(candidate, goal): + planner = closed_loop.ClosedLoopRRTStar( + start=[-1.0, 0.0, 0.0], goal=[0.0, 0.0, goal], + obstacle_list=[], rand_area=[-2.0, 2.0], max_iter=0) + planner.node_list = [planner.Node(0.0, 0.0, goal + 0.2), + planner.Node(0.0, 0.0, candidate)] + + assert planner.get_goal_indexes() == [1] + + +@pytest.mark.parametrize( + "candidate, goal", + [(math.pi - 0.1, -math.pi + 0.1), (0.2, 0.0)], +) +def test_closed_loop_goal_rejects_wrong_heading(candidate, goal): + planner = closed_loop.ClosedLoopRRTStar( + start=[-1.0, 0.0, 0.0], goal=[0.0, 0.0, goal], + obstacle_list=[], rand_area=[-2.0, 2.0], max_iter=0) + planner.node_list = [planner.Node(0.0, 0.0, candidate)] + + assert planner.get_goal_indexes() == [] + + +@pytest.mark.parametrize( + "tracked, goal, expected", + [(math.pi - 0.01, -math.pi + 0.01, True), + (-math.pi + 0.01, math.pi - 0.01, True), + (0.2, 0.2 + 2 * math.pi, True), + (math.pi - 0.35, -math.pi + 0.35, False), + (0.7, 0.0, False)], +) +def test_closed_loop_tracking_heading_wrap(monkeypatch, tracked, goal, expected): + planner = closed_loop.ClosedLoopRRTStar( + start=[0.0, 0.0, 0.0], goal=[1.0, 0.0, goal], + obstacle_list=[], rand_area=[-2.0, 2.0], max_iter=0) + monkeypatch.setattr(closed_loop.pure_pursuit, "extend_path", + lambda cx, cy, cyaw: (cx, cy, cyaw)) + monkeypatch.setattr(closed_loop.pure_pursuit, "calc_speed_profile", + lambda *args: [0.0, 0.0]) + + def simulate_tracking(*args): + return ([0.0, 1.0], [0.0, 1.0], [0.0, 0.0], + [0.0, tracked], [0.0, 1.0], [0.0, 0.0], + [0.0, 0.0], True) + + monkeypatch.setattr(closed_loop.pure_pursuit, "closed_loop_prediction", + simulate_tracking) + monkeypatch.setattr(planner, "check_collision", lambda *args: True) + + path = [[1.0, 0.0, goal], [0.0, 0.0, 0.0]] + feasible, *_ = planner.check_tracking_path_is_feasible(path) + + assert feasible is expected + + +if __name__ == "__main__": + conftest.run_this_test(__file__)