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

from dataclasses import replace

from openpilot.starpilot.conditional_mode.policy import Authority, ConditionalModePolicy, ManualIntent, ModeChoice, SceneEvidence
from openpilot.starpilot.conditional_mode.stop import StopLightDetector
from openpilot.starpilot.conditional_mode.tests.test_stop import frame


def prime():
  detector = StopLightDetector()
  for tick in range(30):
    result = detector.step(frame(tick))
  assert result.light_detected and detector.committed
  return detector


def test_acquired_stop_survives_fresh_invalid_geometry_until_valid_clear():
  detector = prime()
  for tick in range(30, 140):
    result = detector.step(frame(tick, horizon=None, speed=4.27))
    assert result.light_detected and detector.committed
  for tick in range(140, 160):
    result = detector.step(frame(tick, horizon=192., speed=4.27))
  assert not result.light_detected and not detector.committed


def test_traffic_and_turn_cannot_abandon_short_committed_stop():
  for changes in ({'traffic': True}, {'left': True, 'steering': 45.}):
    detector = prime()
    for tick in range(30, 140):
      result = detector.step(frame(tick, horizon=6.684, speed=4.27, **changes))
      assert result.light_detected and detector.committed


def test_unknown_source_and_cold_invalid_geometry_cannot_authorize_commit():
  detector = StopLightDetector()
  assert detector.step(frame(0, horizon=None)).light_detected is None
  assert not detector.committed
  detector = prime()
  assert detector.step(replace(frame(30), traffic_mode=None)).light_detected is None
  assert detector.committed
  assert not detector.standstill_committed


def test_manual_chill_and_personality_mode_change_do_not_override_committed_stop():
  policy = ConditionalModePolicy()
  authority = Authority(True, True, False, True, True, True)
  for tick, choice in enumerate((ModeChoice.CEM, ModeChoice.CCM, ModeChoice.CEM)):
    now = 100. + tick * .05
    scene = SceneEvidence(observed_mono_s=now, speed_mps=4.27, committed_stop=True)
    result = policy.step(now, choice, ManualIntent.FORCE_CHILL, authority, scene)
    assert result.requested_experimental and result.reason.value == 'cem_stop'
  scene = replace(scene, observed_mono_s=now+.05, committed_stop=False)
  result = policy.step(now+.05, ModeChoice.CEM, ManualIntent.FORCE_CHILL, authority, scene)
  assert not result.requested_experimental and result.reason.value == 'manual_chill'


def test_pedal_override_releases_acquired_stop_without_authority_recreation():
  detector = prime()
  result = detector.step(frame(30, horizon=6.684, speed=4.27, pedal=True))
  assert not result.light_detected and not detector.committed


def test_standstill_commit_survives_unknowns_and_requires_continuous_clear_evidence():
  detector = prime()
  detector.step(frame(30, speed=0., standstill=True, horizon=6., model_should_stop=True))
  assert detector.standstill_committed
  for tick in range(31, 45):
    observation = detector.step(replace(frame(tick), traffic_mode=None))
    assert observation.light_detected is None
    assert detector.standstill_committed
  for tick in range(45, 65):
    detector.step(frame(tick, speed=0., standstill=True, horizon=192., model_should_stop=True))
    assert detector.standstill_committed
  for tick in range(65, 71):
    detector.step(frame(tick, speed=0., standstill=True, horizon=192., model_should_stop=False))
  detector.step(replace(frame(71), traffic_mode=None))
  for tick in range(72, 79):
    detector.step(frame(tick, speed=0., standstill=True, horizon=192., model_should_stop=False))
    assert detector.standstill_committed  # Unknown time contributes no clear dwell.
  for tick in range(79, 86):
    detector.step(frame(tick, speed=0., standstill=True, horizon=192., model_should_stop=False))
  assert not detector.committed and not detector.standstill_committed


def test_ford_stopped_commit_needs_prior_bounded_geometry_and_current_stop_bit():
  from opendbc.car.ford.values import CAR
  for name in CAR:
    detector = StopLightDetector()
    held = replace(frame(0, speed=0., standstill=True, horizon=None, model_should_stop=True),
                   car_fingerprint=str(name), standstill_path_extent_m=.1, longitudinal_stop_active=True)
    detector.step(held)
    assert not detector.committed
    detector.step(replace(held, observed_mono_s=100.05, now_mono_s=100.055))
    assert detector.committed
    missing = replace(held, observed_mono_s=100.1, now_mono_s=100.105, standstill_path_extent_m=None)
    assert detector.step(missing).standstill_hold
    assert not detector.step(replace(missing, observed_mono_s=100.15, now_mono_s=100.155,
                                     pedal_override=True)).standstill_hold
    assert not detector.committed
  for defect in ('unknown', 'invalid', 'no_stop', 'moving', 'stale', 'long_off'):
    detector = StopLightDetector()
    current = replace(frame(0, speed=0., standstill=True, horizon=None, model_should_stop=True),
                      car_fingerprint=str(CAR.FORD_MUSTANG_MACH_E_MK1), standstill_path_extent_m=.1, longitudinal_stop_active=True)
    if defect == 'unknown':
      current = replace(current, car_fingerprint='FORD_UNKNOWN')
    elif defect == 'invalid':
      current = replace(current, standstill_path_extent_m=None)
    elif defect == 'no_stop':
      current = replace(current, model_should_stop=False)
    elif defect == 'moving':
      current = replace(current, standstill=False)
    elif defect == 'stale':
      current = replace(current, observed_mono_s=99.)
    elif defect == 'long_off':
      current = replace(current, longitudinal_stop_active=False)
    detector.step(current)
    detector.step(replace(current, now_mono_s=current.now_mono_s+.05,
                          observed_mono_s=current.observed_mono_s+.05))
    assert not detector.committed, defect