Wrap yaw error when checking Reeds-Shepp goal pose

[huxiaoyi-ovo]

Summary

• compute the shortest signed angular difference for Reeds-Shepp goal checks
• handle the -pi / +pi wraparound correctly
• add a deterministic regression test at the angle boundary

Problem

RRTStarReedsShepp.search_best_goal_node() currently checks the goal yaw with a direct subtraction:

abs(node.yaw - self.end.yaw) <= self.goal_yaw_th

Angles are periodic, so poses near opposite sides of the -pi / +pi representation boundary can be physically very close while their raw numeric difference is almost 2*pi.

For example, pi - 0.005 and -pi + 0.005 differ by only 0.01 rad on the circle, which is within the planner's 1-degree goal threshold, but the current check sees a difference of about 6.273 rad and rejects the node.

Fix

Compute the wrapped signed angle using atan2(sin(diff), cos(diff)) before applying the existing threshold.

Tests

Added test_goal_yaw_wraparound, which places the start and goal at the same position with headings straddling the -pi / +pi boundary and verifies that the start node is accepted as satisfying the goal yaw threshold.

[Afloat16]

The Reeds-Shepp fix by huxiaoyi-ovo in this PR addresses the circular goal-heading boundary. The same raw-yaw comparison remains in these additional callsites on current master:

• RRTDubins.search_best_goal_node
• RRTStarDubins.search_best_goal_node
• ClosedLoopRRTStar.get_goal_indexes
• ClosedLoopRRTStar.check_tracking_path_is_feasible, for the tracked terminal heading

A separate local support patch and tests are inspectable here:
https://github.com/Afloat16/PythonRobotics/tree/support-rrt-heading-regressions-1422-20261008
Immutable regression tests: https://github.com/Afloat16/PythonRobotics/blob/5a2abc351ed483c28b5b4f32c5cd7a23d08bb42e/tests/test_rrt_goal_heading.py

The added regressions cover ±π boundaries, full-turn offsets and non-equivalent controls. On unchanged cdd0cc8, the full native suite reports 24 new regression failures and 169 passes; seventeen failures are in the uncovered callsites above. The separate support-branch patch applies the existing utils.angle.angle_mod consistently to all five heading checks and gives 193 passes with warnings treated as errors. This 193-pass result is for that separate local patch, not an executed copy of this PR author's patch.

Reproduction from the support branch with Python 3.13.x: install requirements/requirements.txt, then run bash runtests.sh. The observed environment was Python 3.13.16 with the repository-pinned dependencies.

The closed-loop terminal test first runs the native canonical controller, then reuses its measured trajectory to isolate the heading predicate. It does not establish arbitrary unwrapped reference-course behavior. Other CI platforms/versions were not run locally.

[huxiaoyi-ovo]

Thanks for identifying the additional call sites and for sharing the independent regression tests. I verified the four remaining heading comparisons and opened #1457 to address them with the existing angle_mod helper and targeted regressions.

I kept this PR (#1422) limited to the Reeds-Shepp fix, so the two changes can be reviewed independently. The new patch passed the full Linux, macOS, and Windows Python 3.14 test suites on my fork (178 passed on each), as well as CodeQL.