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

CHANGELOG.md

@@ -25,6 +25,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 - Docs: plugin registry field tables list every field with a required column (README, Overview, Plugin Development)
 
 ### Fixed
+- Common: closed-loop Frenet XY→SD also scores the wrap segment when `first==last` — ego on the last lap meters no longer snaps to `s≈0` with a huge false CTE (KD-tree ties the finish to index 0)
 - Common: `TrajectoryTracker` initializes `path_s` from cumulative arc-length instead of re-projecting the reference through KD-tree Frenet conversion — closed tracks with `first==last` (e.g. bundled Yas Marina race line) no longer get non-monotonic `path_s` with `path_s[-1] == 0`
 - Common: Frenet XY→SD picks the better adjacent segment around the nearest waypoint (and SD→XY brackets by arc-length) — on-path points after corners no longer pick up a huge false CTE from the previous segment
 - Common / Planning: lattice sampling, replan end-of-track gates, and race lap detection use `TrajectoryTracker.track_end_s` (`path_s[-1]`) instead of the stale `path_s[-2]` workaround — avoids `IndexError` on 1-point paths and restores the final closed-track segment after the cumulative `path_s` fix

avlite/c50_common/c54_trajectory_tracker.py

@@ -43,6 +43,7 @@ def __post_init__(self):
     def initialize_trajectory(self, reference_xy_path: list[tuple[float, float]], velocity: list[float]):
         self.path = reference_xy_path
         if reference_xy_path is None or len(reference_xy_path) == 0:
+            self.__closed_duplicated_endpoints = False
             return
 
         self.is_initialized = True  
