StarPilot/openpilot/starpilot/conditional_mode/tests/test_adjacent.py

"""Current serialized model lanes and qualified raw radar-track evidence."""

from dataclasses import replace
import math
import unittest

from openpilot.cereal import messaging
from openpilot.selfdrive.controls.radard import DT_MDL, KalmanParams, Track
from openpilot.starpilot.conditional_mode.adjacent import AdjacentFrame, AdjacentTrack, MonoSource, evaluate


MONO = 100_000_000_000
BOOT = MONO + 2_000_000_000


def wire_model(*, left=-1.8, right=1.8, origin=0.0, step=5.0, bad_length=False, nonmonotonic=False):
  event = messaging.new_message('modelV2', valid=True)
  event.modelV2.timestampEof = BOOT + 10_000_000
  lines = event.modelV2.init('laneLines', 4)
  xs = [origin + float(index) * step for index in range(33)]
  if nonmonotonic:
    xs[5] = xs[4]
  for index, line in enumerate(lines):
    line.x = xs[:-1] if bad_length and index == 1 else xs
    line.y = [left if index == 1 else right if index == 2 else 0.0] * len(line.x)
  return messaging.log_from_bytes(event.to_bytes()).modelV2


def frame(*, tracks=(), model=None, primary=frozenset(), standstill=False, speed=15.0):
  stamp = MONO + 10_000_000
  return AdjacentFrame(
    model=wire_model() if model is None else model,
    tracks=tuple(tracks),
    primary_track_ids=frozenset(primary),
    radar_available=True,
    radar_valid=True,
    radar_error_free=True,
    model_valid=True,
    car_valid=True,
    standstill=standstill,
    ego_speed_mps=speed,
    radar=MonoSource(stamp, stamp),
    model_source=MonoSource(stamp, stamp),
    car=MonoSource(stamp, stamp),
    model_eof_boot_ns=BOOT + 10_000_000,
    now_mono_ns=MONO + 12_000_000,
    now_boot_ns=BOOT + 12_000_000,
    expected_boot_minus_mono_ns=BOOT - MONO,
    barrier_mono_ns=MONO,
    barrier_boot_ns=BOOT,
    sample_skew_ns=1000,
  )


