[Afloat16]

Problem

The curvature planner subtracts headings directly in its terminal cost. Equivalent headings separated by a whole turn therefore contribute a nonzero error, and nearby headings across the angle cut appear almost a full turn apart. In a real query from (0, 0, 0) to (2, 0, 2*pi), this produces a longer curved route instead of the straight two-unit path.

Change

Compute the angular residual with Spatial Math's angdiff before taking the existing pose-error norm. Document that the cost uses the shortest angular difference. Position residuals and the path integration model are retained.

Regressions cover positive and negative whole-turn offsets, the angle cut, and a real planner query whose endpoint heading is equivalent to the start heading.

Validation

• All 6 new regressions fail on the original source. Its equivalent-heading query returns length 3.258883 instead of 2.
• PYTHONPATH=src python -m pytest tests/test_curvature_heading_error.py tests/test_mobile_planners_a.py -k 'Curvature or cost_ or equivalent_goal_heading' -q: 7 passed, 24 deselected.
• Ruff passes for the changed source and new regression file; new-file formatting passes.
• git diff --check passed.

Numerical optimisation results can change when headings differ by whole turns or straddle the angle cut, because the objective now measures their circular separation.

[codacy-production]

Up to standards ✅

🟢 Issues 0 issues

Results:
0 new issues

View in Codacy

🟢 Metrics 0 complexity · 0 duplication

Metric	Results
Complexity	0
Duplication	0

View in Codacy

_{NEW Get contextual insights on your PRs based on Codacy's metrics, along with PR and Jira context, without leaving GitHub. Enable AI reviewer
TIP This summary will be updated as you push new changes.}