@@ -70,6 +71,14 @@ def initialize_trajectory(self, reference_xy_path: list[tuple[float, float]], ve
         # np.searchsorted can do O(log n) SD lookups without a second spatial index.
         self.__path_s_array = np.array(self.path_s)
 
+        # Closed race lines ship with a duplicated finish==start waypoint. The KD-tree
+        # then ties the finish to index 0, so XY→SD must also score the wrap segment
+        # (n-2 → n-1); cache the flag so per-query Frenet stays cheap.
+        self.__closed_duplicated_endpoints = (
+            len(self.__reference_path) >= 3
+            and bool(np.allclose(self.__reference_path[0], self.__reference_path[-1]))
+        )
+
         self.path_heading = self.__precompute_path_orientation()
 
         # log.debug(f"TrajectoryTracker initialized with {len(self.path)} waypoints: {self.__reference_sd_path}")
@@ -689,6 +698,26 @@ def convert_sd_orientation_to_xy_orientation(self, s: float, d: float, theta:flo
 
         return x, y, theta
 
+    def _frenet_segment_candidates(self, closest_wp: int) -> list[tuple[int, int]]:
+        """Adjacent segments to score for Frenet projection around ``closest_wp``.
+
+        Open paths: the incoming and/or outgoing segment. Closed paths with
+        duplicated endpoints (``first==last``): when the KD-tree returns index 0
+        at the finish, also score the wrap segment ``(n-2, n-1)`` so the last
+        lap meters do not snap to ``s≈0`` with a huge false CTE.
+        """
+        n = len(self.__reference_path)
+        candidates: list[tuple[int, int]] = []
+        if closest_wp > 0:
+            candidates.append((closest_wp - 1, closest_wp))
+        if closest_wp < n - 1:
+            candidates.append((closest_wp, closest_wp + 1))
+        if self.__closed_duplicated_endpoints and closest_wp == 0:
+            wrap = (n - 2, n - 1)
+            if wrap not in candidates:
+                candidates.append(wrap)
+        return candidates
+
     def _frenet_on_segment(self, point, prev_wp: int, next_wp: int) -> tuple[float, float, float]:
         """Project ``point`` onto segment (prev_wp, next_wp).
 
@@ -755,14 +784,9 @@ def convert_xy_path_to_sd_path(self, points):
         frenet_coords = []
         for idx, point in enumerate(points_array):
             closest_wp = int(closest_wps[idx])
-            # Nearest waypoint alone is ambiguous after corners: the point may lie on
-            # the outgoing segment while the old code always used the incoming one,
-            # producing huge false CTE (e.g. on-path after a 90° turn). Score both.
-            candidates: list[tuple[int, int]] = []
-            if closest_wp > 0:
-                candidates.append((closest_wp - 1, closest_wp))
-            if closest_wp < n - 1:
-                candidates.append((closest_wp, closest_wp + 1))
+            # Nearest waypoint alone is ambiguous after corners / at the closed-loop
+            # seam: score adjacent segments (and the wrap segment when first==last).
+            candidates = self._frenet_segment_candidates(closest_wp)
 
             best = None
             for prev_wp, next_wp in candidates:
@@ -794,11 +818,7 @@ def convert_xy_path_to_sd_path_np(self, points):
         out = np.empty((m, 2), dtype=float)
         for i in range(m):
             closest_wp = int(closest_wps[i])
-            candidates: list[tuple[int, int]] = []
-            if closest_wp > 0:
-                candidates.append((closest_wp - 1, closest_wp))
-            if closest_wp < n - 1:
-                candidates.append((closest_wp, closest_wp + 1))
+            candidates = self._frenet_segment_candidates(closest_wp)
             best = None
             for prev_wp, next_wp in candidates:
                 s, d, dist_sq = self._frenet_on_segment(points_array[i], prev_wp, next_wp)

test/c50_common/test_c54_closed_loop_path_s.py

@@ -71,3 +71,45 @@ def test_track_end_s_matches_final_arc_length_including_short_paths():
     assert abs(tj.track_end_s - tj.path_s[-1]) < 1e-9
     # Stale [-2] workaround is one segment short of the true lap length.
     assert tj.path_s[-2] < tj.track_end_s - 1.0
+
+
+def test_frenet_on_closed_loop_last_segment_keeps_s_near_track_end():
+    """first==last: KD nearest at finish is index 0 — must still score the wrap segment."""
+    path = [(0.0, 0.0), (10.0, 0.0), (10.0, 10.0), (0.0, 10.0), (0.0, 0.0)]
+    tj = TrajectoryTracker(path, velocity=[5.0] * len(path))
+
+    # Mid last segment (0,10)→(0,0): previously snapped to s=0, d=5.
+    s, d = tj.convert_xy_to_sd(0.0, 5.0)
+    assert abs(d) < 1e-6
+    assert abs(s - 35.0) < 1e-6
+
+    s_np, d_np = tj.convert_xy_path_to_sd_path_np([(0.0, 5.0)])[0]
+    assert abs(d_np) < 1e-6
+    assert abs(s_np - 35.0) < 1e-6
+
+    # Near finish on the wrap segment.
+    s2, d2 = tj.convert_xy_to_sd(0.0, 0.5)
+    assert abs(d2) < 1e-6
+    assert abs(s2 - 39.5) < 1e-6
+
+
+def test_bundled_yas_marina_frenet_near_finish_stays_on_wrap_segment():
+    """Shipped Yas Marina race line: last ~0.3 m must not jump to s≈0 / full-lap CTE."""
+    path_json = Path(__file__).resolve().parents[2] / (
+        "avlite/data/yas_marina_real_race_line_mue_0_5_3_m_margin.json"
+    )
+    data = json.loads(path_json.read_text())
+    path = [tuple(pt[:2]) for pt in data["ReferenceLine"]]
+    tj = TrajectoryTracker(path, velocity=list(data["ReferenceSpeed"]))
+
+    x0, y0 = path[-2]
+    x1, y1 = path[-1]
+    t = 0.9
+    x = x0 + t * (x1 - x0)
+    y = y0 + t * (y1 - y0)
+    true_s = tj.path_s[-2] + t * (tj.path_s[-1] - tj.path_s[-2])
+
+    s, d = tj.convert_xy_to_sd(x, y)
+    assert abs(d) < 1e-3
+    assert abs(s - true_s) < 1e-2
+    assert s > tj.track_end_s * 0.99