Preserve bounded clearance with one visible lane line

openpilot/starpilot/lateral/lane_centering.py

@@ -133,6 +133,40 @@ def _at(path: tuple[np.ndarray, np.ndarray], distance: float) -> float | None:
   return float(np.interp(distance, x, y))
 
 
+def _single_boundary_target(model: Any, paths, qualified, lookahead: float, offset: float) -> float | None:
+  """Keep the confident trajectory clear of one line; an edge only bounds it.
+
+  A road edge may include a shoulder or another lane, so its midpoint is never
+  a centering target. No width or confidence is assigned to the missing line.
+  """
+  known_left = qualified[0]
+  line_path = paths[0 if known_left else 1]
+  model_path, std_path = paths[2:4]
+  if line_path is None or model_path is None or std_path is None:
+    return None
+  edge_index = 1 if known_left else 0
+  edge_std = _finite_number(model.roadEdgeStds[edge_index])
+  if edge_std is None or not 0.0 <= edge_std <= MAX_LANE_STD:
+    return None
+  edge_path = _path(model.roadEdges[edge_index])
+  if edge_path is None:
+    return None
+  target = None
+  for distance in (lookahead * 0.25, lookahead * 0.5, lookahead):
+    line, edge = _at(line_path, distance), _at(edge_path, distance)
+    model_y, path_std = _at(model_path, distance), _at(std_path, distance)
+    if line is None or edge is None or model_y is None or path_std is None or not 0.0 <= path_std <= E2E_MAX_PATH_STD:
+      return None
+    left, right = (line, edge) if known_left else (edge, line)
+    if not MIN_LANE_WIDTH <= right - left <= MAX_LANE_WIDTH or not left < model_y < right:
+      return None
+    # Project the existing trajectory onto the clearance interval, without
+    # steering toward the unknown lane center or an edge/shoulder midpoint.
+    target = clip_scalar(model_y + clip_scalar(offset, -MAX_OFFSET, MAX_OFFSET),
+                         left + MIN_CENTER_TO_LINE, right - MIN_CENTER_TO_LINE)
+  return target
+
+
 def raw_correction(model: Any, speed_mps: float, offset_m: float, e2e_authority: float, *, _paths=None) -> float | None:
   """Return a finite raw curvature correction, or None for unqualified model data."""
   speed = _finite_number(speed_mps)
