StarPilot/openpilot/starpilot/lateral/lane_tracking.py

"""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
  DEFAULT_SPACING = 1.7
  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))
        if not all(math.isfinite(value) for value in widths):
          raise ValueError
        width = sorted(widths)[1]
        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, sorted(distances)[1])
        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(sorted(distances)[1] - self.pending) > 0.25:
          self.acquired_at = self.clock
          self.pending = max(self.MIN_CLEARANCE, sorted(distances)[1])
        else:
          # Pending spacing checks acquisition stability. It must not make
          # the model's existing lateral position the tracking objective.
          self.spacing = self.DEFAULT_SPACING
      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)
      # Allow for Float32 model-coordinate rounding at the disagreement bound.
      if target.y is None or abs(target.y - model_y) > self.MAX_PATH_DISAGREEMENT + 1e-6:
        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)))