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

"""Frozen Following/CEM slower-lead behavior with actual current RadarState wire."""

from dataclasses import replace
import unittest

from openpilot.cereal import messaging
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE
from openpilot.starpilot.conditional_mode.policy import LeadEvidence
from openpilot.starpilot.conditional_mode.slower_lead import SlowerLeadDetector, SlowerLeadFrame, following_slow_lead


def wire_lead(*, present=True, radar=True, distance=50.0, speed=20.0, probability=0.95, tracked=True):
  event = messaging.new_message('radarState', valid=True)
  lead = event.radarState.leadOne
  lead.present = present
  lead.radar = radar
  lead.dRel = distance
  lead.vLead = speed
  lead.aLeadK = 0.0
  lead.modelProb = probability
  decoded = messaging.log_from_bytes(event.to_bytes()).radarState.leadOne
  return LeadEvidence(decoded.present, tracked, decoded.radar, decoded.dRel, decoded.vLead, decoded.aLeadK, decoded.modelProb)


def frame(
  tick: int,
  *,
  lead=None,
  speed=30.0,
  headway=1.45,
  long_active=True,
  slower=True,
  stopped=False,
  previous_experimental=False,
  traffic=False,
  sign=False,
  turn=False,
  standstill=False,
):
  return SlowerLeadFrame(
    observed_mono_s=100.0 + tick * 0.05,
    now_mono_s=100.005 + tick * 0.05,
    speed_mps=speed,
    selected_follow_s=headway,
    long_active=long_active,
    lead=lead if lead is not None else wire_lead(),
    slower_option=slower,
    stopped_option=stopped,
    previous_experimental=previous_experimental,
    traffic_mode=traffic,
    stop_sign_confirmed=sign,
    committed_turn_scene=turn,
    standstill=standstill,
  )


class TestSlowerLead(unittest.TestCase):
  def test_following_formula_uses_actual_comfort_brake_and_headway(self):
    self.assertEqual(COMFORT_BRAKE, 2.5)
    self.assertTrue(
      following_slow_lead(long_active=True, tracking=True, option=True, speed_mps=30, lead_distance_m=50, lead_speed_mps=20, selected_follow_s=1.45)
    )
    self.assertFalse(
      following_slow_lead(long_active=True, tracking=True, option=True, speed_mps=30, lead_distance_m=30, lead_speed_mps=20, selected_follow_s=1.45)
    )
    self.assertFalse(
      following_slow_lead(long_active=False, tracking=True, option=True, speed_mps=30, lead_distance_m=50, lead_speed_mps=20, selected_follow_s=1.45)
    )

  def test_filtered_slower_lead_and_authority_source(self):
    detector = SlowerLeadDetector()
    states = [detector.step(frame(tick)) for tick in range(25)]
    self.assertTrue(states[0].following_slow)
    self.assertFalse(states[0].detected)
    self.assertTrue(states[-1].detected)
    off = detector.step(frame(25, long_active=False))
    self.assertFalse(off.following_slow)
    self.assertTrue(off.detected)  # Frozen filter release is not instantaneous.
    for tick in range(26, 46):
      state = detector.step(frame(tick, long_active=False))
    self.assertFalse(state.detected)

  def test_stopped_and_vision_continuity_are_distinct(self):
    detector = SlowerLeadDetector()
    stopped_lead = wire_lead(distance=10, speed=0, radar=True)
    low = detector.step(frame(0, speed=5, lead=stopped_lead, slower=False, stopped=True))
    self.assertTrue(low.stopped_candidate)
    self.assertFalse(low.following_slow)
    for tick in range(1, 15):
      low = detector.step(frame(tick, speed=5, lead=stopped_lead, slower=False, stopped=True))
    self.assertTrue(low.detected)

    vision = SlowerLeadDetector()
    tracked = wire_lead(distance=30, speed=10, radar=False, tracked=True)
    for tick in range(25):
      out = vision.step(frame(tick, speed=20, lead=tracked, previous_experimental=True))
    self.assertTrue(out.detected)
    untracked = wire_lead(distance=30, speed=10, radar=False, tracked=False)
    continuity = vision.step(frame(25, speed=20, lead=untracked, previous_experimental=True))
    self.assertFalse(continuity.following_slow)
    self.assertTrue(continuity.vision_candidate)
    self.assertTrue(continuity.detected)

  def test_fast_clear_unknown_and_repeated_source(self):
    detector = SlowerLeadDetector()
    for tick in range(25):
      detector.step(frame(tick))
    self.assertTrue(detector.detected)
    near_match = wire_lead(distance=50, speed=29.8)
    cleared = detector.step(frame(25, lead=near_match))
    self.assertFalse(cleared.detected)
    value = detector.filter.x
    repeated = detector.step(replace(frame(25), now_mono_s=101.27))
    self.assertIsNone(repeated.detected)
    self.assertEqual(detector.filter.x, value)
    unknown = detector.step(replace(frame(26), selected_follow_s=None))
    self.assertIsNone(unknown.detected)
    self.assertEqual(detector.filter.x, 0.0)
    stale = detector.step(replace(frame(27), now_mono_s=102.0))
    self.assertIsNone(stale.detected)

  def test_standstill_drops_old_consensus_and_malformed_lead_stays_unknown(self):
    detector = SlowerLeadDetector()
    for tick in range(25):
      detector.step(frame(tick))
    halted = detector.step(frame(25, standstill=True))
    self.assertIsNone(halted.detected)
    self.assertEqual(detector.filter.x, 0.0)
    invalid = detector.step(replace(frame(26), lead=wire_lead(distance=50, probability=float('nan'))))
    self.assertIsNone(invalid.detected)

  def test_fresh_known_absence_releases_without_numeric_lead_defaults(self):
    detector = SlowerLeadDetector()
    for tick in range(25):
      present = detector.step(frame(tick))
    self.assertTrue(present.detected)
    absent = LeadEvidence(False, False, True)
    released = detector.step(frame(25, lead=absent))
    self.assertFalse(released.following_slow)
    self.assertFalse(released.detected)
    self.assertFalse(released.stopped_candidate)
    self.assertFalse(released.vision_candidate)
    unknown = detector.step(replace(frame(26, lead=absent), observed_mono_s=100.0))
    self.assertIsNone(unknown.detected)

  def test_model_cadence_advances_when_radar_and_options_update_at_half_rate(self):
    detector = SlowerLeadDetector()
    for tick in range(28):
      model_time = 100.0 + tick * 0.05
      source_time = 100.0 + (tick // 2) * 0.1
      result = detector.step(replace(frame(tick), observed_mono_s=source_time, model_tick_mono_s=model_time))
    self.assertTrue(result.detected)
    value = detector.filter.x
    repeated = detector.step(replace(frame(27), observed_mono_s=model_time, model_tick_mono_s=model_time, now_mono_s=model_time + 0.01))
    self.assertIsNone(repeated.detected)
    self.assertEqual(detector.filter.x, value)


if __name__ == '__main__':
  unittest.main()