@@ -146,23 +180,38 @@ def raw_correction(model: Any, speed_mps: float, offset_m: float, e2e_authority:
     stds = model.laneLineStds
     if len(lines) < 3 or len(probs) < 3 or len(stds) < 3:
       return None
+    qualified = []
     for i in (1, 2):
       prob = _finite_number(probs[i])
       std = _finite_number(stds[i])
-      if prob is None or std is None or not MIN_LANE_PROB <= prob <= 1.0 or not 0.0 <= std <= MAX_LANE_STD:
+      if prob is None or std is None or not 0.0 <= prob <= 1.0 or std < 0.0:
         return None
+      qualified.append(prob >= MIN_LANE_PROB and std <= MAX_LANE_STD)
+    if not any(qualified):
+      return None
     left_path, right_path, model_path = (_path(lines[1]), _path(lines[2]), _path(model.position)) if _paths is None else _paths[:3]
-    if left_path is None or right_path is None or model_path is None:
+    if model_path is None:
       return None
     lookahead = clip_scalar(speed, 8.0, 35.0)
-    left, right, model_y = _at(left_path, lookahead), _at(right_path, lookahead), _at(model_path, lookahead)
-    if left is None or right is None or model_y is None:
+    model_y = _at(model_path, lookahead)
+    if model_y is None:
       return None
-    width = right - left
-    if not MIN_LANE_WIDTH <= width <= MAX_LANE_WIDTH:
-      return None
-    safe_offset = min(MAX_OFFSET, max(0.0, width * 0.5 - MIN_CENTER_TO_LINE))
-    target_y = (left + right) * 0.5 + clip_scalar(offset, -safe_offset, safe_offset)
+    if all(qualified):
+      if left_path is None or right_path is None:
+        return None
+      left, right = _at(left_path, lookahead), _at(right_path, lookahead)
+      if left is None or right is None:
+        return None
+      width = right - left
+      if not MIN_LANE_WIDTH <= width <= MAX_LANE_WIDTH:
+        return None
+      safe_offset = min(MAX_OFFSET, max(0.0, width * 0.5 - MIN_CENTER_TO_LINE))
+      target_y = (left + right) * 0.5 + clip_scalar(offset, -safe_offset, safe_offset)
+    else:
+      std_path = _path_xy(model.position.x, model.position.yStd) if _paths is None else _paths[3]
+      target_y = _single_boundary_target(model, (left_path, right_path, model_path, std_path), qualified, lookahead, offset)
+      if target_y is None:
+        return None
     error = target_y - model_y
     error_abs = abs(error)
     error = math.copysign(max(0.0, error_abs - CENTER_ERROR_DEADBAND), error)

openpilot/starpilot/lateral/tests/test_lane_centering.py

@@ -1,6 +1,8 @@
 """Native model-message fixtures for the standalone curvature contribution."""
 from dataclasses import replace
+import json
 import math
+from pathlib import Path
 from types import SimpleNamespace
 from typing import Any, cast
 import unittest
@@ -320,3 +322,119 @@ class TestLaneCentering(unittest.TestCase):
     result = controller.update(observation(elapsed_seconds=0.2))
     self.assertEqual(result.reason, "qualified")
     self.assertGreater(result.correction, 0.0)
+
+
+def single(*, missing_right=True, line=-0.9, edge=3.0, path=0.0):
+  m = model(left=line if missing_right else -3.0, right=2.0 if missing_right else -line, path=path)
+  m.laneLineProbs = [0., .95 if missing_right else .05, .05 if missing_right else .95, 0.]
+  m.init('roadEdges', 2)
+  for item, y in zip(m.roadEdges, (-3.0 if missing_right else -edge, edge if missing_right else 3.0), strict=True):
+    item.x = XS
+    item.y = [y] * len(XS)
+  m.roadEdgeStds = [.1, .1]
+  return m
+
+
+class TestSingleBoundary(unittest.TestCase):
+  def test_recorded_bd_missing_right_line_recovers_bounded_clearance(self):
+    fixture = json.loads((Path(__file__).parent / 'fixtures/route_bd_single_boundary.json').read_text())
+    m = messaging.new_message('modelV2').modelV2
+    m.from_dict(fixture['model'])
+    self.assertTrue(fixture['recordedCarControl']['latActive'])
+    self.assertEqual(fixture['recordedFeedback']['requestedCorrection'], 0.)
+    self.assertEqual(fixture['recordedFeedback']['reason'], 'unqualified_model')
+    self.assertLess(m.laneLineProbs[2], .6)
+    raw = raw_correction(m, fixture['speedMps'], 0., 0.)
+    assert raw is not None
+    self.assertGreater(raw, 0.)
+    self.assertAlmostEqual(raw, fixture['expectedRaw'], places=12)
+    result = settled(LaneCenteringController(), observation(m, speed_mps=fixture['speedMps']))
+    self.assertLessEqual(abs(result.correction), MAX_CORRECTION)
+    # At the exact earlier bookmark BOTH line probabilities are low and the
+    # left-line std is .919 m. Preserve that refusal rather than invent a lane.
+    refused = messaging.new_message('modelV2').modelV2
+    refused.from_dict(fixture['firstBookmarkRefusal']['model'])
+    self.assertIsNone(raw_correction(refused, 15.75, 0., 0.))
+
+  def test_missing_either_line_recovers_clearance_without_centering_on_edge(self):
+    for missing_right, sign in ((True, 1), (False, -1)):
+      for edge in (2.5, 3.0):
+        with self.subTest(missing_right=missing_right, edge=edge):
+          m = single(missing_right=missing_right, edge=edge)
+          raw = raw_correction(m, 20., 0., 0.)
+          assert raw is not None
+          self.assertAlmostEqual(raw, sign * 2. * (.2 - .08) / 400., places=8)
+          result = settled(LaneCenteringController(), observation(m))
+          self.assertEqual(result.reason, 'qualified')
+          self.assertGreater(sign * result.correction, 0.)
+          self.assertLessEqual(abs(result.correction), MAX_CORRECTION)
+
+  def test_safe_model_path_is_retained_and_missing_line_geometry_is_unused(self):
+    m = single(line=-1.5)
+    self.assertEqual(raw_correction(m, 20., 0., 0.), 0.)
+    m.laneLines[2].x = []
+    m.laneLines[2].y = []
+    self.assertEqual(raw_correction(m, 20., 0., 0.), 0.)
+
+  def test_refuses_both_missing_and_unqualified_path_edge_or_corridor(self):
+    for defect in ('both', 'path_std', 'edge_std', 'nan_edge', 'no_edge', 'crossed', 'wide', 'narrow',
+                   'path_outside', 'near_crossing', 'missing_std', 'short_edge', 'bad_x', 'nan_prob'):
+      with self.subTest(defect=defect):
+        m = single()
+        if defect == 'both':
+          m.laneLineProbs = [0., .05, .05, 0.]
+        elif defect == 'path_std':
+          m.position.yStd = [.36] * len(XS)
+        elif defect == 'edge_std':
+          m.roadEdgeStds = [.1, .31]
+        elif defect == 'nan_edge':
+          m.roadEdges[1].y = [float('nan')] * len(XS)
+        elif defect == 'no_edge':
+          m.roadEdges = []
+        elif defect == 'crossed':
+          m.roadEdges[1].y = [-2.] * len(XS)
+        elif defect == 'wide':
+          m.roadEdges[1].y = [4.] * len(XS)
+        elif defect == 'narrow':
+          m.roadEdges[1].y = [1.] * len(XS)
+        elif defect == 'path_outside':
+          m.position.y = [-1.] * len(XS)
+        elif defect == 'near_crossing':
+          m.roadEdges[1].y = [-2. if x <= 10 else 3. for x in XS]
+        elif defect == 'missing_std':
+          m.position.yStd = []
+        elif defect == 'short_edge':
+          m.roadEdges[1].x = XS[:10]
+          m.roadEdges[1].y = [3.] * 10
+        elif defect == 'bad_x':
+          m.roadEdges[1].x = list(reversed(XS))
+        else:
+          m.laneLineProbs = [0., .95, float('nan'), 0.]
+        self.assertIsNone(raw_correction(m, 20., 0., 0.))
+
+  def test_current_edge_is_rechecked_for_held_reader(self):
+    m = single()
+    source = observation(m.as_reader())
+    controller = LaneCenteringController()
+    first = controller.update(source)
+    self.assertGreater(first.correction, 0.)
+    m.roadEdgeStds = [.1, .31]
+    refused = controller.update(source)
+    self.assertEqual(refused.reason, 'unqualified_model')
+    self.assertAlmostEqual(refused.correction, first.correction * math.exp(-.01 / .2))
+    m.roadEdgeStds = [.1, .1]
+    self.assertEqual(controller.update(source).reason, 'qualified')
+
+  def test_single_boundary_still_obeys_override_freshness_signal_and_axis(self):
+    source = observation(single())
+    for updates in ({'driver_override':True}, {'model_valid':False}, {'lateral_active':False},
+                    {'time_discontinuity':True}):
+      controller = LaneCenteringController()
+      self.assertGreater(settled(controller, source).correction, 0.)
+      self.assertEqual(controller.update(replace(source, **updates)).correction, 0.)
+    controller = LaneCenteringController()
+    prior = settled(controller, source).correction
+    faded = controller.update(replace(source, turn_signal_active=True))
+    self.assertEqual(faded.reason, 'signal_release')
+    self.assertLess(faded.correction, prior)
+

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "6eb344118272c52e2d864a01ad40934ceb8fc964e9037f936a1fd4fc4ef94b40", "schema_version": 1}
+{"protected_inventory_sha256": "8c00f349b0bfdafea755cdc8cb73e63050357ecdf2609f2c0f4f6d917ee983a0", "schema_version": 1}