Track single lane lines with smooth transitions and predicted clearance

opendbc_repo/opendbc/car/hyundai/interface.py

@@ -33,6 +33,14 @@ class CarInterface(CarInterfaceBase):
   CarController = CarController
   RadarInterface = RadarInterface
 
+  @staticmethod
+  def get_lateral_body_dimensions(cp):
+    # Hyundai's published pre-facelift IONIQ 6 body dimensions, metres.
+    # https://www.hyundai.news/uk/models/electrified/ioniq-6/press-kit/hyundai-ioniq-6-delivers-a-new-electric-mobility-experience.html
+    if cp.carFingerprint == CAR.HYUNDAI_IONIQ_6:
+      return 1.880, 4.855
+    return None
+
   @staticmethod
   def startup_required(cp):
     from opendbc.car.hyundai.g90_startup import required

openpilot/selfdrive/controls/controlsd.py

@@ -37,6 +37,7 @@ from openpilot.starpilot.lateral.lane_centering import (
   LaneCenteringController, LaneCenteringInput, LaneCenteringRequest, LaneCenteringResult,
 )
 from openpilot.starpilot.lateral.lane_feedback import feedback_message
+from openpilot.starpilot.lateral.lane_clearance import current_delay, interface_dimensions
 from openpilot.starpilot.lateral.lane_change_preferences import LiveLaneChangePolicy
 from openpilot.starpilot.lateral.lane_change_smoothing import LaneChangeSmoother, limit_rate
 from openpilot.starpilot.lateral.lane_runtime import LaneCenteringHost, runtime_supported as lane_runtime_supported
@@ -95,7 +96,7 @@ class Controls:
     self.curvature = 0.0
     self.desired_curvature = 0.0
     self.model_turn_assist = ModelTurnAssist()
-    self.lane_centering_controller = LaneCenteringController()
+    self.lane_centering_controller = LaneCenteringController(body=interface_dimensions(self.CI, self.CP))
     self.lateral_pause = LateralPause(read_lateral_pause(self.params), self.params)
     self.lane_change_preferences = LiveLaneChangePolicy(self.params,
       now_ns=0 if os.getenv('REPLAY') == '1' else time.monotonic_ns())
@@ -364,6 +365,7 @@ class Controls:
     lane_base_curvature = new_desired_curvature
     self.lane_centering_applied = 0.0
     self.last_lane_centering_result = None
+    lane_delay = None
     if lane_centering is None and self.lane_centering_host is not None:
       now_ns = int(self.sm.logMonoTime['carState']) if os.getenv('REPLAY') == '1' else time.monotonic_ns()
       lane_centering = self.lane_centering_host.sample(self.sm, now_ns=now_ns,
@@ -374,6 +376,8 @@ class Controls:
       self.lane_centering_controller.reset()
       self.last_lane_centering_result = LaneCenteringResult(None, 0.0, 0, "invalid_request")
     else:
+      lane_now_ns = int(self.sm.logMonoTime['carState']) if os.getenv('REPLAY') == '1' else time.monotonic_ns()
+      lane_delay = current_delay(self.sm, lane_now_ns)
       result = self.lane_centering_controller.update(LaneCenteringInput(
         model=model_v2, model_sample_id=self.sm.logMonoTime['modelV2'],
         model_valid=bool(self.sm.all_checks(['modelV2'])), base_curvature=new_desired_curvature,
@@ -381,6 +385,8 @@ class Controls:
         settings=lane_centering.settings, elapsed_seconds=lane_centering.elapsed_seconds,
         turn_signal_active=bool(CS.leftBlinker or CS.rightBlinker), driver_override=bool(CS.steeringPressed),
         time_discontinuity=lane_centering.time_discontinuity,
+        measured_curvature=self.curvature,
+        lateral_delay_s=lane_delay,
       ))
       self.last_lane_centering_result = result
       if result.candidate_curvature is not None:
@@ -398,6 +404,12 @@ class Controls:
     self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, previous_curvature, new_desired_curvature, lp.roll)
     if self.last_lane_centering_result is not None and self.last_lane_centering_result.correction != 0.0:
       baseline_curvature, _ = clip_curvature(CS.vEgo, previous_curvature, limit_rate(CS.vEgo, previous_curvature, lane_base_curvature, factor), lp.roll)
+      guarded = self.lane_centering_controller.guard_applied(
+        model_v2, baseline_curvature, self.desired_curvature, self.curvature, CS.vEgo,
+        lane_delay)
+      if guarded != self.desired_curvature:
+        self.desired_curvature = guarded
+        curvature_limited = True
       self.lane_centering_applied = self.desired_curvature - baseline_curvature
     from openpilot.starpilot.car.subaru.smoothing import seconds as subaru_smooth_seconds, control_maximum
     lat_delay = self.sm["lateralDelay"].lateralDelay + LAT_SMOOTH_SECONDS + subaru_smooth_seconds(CS.vEgo, control_maximum(self))

openpilot/starpilot/lateral/lane_centering.py

@@ -15,6 +15,8 @@ import numpy as np
 
 from openpilot.cereal import log
 from openpilot.common.scalar_clip import clip_scalar
+from openpilot.starpilot.lateral.lane_tracking import SingleLineTracker
+from openpilot.starpilot.lateral.lane_clearance import BodyDimensions, limit_correction, limit_tracking
 
 
 STRENGTH_BASELINE = 1.5
@@ -80,6 +82,8 @@ class LaneCenteringInput:
   turn_signal_active: bool = False
   driver_override: bool = False
   time_discontinuity: bool = False
+  measured_curvature: float | None = None
+  lateral_delay_s: float | None = None
 
 
 @dataclass(frozen=True)
