Skip to content

Fix closed-loop Frenet wrap snap to s≈0 on last lap meters - #24

Draft
cursor[bot] wants to merge 1 commit into
devfrom
cursor/critical-bug-inspection-b93a
Draft

Fix closed-loop Frenet wrap snap to s≈0 on last lap meters#24
cursor[bot] wants to merge 1 commit into
devfrom
cursor/critical-bug-inspection-b93a

Conversation

@cursor

@cursor cursor Bot commented Aug 1, 2026

Copy link
Copy Markdown

Bug and impact

On closed race lines with duplicated endpoints (first==last, e.g. bundled Yas Marina), ego on the last segment got Frenet s≈0 (or slightly negative) instead of s≈track_end. Lattice sampling / lap detection then acted as if the car were at the start/finish while it was still meters before the line — wrong local plans and premature lap counts every lap.

Trigger: Load a closed race line → drive (or place ego) onto the final polyline segment before S/F. Reproduced on Yas Marina: true s≈5250.7 reported as s≈-0.3.

Root cause

PR #21 fixed path_s init and open-path corner segment scoring, but XY→SD still only scored segments adjacent to the KD nearest waypoint. At the finish, first==last makes the tree return index 0, so only segment (0,1) was considered — never the wrap segment (n-2, n-1).

Fix

  • Cache __closed_duplicated_endpoints at init.
  • _frenet_segment_candidates also scores (n-2, n-1) when closest_wp==0 on those paths.
  • Shared by scalar and numpy Frenet converters.

Validation

PYTHONPATH=/workspace python3 -m pytest test/c50_common/test_c54_closed_loop_path_s.py -v

7 passed (includes new square + Yas Marina wrap-segment regressions).

Broader test/c50_common + lattice/velocity/PP: 127 passed (2 failures are env tkinter missing, unrelated).

Memory cleanup

Open in Web View Automation 

On first==last race lines the KD-tree ties the finish to index 0, so
XY→SD only scored the outgoing start segment and reported s≈0 with a
huge false CTE on the last lap meters. Score the wrap segment (n-2,n-1)
when closest_wp==0.

Co-authored-by: Majid Khonji <majid-khonji@users.noreply.github.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant