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.