class TestAdjacentEvidence(unittest.TestCase):
  def test_current_radard_track_fields_adapt_without_lead_two(self):
    raw = Track(17, 8.0, KalmanParams(DT_MDL))
    raw.update(25.0, 3.0, -2.0, 8.0)
    candidate = AdjacentTrack(raw.identifier, raw.dRel, raw.yRel, raw.vLead)
    result = evaluate(frame(tracks=(candidate,)))
    self.assertTrue(result.ambiguous)
    self.assertEqual(result.left.track_id, 17)

  def test_left_right_closest_and_selected_primary_exclusion(self):
    tracks = (
      AdjacentTrack(1, 30.0, 3.0, 12.0),
      AdjacentTrack(2, 20.0, -3.0, 14.0),
      AdjacentTrack(3, 10.0, 2.4, 10.0),
      AdjacentTrack(4, 15.0, -2.4, 11.0),
    )
    result = evaluate(frame(tracks=tracks))
    self.assertTrue(result.ambiguous)
    self.assertEqual(result.left.track_id, 3)
    self.assertEqual(result.right.track_id, 4)
    excluded = evaluate(frame(tracks=tracks, primary={3, 4}))
    self.assertTrue(excluded.ambiguous)
    self.assertEqual(excluded.left.track_id, 1)
    self.assertEqual(excluded.right.track_id, 2)
    self.assertEqual(result.observed_mono_ns, MONO + 10_000_000)

  def test_closest_diagnostic_does_not_mask_another_qualified_track(self):
    # Frozen closest-first CCM misses the farther actual ambiguity. Preserve
    # closest diagnostics, but veto for any independently qualified track.
    tracks = (AdjacentTrack(1, 10.0, 6.0, 8.0), AdjacentTrack(2, 20.0, 3.0, 8.0))
    result = evaluate(frame(tracks=tracks))
    self.assertTrue(result.ambiguous)
    self.assertEqual(result.left.track_id, 1)
    slow_near = evaluate(frame(tracks=(AdjacentTrack(1, 10.0, 3.0, 1.0), AdjacentTrack(2, 20.0, 3.0, 8.0))))
    self.assertTrue(slow_near.ambiguous)
    self.assertEqual(slow_near.left.track_id, 1)
    self.assertFalse(evaluate(frame()).ambiguous)
    self.assertFalse(evaluate(frame(tracks=tracks, standstill=True)).ambiguous)
    self.assertIsNone(evaluate(replace(frame(), radar_available=False)).ambiguous)
    self.assertIsNone(evaluate(replace(frame(), radar_valid=False)).ambiguous)
    self.assertIsNone(evaluate(replace(frame(), radar_error_free=False)).ambiguous)
    self.assertIsNone(evaluate(replace(frame(), model_valid=False)).ambiguous)
    self.assertIsNone(evaluate(replace(frame(), car_valid=False)).ambiguous)

  def test_strict_distance_speed_and_lateral_boundaries(self):
    # At 15 m/s, frozen maximum is 52.5 m, and both distance/speed tests
    # are strict. A track that is only 1.0 m/s is not a CCM ambiguity.
    self.assertFalse(evaluate(frame(tracks=(AdjacentTrack(1, 52.5, 3.0, 5.0),))).ambiguous)
    self.assertFalse(evaluate(frame(tracks=(AdjacentTrack(1, 30.0, 3.0, 1.0),))).ambiguous)
    self.assertTrue(evaluate(frame(tracks=(AdjacentTrack(1, 30.0, 5.5, 1.01),))).ambiguous)
    self.assertFalse(evaluate(frame(tracks=(AdjacentTrack(1, 30.0, 5.51, 5.0),))).ambiguous)

  def test_invalid_geometry_and_raw_track_are_unavailable_not_no_lead(self):
    valid_track = (AdjacentTrack(1, 30.0, 3.0, 12.0),)
    for model in (wire_model(bad_length=True), wire_model(nonmonotonic=True), wire_model(left=2.0, right=-2.0)):
      self.assertIsNone(evaluate(frame(model=model, tracks=valid_track)).ambiguous)
    for track in (AdjacentTrack(1, 30.0, math.nan, 12.0), AdjacentTrack(1, 53.0, 3.0, 12.0)):
      if track.distance_m > 52.5:
        # Outside the veto distance, this is valid but irrelevant.
        self.assertFalse(evaluate(frame(tracks=(track,))).ambiguous)
        continue
      self.assertIsNone(evaluate(frame(tracks=(track,))).ambiguous)
    self.assertFalse(evaluate(frame(tracks=(AdjacentTrack(1, 200.0, 3.0, 12.0),))).ambiguous)
    self.assertIsNone(evaluate(frame(tracks=(AdjacentTrack(1, 40.0, 3.0, 12.0),), model=wire_model(step=1.0))).ambiguous)

  def test_small_positive_model_origin_uses_frozen_interpolation_clamp(self):
    self.assertTrue(evaluate(frame(model=wire_model(origin=0.1), tracks=(AdjacentTrack(1, 0.05, 3.0, 8.0),))).ambiguous)

  def test_stale_or_mixed_clock_sources_and_resume_barrier_are_unavailable(self):
    source = frame(tracks=(AdjacentTrack(1, 30.0, 3.0, 12.0),))
    self.assertIsNone(evaluate(replace(source, radar=MonoSource(MONO - 1, MONO + 10_000_000))).ambiguous)
    self.assertIsNone(evaluate(replace(source, model_source=MonoSource(MONO - 1, MONO + 10_000_000))).ambiguous)
    self.assertIsNone(evaluate(replace(source, model_eof_boot_ns=BOOT - 1)).ambiguous)
    self.assertIsNone(evaluate(replace(source, car=MonoSource(MONO - 1, MONO + 10_000_000))).ambiguous)
    self.assertIsNone(evaluate(replace(source, now_boot_ns=source.now_boot_ns + 90_000_000)).ambiguous)
    self.assertIsNone(evaluate(replace(source, barrier_mono_ns=MONO + 10_000_000)).ambiguous)

  def test_producer_age_not_refreshed_by_poll_time(self):
    source = frame(tracks=(AdjacentTrack(1, 30.0, 3.0, 12.0),))
    later = replace(source, now_mono_ns=source.now_mono_ns + 100_000_000, now_boot_ns=source.now_boot_ns + 100_000_000)
    self.assertTrue(evaluate(later).ambiguous)
    self.assertEqual(evaluate(later).observed_mono_ns, source.radar.producer_ns)
    expired = replace(source, now_mono_ns=source.now_mono_ns + 300_000_000, now_boot_ns=source.now_boot_ns + 300_000_000)
    self.assertIsNone(evaluate(expired).ambiguous)