@@ -167,7 +171,7 @@ def _single_boundary_target(model: Any, paths, qualified, lookahead: float, offs
   return target
 
 
-def raw_correction(model: Any, speed_mps: float, offset_m: float, e2e_authority: float, *, _paths=None) -> float | None:
+def raw_correction(model: Any, speed_mps: float, offset_m: float, e2e_authority: float, *, _paths=None, _single_target=None) -> float | None:
   """Return a finite raw curvature correction, or None for unqualified model data."""
   speed = _finite_number(speed_mps)
   offset = _finite_number(offset_m)
@@ -209,7 +213,14 @@ def raw_correction(model: Any, speed_mps: float, offset_m: float, e2e_authority:
       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 _single_target is not None:
+        known_path = left_path if qualified[0] else right_path
+        path_std = _at(std_path, lookahead) if std_path is not None else None
+        if (known_path is None or _at(known_path, lookahead) is None or
+            path_std is None or not 0. <= path_std <= E2E_MAX_PATH_STD):
+          return None
+      target_y = (_single_target if _single_target is not None else
+                  _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
@@ -239,21 +250,35 @@ def _reader_geometry_key(model):
   try:
     return (tuple(model.laneLineProbs), tuple(model.laneLineStds),
             *((tuple(line.x), tuple(line.y)) for line in (model.laneLines[1], model.laneLines[2])),
-            tuple(model.position.x), tuple(model.position.y), tuple(model.position.yStd))
+            tuple(model.position.x), tuple(model.position.y), tuple(model.position.yStd),
+            tuple(model.roadEdgeStds), tuple((tuple(edge.x), tuple(edge.y)) for edge in model.roadEdges))
   except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
     return None
 
 
 class LaneCenteringController:
-  def __init__(self) -> None:
+  def __init__(self, *, body: BodyDimensions | None = None) -> None:
     self._correction = 0.0
     self._geometry_cache = None
+    self._tracker = SingleLineTracker()
+    self._body = body
+    self._clearance_observation = None
 
   def reset(self) -> None:
     self._correction = 0.0
     self._geometry_cache = None
+    self._tracker.reset()
 
   def _result(self, base: float, reason: str) -> LaneCenteringResult:
+    if self._body is not None and self._correction != 0.:
+      source = self._clearance_observation
+      measured = _finite_number(source.measured_curvature) if source is not None else None
+      delay = _finite_number(source.lateral_delay_s) if source is not None else None
+      guarded = (0. if measured is None or delay is None else
+                 limit_correction(source.model, base, self._correction, measured, source.speed_mps, delay, self._body))
+      if guarded != self._correction:
+        self._correction = guarded
+        reason = 'clearance_limited'
     correction = self._correction
     return LaneCenteringResult(base + correction, correction, (correction > 0) - (correction < 0), reason)
 
@@ -263,10 +288,29 @@ class LaneCenteringController:
       self._correction = 0.0
     return self._result(base, reason)
 
+  def guard_applied(self, model, base: float, candidate: float, measured: float, speed: float, delay) -> float:
+    if base == candidate or self._body is None and self._tracker.path is None:
+      return candidate
+    checked_delay = _finite_number(delay)
+    delta = candidate - base
+    allowed = 0. if checked_delay is None else delta
+    if checked_delay is not None:
+      if self._tracker.path is not None:
+        x, y = self._tracker.path
+        source = self._clearance_observation
+        offset = source.settings.offset_m if source is not None else 0.
+        allowed = limit_tracking((x, y + offset), base, allowed, measured, speed, checked_delay)
+      if self._body is not None:
+        allowed = limit_correction(model, base, allowed, measured, speed, checked_delay, self._body)
+    if allowed != delta:
+      self._correction *= abs(allowed / delta)
+    return base + allowed
+
   def update(self, observation: LaneCenteringInput) -> LaneCenteringResult:
     if not isinstance(observation, LaneCenteringInput):
       self.reset()
       return LaneCenteringResult(None, 0.0, 0, "invalid_input")
+    self._clearance_observation = observation
     base = _finite_number(observation.base_curvature)
     speed = _finite_number(observation.speed_mps)
     dt = _finite_number(observation.elapsed_seconds)
@@ -294,6 +338,7 @@ class LaneCenteringController:
       self.reset()
       return self._result(base, "driver_override")
     if settings.pause_on_signal and observation.turn_signal_active:
+      self._tracker.reset()
       return self._fade(base, dt, "signal_release")
     try:
       if observation.model.meta.laneChangeState != log.LaneChangeState.off:
@@ -316,7 +361,9 @@ class LaneCenteringController:
         self._geometry_cache = (observation.model, cache_key, paths)
     else:
       self._geometry_cache = None
-    raw = raw_correction(observation.model, speed, offset, authority, _paths=paths)
+    tracked = self._tracker.target(observation.model, observation.model_sample_id, speed, dt, offset, revision=geometry)
+    raw = None if tracked.blocked else raw_correction(observation.model, speed, offset, authority,
+                                                     _paths=paths, _single_target=tracked.y)
     if raw is None or not math.isfinite(raw):
       return self._fade(base, dt, "unqualified_model")
     target = clip_scalar(raw, -0.004, 0.004) * 0.30

openpilot/starpilot/lateral/lane_clearance.py

@@ -0,0 +1,144 @@
+"""Compare nominal and corrected motion against current observed road edges."""
+
+from dataclasses import dataclass
+import math
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class BodyDimensions:
+  width: float
+  length: float
+
+  @property
+  def valid(self):
+    return (
+      type(self.width) is float
+      and type(self.length) is float
+      and math.isfinite(self.width)
+      and math.isfinite(self.length)
+      and 1.3 <= self.width <= 2.6
+      and 2.5 <= self.length <= 7.0
+    )
+
+
+def interface_dimensions(interface, cp) -> BodyDimensions | None:
+  provider = getattr(interface, 'get_lateral_body_dimensions', None)
+  if not callable(provider):
+    return None
+  dimensions = provider(cp)
+  if dimensions is None:
+    return None
+  body = BodyDimensions(*dimensions)
+  return body if body.valid else None
+
+
+def current_delay(sm, now_ns: int) -> float | None:
+  from openpilot.cereal.services import SERVICE_LIST
+  stamp = int(sm.logMonoTime['lateralDelay'])
+  if (not sm.all_checks(['lateralDelay']) or not 0 < stamp <= now_ns or
+      now_ns - stamp > int(2.5e9 / SERVICE_LIST['lateralDelay'].frequency)):
+    return None
+  value = float(sm['lateralDelay'].lateralDelay)
+  return value if math.isfinite(value) and 0. <= value <= 1. else None
+
+
+def _center_motion(curvature: float, measured: float, speed: float, delay: float):
+  horizon = min(35.0, max(8.0, speed * 1.5))
+  ds = horizon / 16.0
+  distances = (np.arange(16, dtype=float) + 0.5) * ds
+  # Test the measured-to-command handoff rather than substituting the auxiliary
+  # model position path for the commanded motion.
+  blend = np.clip((distances / speed - delay) / 0.2, 0.0, 1.0)
+  ks = measured + (curvature - measured) * blend
+  headings = np.cumsum(ks * ds)
+  midpoint = headings - ks * ds * 0.5
+  x, y = np.cumsum(np.cos(midpoint) * ds), np.cumsum(np.sin(midpoint) * ds)
+  return x, y, headings
+
+
+def _motion(curvature: float, measured: float, speed: float, delay: float, body: BodyDimensions):
+  x, y, headings = _center_motion(curvature, measured, speed, delay)
+  # Model coordinates originate below the camera. With no measured mounting
+  # offset, full length/width on EACH side conservatively cover any camera
+  # position inside the body rectangle. These are not assumed axle coordinates.
+  longitudinal = np.array([-body.length, -body.length, body.length, body.length])
+  lateral = np.array([-body.width, body.width, -body.width, body.width])
+  c, s = np.cos(headings)[:, None], np.sin(headings)[:, None]
+  return x[:, None] + c * longitudinal - s * lateral, y[:, None] + s * longitudinal + c * lateral
+
+
+def limit_tracking(path, base: float, correction: float, measured: float, speed: float, delay: float) -> float:
+  """A line-tracking contribution must improve its predicted spacing objective."""
+  if not all(math.isfinite(v) for v in (base, correction, measured, speed, delay)) or speed < 5.0 or not 0.0 <= delay <= 1.0:
+    return 0.0
+  x, y, _ = _center_motion(base, measured, speed, delay)
+  selected = (x >= max(2.0, path[0][0])) & (x <= path[0][-1])
+  if np.count_nonzero(selected) < 4:
+    return 0.0
+  original = float(np.mean((y[selected] - np.interp(x[selected], *path)) ** 2))
+  for factor in (1.0, 0.75, 0.5, 0.25):
+    cx, cy, _ = _center_motion(base + correction * factor, measured, speed, delay)
+    if not np.all((cx[selected] >= path[0][0]) & (cx[selected] <= path[0][-1])):
+      continue
+    candidate = float(np.mean((cy[selected] - np.interp(cx[selected], *path)) ** 2))
+    if candidate <= original + 1e-12:
+      return correction * factor
+  return 0.0
+
+
+def limit_correction(model, base: float, correction: float, measured: float, speed: float, delay: float, body: BodyDimensions) -> float:
+  """Retain the largest tested correction that does not worsen edge intrusion.
+
+  This guards only the added lane contribution. The original model command and
+  native curvature, jerk, authority and driver-override limits remain upstream.
+  """
+  if not body.valid or not all(math.isfinite(v) for v in (base, correction, measured, speed, delay)):
+    return 0.0
+  if speed < 5.0 or not 0.0 <= delay <= 1.0 or abs(base) > 0.2 or abs(measured) > 0.2:
+    return 0.0
+  if correction == 0.0:
+    return 0.0
+  edges = []
+  try:
+    for index in (0, 1):
+      if len(model.roadEdges) <= index or len(model.roadEdgeStds) <= index:
+        continue
+      std = float(model.roadEdgeStds[index])
+      if not math.isfinite(std) or std < 0.0:
+        return 0.0
+      if std > 0.3:
+        continue
+      edge = model.roadEdges[index]
+      x, y = np.asarray(edge.x, dtype=float), np.asarray(edge.y, dtype=float)
+      if x.ndim != 1 or y.ndim != 1 or len(x) < 2 or len(x) != len(y) or not np.isfinite(x).all() or not np.isfinite(y).all() or np.any(np.diff(x) <= 0):
+        return 0.0
+      edges.append((index, x, y, np.gradient(y, x), std))
+  except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
+    return 0.0
+  if not edges:
+    return correction
+  original_x, original_y = _motion(base, measured, speed, delay, body)
+  for factor in (1.0, 0.75, 0.5, 0.25):
+    candidate_x, candidate_y = _motion(base + correction * factor, measured, speed, delay, body)
+    accepted = True
+    for side, x, y, slope, std in edges:
+      # Use the same future body samples in both arms; never extrapolate an edge
+      # behind the camera or beyond the observed geometry.
+      rows = np.all((original_x >= x[0]) & (original_x <= x[-1]), axis=1)
+      if np.count_nonzero(rows) < 4 or not np.all((candidate_x[rows] >= x[0]) & (candidate_x[rows] <= x[-1])):
+        accepted = False
+        break
+      ox, oy = original_x[rows].ravel(), original_y[rows].ravel()
+      cx, cy = candidate_x[rows].ravel(), candidate_y[rows].ravel()
+      sign = 1.0 if side == 0 else -1.0
+      old = float(np.min(sign * (oy - np.interp(ox, x, y)) / np.hypot(1.0, np.interp(ox, x, slope))))
+      new = float(np.min(sign * (cy - np.interp(cx, x, y)) / np.hypot(1.0, np.interp(cx, x, slope))))
+      margin = 0.1 + std
+      if min(new, margin) < min(old, margin) - 1e-6:
+        accepted = False
+        break
+    if accepted:
+      return correction * factor
+  return 0.0

openpilot/starpilot/lateral/lane_tracking.py

@@ -0,0 +1,200 @@
+"""Current-line spacing preferences with bounded acquisition and ownership."""
+
+from dataclasses import dataclass
+import math
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class TrackingTarget:
+  y: float | None = None
+  blocked: bool = False
+
+
+class SingleLineTracker:
+  ACQUIRE_SECONDS = 0.20
+  MAX_SOURCE_GAP = 0.15
+  WIDTH_MAX_AGE = 5.0
+  WIDTH_MAX_DISTANCE = 75.0
+  MAX_PATH_DISAGREEMENT = 0.8
+  MIN_CLEARANCE = 1.1
+
+  def __init__(self):
+    self.reset()
+
+  def reset(self):
+    self.clock = 0.0
+    self.travel = 0.0
+    self.width = None
+    self.width_at = self.width_travel = 0.0
+    self.side = None
+    self.spacing = None
+    self.pending = None
+    self.acquired_at = 0.0
+    self.last_good_at = None
+    self.last_id = None
+    self.last_revision = None
+    self.frame_at = 0.0
+    self.last_near = None
+    self.last_slope = None
+    self.path = None
+    self.blocked = False
+
+  def _clear_line(self):
+    self.side = self.spacing = self.pending = self.path = None
+    self.last_near = self.last_slope = self.last_good_at = None
+
+  @staticmethod
+  def _points(path, *, uncertainty=False):
+    x, y = np.asarray(path.x, dtype=float), np.asarray(path.yStd if uncertainty else path.y, dtype=float)
+    if x.ndim != 1 or y.ndim != 1 or len(x) < 2 or len(x) != len(y) or not np.isfinite(x).all() or not np.isfinite(y).all() or np.any(np.diff(x) <= 0):
+      return None
+    return x, y
+
+  @staticmethod
+  def _at(points, x):
+    return float(np.interp(x, *points)) if points[0][0] <= x <= points[0][-1] else None
+
+  def target(self, model, sample_id: int, speed: float, dt: float, offset: float, *, revision=None) -> TrackingTarget:
+    self.clock += dt
+    self.travel += speed * dt
+    lookahead = min(35.0, max(8.0, speed))
+    revision = id(model) if revision is None else revision
+    if sample_id == self.last_id and revision == self.last_revision:
+      if self.clock - self.frame_at > self.MAX_SOURCE_GAP:
+        self._clear_line()
+        self.blocked = False
+        return TrackingTarget()
+      return self._target(lookahead, offset)
+    previous_id, self.last_id = self.last_id, sample_id
+    self.last_revision, self.frame_at = revision, self.clock
+    if previous_id is not None and sample_id < previous_id:
+      self.reset()
+      self.last_id = sample_id
+      return TrackingTarget(blocked=True)
+    try:
+      probs, stds = model.laneLineProbs, model.laneLineStds
+      if len(probs) < 3 or len(stds) < 3 or len(model.laneLines) < 3:
+        raise ValueError
+      if not all(math.isfinite(float(v)) for v in (probs[1], probs[2], stds[1], stds[2])):
+        raise ValueError
+      good = [0.6 <= probs[i] <= 1.0 and 0.0 <= stds[i] <= 0.3 for i in (1, 2)]
+      lines = [self._points(model.laneLines[i]) if good[i - 1] else None for i in (1, 2)]
+      if all(good):
+        if any(line is None for line in lines):
+          raise ValueError
+        valid_lines = [line for line in lines if line is not None]
+        samples = (lookahead * 0.25, lookahead * 0.5, lookahead)
+        widths = []
+        for distance in samples:
+          left, right = (self._at(line, distance) for line in lines)
+          if left is None or right is None or not 2.6 <= right - left <= 4.8:
+            raise ValueError
+          slope = sum(float(np.interp(distance, x, np.gradient(y, x))) for x, y in valid_lines) * 0.5
+          widths.append((right - left) / math.hypot(1.0, slope))
+        width = float(np.median(widths))
+        if not 2.6 <= width <= 4.8:
+          raise ValueError
+        self.width, self.width_at, self.width_travel = width, self.clock, self.travel
+        self._clear_line()
+        self.blocked = False
+        return TrackingTarget()
+      if not any(good):
+        self.path = None
+        if self.last_good_at is not None and self.clock - self.last_good_at > self.MAX_SOURCE_GAP:
+          self._clear_line()
+        return TrackingTarget(blocked=True)
+      side = 0 if good[0] else 1
+      line = lines[side]
+      position = self._points(model.position)
+      uncertainty = self._points(model.position, uncertainty=True)
+      if line is None or position is None or uncertainty is None:
+        raise ValueError
+      x, y = line
+      slopes = np.gradient(y, x)
+      samples = (lookahead * 0.25, lookahead * 0.5, lookahead)
+      distances = []
+      for distance in samples:
+        line_y, path_y, std = self._at(line, distance), self._at(position, distance), self._at(uncertainty, distance)
+        if line_y is None or path_y is None or std is None or not 0.0 <= std <= 0.35:
+          raise ValueError
+        signed = (path_y - line_y) * (1 if side == 0 else -1)
+        normal = signed / math.hypot(1.0, float(np.interp(distance, x, slopes)))
+        if not 0.5 <= normal <= 2.4:
+          raise ValueError
+        distances.append(normal)
+      near = self._at(line, samples[0])
+      slope = float(np.interp(samples[0], x, slopes))
+      gap = self.last_good_at is None or self.clock - self.last_good_at > self.MAX_SOURCE_GAP
+      jump = (
+        self.side == side
+        and self.last_near is not None
+        and self.last_slope is not None
+        and (abs(near - self.last_near) > 0.35 or abs(slope - self.last_slope) > 0.12)
+      )
+      if self.side != side or gap or jump:
+        self._clear_line()
+        self.side = side
+        self.acquired_at = self.clock
+        self.pending = max(self.MIN_CLEARANCE, float(np.median(distances)))
+        if (
+          not jump
+          and self.width is not None
+          and self.clock - self.width_at <= self.WIDTH_MAX_AGE
+          and self.travel - self.width_travel <= self.WIDTH_MAX_DISTANCE
+        ):
+          self.spacing = self.width * 0.5
+      elif self.spacing is None and self.clock - self.acquired_at >= self.ACQUIRE_SECONDS:
+        assert self.pending is not None
+        if abs(float(np.median(distances)) - self.pending) > 0.25:
+          self.acquired_at = self.clock
+          self.pending = max(self.MIN_CLEARANCE, float(np.median(distances)))
+        else:
+          self.spacing = self.pending
+      self.last_good_at, self.last_near, self.last_slope = self.clock, near, slope
+      self.blocked = jump
+      if self.spacing is None:
+        self.path = None
+        return TrackingTarget(blocked=jump)
+      signed_spacing = self.spacing if side == 0 else -self.spacing
+      norm = np.hypot(1.0, slopes)
+      target_x, target_y = x - signed_spacing * slopes / norm, y + signed_spacing / norm
+      if np.any(np.diff(target_x) <= 0):
+        raise ValueError
+      self.path = target_x, target_y
+      target = self._target(lookahead, offset)
+      model_y = self._at(position, lookahead)
+      if target.y is None or abs(target.y - model_y) > self.MAX_PATH_DISAGREEMENT:
+        raise ValueError
+      # An observed road edge vetoes a conflicting target. A distant edge is
+      # not interpreted as the missing lane line or a lane-width measurement.
+      for index in (0, 1):
+        if len(model.roadEdges) <= index or len(model.roadEdgeStds) <= index:
+          continue
+        std = float(model.roadEdgeStds[index])
+        if not math.isfinite(std) or std < 0.0:
+          raise ValueError
+        if std > 0.3:
+          continue
+        edge = self._points(model.roadEdges[index])
+        if edge is None:
+          raise ValueError
+        for distance in samples:
+          edge_y, target_y = self._at(edge, distance), self._at(self.path, distance)
+          if edge_y is None or target_y is None:
+            raise ValueError
+          clearance = (target_y - edge_y) if index == 0 else (edge_y - target_y)
+          if clearance < self.MIN_CLEARANCE + max(-0.3, min(0.3, offset)) * (1 if index == 1 else -1):
+            raise ValueError
+      return target
+    except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
+      self._clear_line()
+      self.blocked = True
+      return TrackingTarget(blocked=True)
+
+  def _target(self, lookahead: float, offset: float) -> TrackingTarget:
+    if self.path is None or self.blocked:
+      return TrackingTarget(blocked=self.blocked)
+    value = self._at(self.path, lookahead)
+    return TrackingTarget(None if value is None else value + max(-0.3, min(0.3, offset)))

openpilot/starpilot/lateral/tests/test_global_lane_runtime.py

@@ -106,6 +106,35 @@ def test_toyota_full_card_startup_admits_normal_lane_host_without_replay_opt_in(
     assert controls.lane_centering_host is not None
 
 
+@pytest.mark.parametrize('identity', GENERIC_STEERING)
+def test_actual_controller_paths_track_one_current_line_and_withdraw_for_fault(identity):
+  from openpilot.starpilot.lateral.tests.test_lane_centering import single
+  with OpenpilotPrefix(), mock.patch.dict(os.environ, {'SIMULATION': '1', 'REPLAY': '1',
+       'LANE_CENTERING_REPLAY_RUNTIME': '0', 'AOL_REPLAY_RUNTIME': '0'}), \
+       mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
+    params = Params()
+    cp = native_steering_params(identity)
+    params.put('CarParams', cp.to_bytes(), block=True)
+    params.put_bool('LaneCentering', True, block=True)
+    params.put('LaneCenteringE2EAuthority', 0., block=True)
+    controls = Controls()
+    for tick in range(20):
+      feed(controls, 2_000_000_000 + tick * 10_000_000, tick)
+      controls.state_control()
+    with mock.patch('openpilot.starpilot.lateral.tests.test_lane_runtime.model',
+                    side_effect=lambda:single(line=-1.2, edge=10.)):
+      for tick in range(20, 90):
+        feed(controls, 2_000_000_000 + tick * 10_000_000, tick)
+        command, _ = controls.state_control()
+      assert command.latActive
+      assert controls.last_lane_centering_result.correction > 0.
+      assert controls.lane_centering_applied > 0.
+      feed(controls, 2_900_000_000, 90, fault=True)
+      command, _ = controls.state_control()
+      assert not command.latActive
+      assert controls.lane_centering_applied == 0.
+
+
 def test_actual_secoc_configuration_requires_its_key_without_affecting_nonsecoc():
   cp = interfaces[TOYOTA.TOYOTA_SIENNA_4TH_GEN].get_non_essential_params(TOYOTA.TOYOTA_SIENNA_4TH_GEN)
   assert cp.secOcRequired and not cp.secOcKeyAvailable

openpilot/starpilot/lateral/tests/test_lane_centering.py

@@ -8,6 +8,8 @@ from typing import Any, cast
 import unittest
 from unittest.mock import patch
 
+import numpy as np
+
 from openpilot.cereal import messaging
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
 from openpilot.starpilot.lateral.lane_centering import (ControlMode, LaneCenteringController, LaneCenteringInput,
@@ -438,3 +440,157 @@ class TestSingleBoundary(unittest.TestCase):
     self.assertEqual(faded.reason, 'signal_release')
     self.assertLess(faded.correction, prior)
 
+
+class TestSingleLineTracking(unittest.TestCase):
+  def test_spacing_preference_cannot_bypass_current_line_geometry_or_path_uncertainty(self):
+    m = single(line=-1.2, edge=10.)
+    m.laneLines[1].x = XS[:10]
+    m.laneLines[1].y = [-1.2] * 10
+    self.assertIsNone(raw_correction(m, 20., 0., 0., _single_target=.6))
+    m = single(line=-1.2, edge=10.)
+    m.position.yStd = [.36] * len(XS)
+    self.assertIsNone(raw_correction(m, 20., 0., 0., _single_target=.6))
+
+  def test_trusted_width_transfers_to_current_line_across_wide_road(self):
+    controller = LaneCenteringController()
+    for frame in range(10):
+      controller.update(observation(model(left=-1.8, right=1.8), model_sample_id=1_000_000_000 + frame * 50_000_000,
+                                    elapsed_seconds=.05))
+    for frame in range(10, 30):
+      m = single(line=-1.2, edge=10.)
+      result = controller.update(observation(m.as_reader(), model_sample_id=1_000_000_000 + frame * 50_000_000,
+                                             elapsed_seconds=.05))
+    self.assertEqual(result.reason, 'qualified')
+    self.assertGreater(result.correction, .0002)
+    assert controller._tracker.spacing is not None
+    self.assertAlmostEqual(controller._tracker.spacing, 1.8, places=6)
+
+  def test_normal_offset_tracks_a_sloping_line_in_both_directions(self):
+    from openpilot.starpilot.lateral.lane_tracking import SingleLineTracker
+    for side in (0, 1):
+      tracker = SingleLineTracker()
+      for frame in range(10):
+        m = model(left=-1.8, right=1.8)
+        for index, intercept in ((1, -1.8), (2, 1.8)):
+          m.laneLines[index].y = [intercept + .15 * x for x in XS]
+        m.position.y = [.15 * x for x in XS]
+        tracker.target(m, frame, 20., .05, 0.)
+      m.laneLineProbs = [0., .95 if side == 0 else .05, .05 if side == 0 else .95, 0.]
+      for index, intercept in ((1, -1.8), (2, 1.8)):
+        m.laneLines[index].y = [intercept + .2 + .15 * x for x in XS]
+      target = tracker.target(m, 11, 20., .05, 0.)
+      self.assertFalse(target.blocked)
+      assert target.y is not None
+      self.assertAlmostEqual(target.y, 3.2, places=5)
+
+  def test_new_line_needs_acquisition_and_jumps_discard_spacing(self):
+    from openpilot.starpilot.lateral.lane_tracking import SingleLineTracker
+    tracker = SingleLineTracker()
+    for frame in range(8):
+      target = tracker.target(single(line=-1.7, edge=10.), frame, 20., .05, 0.)
+      if frame < 4:
+        self.assertIsNone(target.y)
+    assert tracker.spacing is not None
+    self.assertAlmostEqual(tracker.spacing, 1.7, places=6)
+    jump = tracker.target(single(line=-1.1, edge=10.), 9, 20., .05, 0.)
+    self.assertTrue(jump.blocked)
+    self.assertIsNone(jump.y)
+    self.assertIsNone(tracker.spacing)
+
+  def test_missing_sources_and_driver_override_cannot_keep_line_history(self):
+    controller = LaneCenteringController()
+    for frame in range(12):
+      source = observation(single(line=-1.7, edge=10.), model_sample_id=frame + 1, elapsed_seconds=.05)
+      controller.update(source)
+    self.assertIsNotNone(controller._tracker.spacing)
+    controller.update(replace(source, driver_override=True))
+    self.assertIsNone(controller._tracker.spacing)
+    for frame in range(20, 32):
+      controller.update(replace(source, model_sample_id=frame))
+    controller.update(replace(source, turn_signal_active=True, model_sample_id=33))
+    self.assertIsNone(controller._tracker.spacing)
+
+  def test_line_loss_and_reacquisition_remain_filtered_and_bounded(self):
+    controller = LaneCenteringController()
+    previous = 0.
+    for frame in range(100):
+      m = model(left=-1.8, right=1.8) if frame < 10 or frame >= 70 else single(line=-1.2, edge=10.)
+      if 45 <= frame < 50:
+        m.laneLineProbs = [0., .05, .05, 0.]
+      result = controller.update(observation(m, model_sample_id=frame + 1, elapsed_seconds=.05))
+      self.assertLessEqual(abs(result.correction), MAX_CORRECTION)
+      self.assertLessEqual(abs(result.correction - previous), 2 * MAX_CORRECTION * (1 - math.exp(-.05 / .2)) + 1e-15)
+      if 45 <= frame < 50:
+        self.assertEqual(result.reason, 'unqualified_model')
+        self.assertLessEqual(abs(result.correction), abs(previous))
+      previous = result.correction
+
+
+class TestVehicleClearance(unittest.TestCase):
+  def test_final_command_guard_rejects_worsening_tracking_and_preserves_limits(self):
+    from openpilot.starpilot.lateral.lane_clearance import BodyDimensions
+    controller = LaneCenteringController(body=BodyDimensions(1.88, 4.855))
+    m = single(edge=3.)
+    controller._correction = .001
+    result = controller.guard_applied(m, .006, .007, .006, 20., .1)
+    self.assertEqual(result, .006)
+    self.assertEqual(controller._correction, 0.)
+    controller._tracker.path = (np.array(XS), np.full(len(XS), .3))
+    self.assertEqual(controller.guard_applied(m, -.006, -.007, -.006, 20., .1), -.006)
+
+  def test_commanded_curvature_is_checked_independently_of_model_position(self):
+    from openpilot.starpilot.lateral.lane_clearance import BodyDimensions, limit_correction
+    body = BodyDimensions(1.88, 4.855)
+    m = single(line=-1.8, edge=3.)
+    self.assertEqual(m.position.y[20], 0.)
+    # Same perceived path, different actual commanded motion: adding rightward
+    # steering to an already rightward command must not worsen edge intrusion.
+    self.assertEqual(limit_correction(m, .006, .001, .006, 20., .1, body), 0.)
+    self.assertEqual(limit_correction(m, .006, -.001, .006, 20., .1, body), -.001)
+
+  def test_wide_clear_corridor_keeps_the_requested_contribution(self):
+    from openpilot.starpilot.lateral.lane_clearance import BodyDimensions, limit_correction
+    m = single(line=-1.8, edge=10.)
+    m.roadEdges[0].y = [-10.] * len(XS)
+    body = BodyDimensions(1.88, 4.855)
+    for correction in (-.001, .001):
+      self.assertEqual(limit_correction(m, 0., correction, 0., 20., .1, body), correction)
+
+  def test_short_or_malformed_edge_does_not_gain_extrapolated_clearance(self):
+    from openpilot.starpilot.lateral.lane_clearance import BodyDimensions, limit_correction
+    for bad in ('short', 'nan', 'reversed', 'negative_std'):
+      with self.subTest(bad=bad):
+        m = single(edge=10.)
+        if bad == 'short':
+          m.roadEdges[1].x = XS[:5]
+          m.roadEdges[1].y = [10.] * 5
+        elif bad == 'nan':
+          m.roadEdges[1].y = [math.nan] * len(XS)
+        elif bad == 'reversed':
+          m.roadEdges[1].x = list(reversed(XS))
+        else:
+          m.roadEdgeStds = [.1, -.1]
+        self.assertEqual(limit_correction(m, 0., .001, 0., 20., .1, BodyDimensions(1.88, 4.855)), 0.)
+
+  def test_body_guard_withdraws_extra_correction_without_changing_nominal_command(self):
+    from openpilot.starpilot.lateral.lane_clearance import BodyDimensions
+    controller = LaneCenteringController(body=BodyDimensions(1.88, 4.855))
+    m = single(edge=10.)
+    m.roadEdges[0].y = [-10.] * len(XS)
+    for frame in range(30):
+      source = observation(m, model_sample_id=frame + 1, elapsed_seconds=.05,
+                           measured_curvature=0., lateral_delay_s=.1)
+      result = controller.update(source)
+    self.assertGreater(result.correction, 0.)
+    refused = controller.update(replace(source, measured_curvature=None))
+    self.assertEqual(refused.reason, 'clearance_limited')
+    self.assertEqual(refused.correction, 0.)
+    self.assertEqual(refused.candidate_curvature, source.base_curvature)
+    self.assertEqual(controller.update(replace(source, driver_override=True)).correction, 0.)
+
+  def test_dimensions_are_explicit_and_invalid_values_are_rejected(self):
+    from openpilot.starpilot.lateral.lane_clearance import BodyDimensions, interface_dimensions
+    self.assertIsNone(interface_dimensions(object(), object()))
+    for width, length in ((True, 4.8), (1.8, math.nan), (0., 4.8), (1.8, 20.)):
+      self.assertFalse(BodyDimensions(width, length).valid)
+

openpilot/starpilot/lateral/tests/test_lane_host.py

@@ -218,3 +218,86 @@ if __name__ == "__main__":
     probe(parser.parse_args().probe)
   else:
     unittest.main()
+
+
+def run_tracking_plant(side, road_curvature, enabled):
+  import numpy as np
+  from openpilot.starpilot.lateral import lane_centering as lane
+  from openpilot.starpilot.lateral.lane_clearance import BodyDimensions
+  from openpilot.starpilot.lateral.tests import test_lane_centering as fixtures
+  from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature as clip
+
+  controller = lane.LaneCenteringController(body=BodyDimensions(1.88, 4.855))
+  speed, dt = 20.0, 0.01
+  y = heading = previous = 0.0
+  measured = road_curvature
+  delayed = [road_curvature] * 10
+  samples = []
+  for tick in range(1200):
+    if tick % 5 == 0:
+      nominal = road_curvature - 0.001 * (y - (-0.6 if side == 0 else 0.6)) - 0.08 * heading
+      m = fixtures.model(path_std=0.02)
+      for index, intercept in enumerate((-5.0, -1.8, 1.8, 5.0)):
+        m.laneLines[index].y = [intercept - y - heading * x + 0.5 * road_curvature * x * x for x in fixtures.XS]
+      m.position.y = [0.5 * nominal * x * x for x in fixtures.XS]
+      m.laneLineProbs = [0.0, 0.95 if tick < 100 or side == 0 else 0.05, 0.95 if tick < 100 or side == 1 else 0.05, 0.0]
+      m.init('roadEdges', 2)
+      for edge, intercept in zip(m.roadEdges, (-5.0, 5.0), strict=True):
+        edge.x = fixtures.XS
+        edge.y = [intercept - y - heading * x + 0.5 * road_curvature * x * x for x in fixtures.XS]
+      m.roadEdgeStds = [0.1, 0.1]
+      m.action.desiredCurvature = nominal
+      model = m.as_reader()
+      stamp = 1_000_000_000 + tick * 10_000_000
+    nominal = model.action.desiredCurvature
+    desired = nominal
+    if enabled:
+      result = controller.update(
+        lane.LaneCenteringInput(
+          model,
+          stamp,
+          True,
+          nominal,
+          speed,
+          lane.ControlMode.LATERAL_ONLY,
+          True,
+          lane.LaneCenteringSettings(True, 0.0, 0.0, True),
+          dt,
+          measured_curvature=measured,
+          lateral_delay_s=0.1,
+        )
+      )
+      assert result.candidate_curvature is not None
+      desired = result.candidate_curvature
+    command, _ = clip(speed, previous, desired, 0.0)
+    baseline, _ = clip(speed, previous, nominal, 0.0)
+    if enabled:
+      command = controller.guard_applied(model, baseline, command, measured, speed, 0.1)
+    assert abs(command) <= 3.0 / speed**2 + 1e-12
+    assert abs(command - previous) <= 5.0 / speed**2 * dt + 1e-12
+    previous = command
+    delayed.append(command)
+    measured += (delayed.pop(0) - measured) * dt / 0.1
+    road_speed = speed * math.cos(heading) / (1 - road_curvature * y)
+    y += speed * math.sin(heading) * dt
+    heading += (speed * measured - road_curvature * road_speed) * dt
+    samples.append((y, heading, command))
+  return {
+    'meanFinalAbsOffset': float(np.mean([abs(r[0]) for r in samples[-200:]])),
+    'minimumLineClearance': min(1.8 - abs(r[0]) for r in samples),
+    'maxAbsHeading': max(abs(r[1]) for r in samples),
+    'finalOffset': y,
+  }
+
+
+
+class TestLaneTrackingPlant(unittest.TestCase):
+  def test_single_line_tracking_reduces_bias_with_kinematic_feedback(self):
+    for side in (0, 1):
+      for curvature in (0., .004, -.004):
+        with self.subTest(side=side, curvature=curvature):
+          original = run_tracking_plant(side, curvature, False)
+          candidate = run_tracking_plant(side, curvature, True)
+          self.assertLess(candidate['meanFinalAbsOffset'], original['meanFinalAbsOffset'] * .8)
+          self.assertGreater(candidate['minimumLineClearance'], original['minimumLineClearance'])
+          self.assertLessEqual(candidate['maxAbsHeading'], original['maxAbsHeading'] + .001)

openpilot/starpilot/lateral/tests/test_lane_runtime.py

@@ -71,8 +71,12 @@ def publish_corolla_params(params):
 
 def feed(controls, timestamp, tick, *, active=True, enabled=True, fault=False, override=False, signal=False, model_age=0,
          can_valid=True, can_timeout=False, aol_lat_only=False, native_session='lane-aol-session', speed=20.,
-         drive_id=500_000_000, native_allowed=True, independent=False):
+         drive_id=500_000_000, native_allowed=True, independent=False, delay_fast=False):
   events = []
+  if not controls.sm.seen['lateralDelay']:
+    initial = messaging.new_message('lateralDelay', valid=True, logMonoTime=timestamp - 250_000_000)
+    initial.lateralDelay.lateralDelay = .1
+    controls.sm.update_msgs((timestamp - 250_000_000) / 1e9, [initial.as_reader()])
 
   def msg(service, size=None):
     event = messaging.new_message(service, size, valid=True, logMonoTime=timestamp)
@@ -97,7 +101,8 @@ def feed(controls, timestamp, tick, *, active=True, enabled=True, fault=False, o
     lp = msg('vehicleParameters')
     lp.stiffnessFactor, lp.steerRatio = 1.0, controls.CP.steerRatio
   msg('longitudinalPlan').aTarget = 0.5
-  msg('lateralDelay').lateralDelay = 0.1
+  if tick % 25 == 0 or delay_fast:
+    msg('lateralDelay').lateralDelay = 0.1
   msg('onroadEvents', 0)
   if tick % 5 == 0:
     event = messaging.new_message('modelV2', valid=True, logMonoTime=timestamp - model_age)
@@ -135,6 +140,55 @@ def feed(controls, timestamp, tick, *, active=True, enabled=True, fault=False, o
 
 
 class LaneRuntimeTests(unittest.TestCase):
+  def test_body_dimensions_follow_selected_vehicle_without_retuning_geometry(self):
+    from openpilot.starpilot.lateral.lane_clearance import BodyDimensions, interface_dimensions
+    stock, tagged = ioniq_candidate()
+    for cp in (stock, tagged):
+      before = cp.to_dict()
+      self.assertEqual(interface_dimensions(HyundaiCarInterface, cp), BodyDimensions(1.88, 4.855))
+      self.assertEqual(cp.to_dict(), before)
+      other = cp.as_reader().as_builder()
+      other.carFingerprint = HyundaiCAR.HYUNDAI_IONIQ_5
+      self.assertIsNone(interface_dimensions(HyundaiCarInterface, other))
+    self.assertIsNone(interface_dimensions(HondaCarInterface, stock))
+
+  def test_body_guard_rejects_incorrect_delay_rate_with_lateral_axis_still_active(self):
+    with OpenpilotPrefix(), mock.patch.dict(os.environ, {'LANE_CENTERING_REPLAY_RUNTIME': '0', 'REPLAY': '1',
+                                                         'AOL_REPLAY_RUNTIME': '0'}), \
+         mock.patch('openpilot.selfdrive.controls.controlsd.messaging.PubMaster'):
+      params = Params()
+      _, tagged = ioniq_candidate()
+      params.put('CarParams', tagged.to_bytes(), block=True)
+      params.put_bool('LaneCentering', True, block=True)
+      params.put('LaneCenteringE2EAuthority', 0., block=True)
+      controls = Controls()
+      for tick in range(35):
+        feed(controls, 2_000_000_000 + tick * 10_000_000, tick)
+        command, _ = controls.state_control()
+      self.assertTrue(command.latActive)
+      self.assertGreater(controls.last_lane_centering_result.correction, 0.)
+      for tick in range(35, 45):
+        feed(controls, 2_000_000_000 + tick * 10_000_000, tick, delay_fast=True)
+        command, _ = controls.state_control()
+      self.assertTrue(command.latActive)
+      self.assertFalse(controls.sm.all_checks(['lateralDelay']))
+      self.assertEqual(controls.last_lane_centering_result.correction, 0.)
+      self.assertEqual(controls.lane_centering_applied, 0.)
+
+  def test_clearance_delay_uses_original_header_and_rejects_stale_or_future_source(self):
+    from openpilot.starpilot.lateral.lane_clearance import current_delay
+    frame = SimpleNamespace(logMonoTime={'lateralDelay':1_000_000_000}, all_checks=lambda services:True)
+    class Source:
+      logMonoTime = frame.logMonoTime
+      all_checks = staticmethod(frame.all_checks)
+      def __getitem__(self, service):
+        return SimpleNamespace(lateralDelay=.1)
+    source = Source()
+    self.assertEqual(current_delay(source, 1_250_000_000), .1)
+    self.assertIsNone(current_delay(source, 1_626_000_000))
+    self.assertIsNone(current_delay(source, 999_999_999))
+    self.assertEqual(source.logMonoTime['lateralDelay'], 1_000_000_000)
+
   def test_steering_pause_original_timing_source_loss_and_units(self):
     from openpilot.starpilot.lateral.pause import LateralPause, PauseSettings, read_settings as read_pause
     state = SimpleNamespace(vEgo=0., leftBlinker=True, rightBlinker=False, canValid=True, canTimeout=False)
@@ -350,6 +404,8 @@ class LaneRuntimeTests(unittest.TestCase):
                                                          lateral_active=True, longitudinal_active=False))
       params.put_bool('LaneCentering', True, block=True)
       controls = disabled
+      self.assertEqual(controls.lane_centering_controller._body.width, 1.88)
+      self.assertEqual(controls.lane_centering_controller._body.length, 4.855)
       self.assertIsNotNone(controls.lane_centering_host)
       for tick in range(35):
         now = 2_000_000_000 + tick * 10_000_000

prebuilt.json

@@ -1 +1 @@
-{"protected_inventory_sha256": "0163f04e2bf1dcb4e8e5506975bb5595cccd28d4e825f890ea0de84ced601598", "schema_version": 1}
+{"protected_inventory_sha256": "a62389cbb4d693d3eed387a5115464a5f17a69cc7619aca095e87762a3212195", "schema_version": 1}