Compare CheckGoal2D orientation against target yaw

[sylvesterkaczmarek]

Description

Fix the orientation half of CheckGoal2D.outputs:reachedGoal.

The node currently compares the absolute current yaw directly with the orientation threshold:

rot <= thresholds[1]

That ignores state.target[2] entirely. A robot can therefore be reported as orientation-reached while still far from a non-zero target yaw; negative current yaw values can also satisfy the condition regardless of angular error.

Compute the shortest wrapped angular error between current yaw and target yaw instead:

abs(normalize_angle(rot - state.target[2]))

and compare that value with the configured threshold.

Validation

• regression test verifies zero yaw does not satisfy a 1-radian target yaw with a 0.1-radian threshold
• regression test verifies the shortest angular difference is used across the +pi/-pi boundary
• position-goal logic is unchanged
• production diff is two added lines and one replaced condition