Wrap goal-heading comparisons in Dubins and closed-loop RRT

[huxiaoyi-ovo]

Summary

Goal-heading comparisons in Dubins and closed-loop RRT planners currently use raw yaw subtraction. Because heading is circular, equivalent poses on opposite sides of ±π (or separated by a full turn) can be rejected.

Use the existing utils.angle.angle_mod to compare the shortest signed heading difference at four sites:

• RRTDubins.search_best_goal_node
• RRTStarDubins.search_best_goal_node
• ClosedLoopRRTStar.get_goal_indexes
• ClosedLoopRRTStar.check_tracking_path_is_feasible

This complements #1422, which handles the corresponding Reeds-Shepp goal-node check. There is no file overlap with #1422.

Regression coverage

Add tests/test_rrt_goal_heading.py with 22 parameterized cases covering both directions across ±π, ±2π heading offsets, non-equivalent headings, and the closed-loop tracked terminal-heading check. The closed-loop test isolates the heading predicate using a mocked tracking result, keeping the original path feasibility checks otherwise intact.

Validation

Validated commit f03f0c4d in fork staging PR #1:

• Linux, Python 3.14: full bash runtests.sh — 178 passed
• macOS, Python 3.14: full bash runtests.sh — 178 passed
• Windows, Python 3.14: full bash runtests.sh — 178 passed
• Code scanning (CodeQL): passed

The patch contains only the four heading comparisons, three imports, and the targeted regression test.