StarPilot/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py

from pathlib import Path
import tempfile
import unittest

import numpy as np
from opendbc.car.honda.interface import CarInterface
from opendbc.car.honda.values import CAR
from openpilot.common.params import Params
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
from openpilot.selfdrive.controls.plannerd import update_curve_frame
from openpilot.starpilot.curve_speed.host import CurveHost
from openpilot.starpilot.longitudinal.force_stop import StopPlan
from openpilot.starpilot.longitudinal.force_stop_runtime import ForceStopRuntime
from openpilot.starpilot.longitudinal.tests.test_cruise_ceiling import messages, snapshot
from openpilot.starpilot.longitudinal.tests.test_stop_resume import event as button_event

BASE = 100_000_000_000
DRIVE = BASE - 1_000_000_000


class Frame(dict):
  def __init__(self, tick, *, horizon=40., stopped=False):
    super().__init__(messages()[0])
    self.stamp = BASE + tick * 50_000_000
    self.logMonoTime = dict.fromkeys(self, self.stamp)
    self.recv_time = dict.fromkeys(self, self.stamp / 1e9)
    self.valid = dict.fromkeys(self, True)
    self.alive = dict.fromkeys(self, True)
    self['carState'].vEgo = 0. if stopped else 10.
    self['carState'].standstill = stopped
    self['carState'].canValid = True
    self['carControl'].longActive = True
    model = self['modelV2']
    model.timestampEof = self.stamp + 2_000_000_000
    model.position.x = [horizon * index / 32 for index in range(33)]
    model.position.y = [0.] * 33
    model.orientationRate.z = [0.] * 33
    model.orientationRate.t = [float(index) / 10 for index in range(33)]
    model.velocity.x = [10.] * 33


class ForceStopRuntimeTests(unittest.TestCase):
  def setUp(self):
    temp = tempfile.TemporaryDirectory()
    self.addCleanup(temp.cleanup)
    self.params = Params(temp.name)
    self.params.put_bool('QOLLongitudinal', True, block=True)
    self.params.put_bool('ForceStops', True, block=True)
    self.cp = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
    self.owner = ForceStopRuntime(self.params)

  def sample(self, sm, **changes):
    args: dict = {'now_ns': sm.stamp + 1_000_000, 'now_boot_ns': sm.stamp + 2_001_000_000,
            'drive_id': DRIVE, 'follow_seconds': 1.45, 'traffic_mode': False}
    args.update(changes)
    return self.owner.sample(sm, self.cp, **args)

  def prime(self):
    for tick in range(40):
      output = self.sample(Frame(tick))
    self.assertTrue(output.forcing)
    return output

  def test_committed_stop_survives_valid_mode_settings_and_traffic_changes(self):
    import json
    from openpilot.starpilot.conditional_mode.policy import ModeChoice
    from openpilot.starpilot.conditional_mode.preferences import SavedPreferences, encode_preferences
    self.prime()
    self.params.put('ConditionalModeConfig', json.loads(encode_preferences(SavedPreferences(mode=ModeChoice.CCM))), block=True)
    for tick in range(40, 80):
      plan = self.sample(Frame(tick), traffic_mode=True)
      self.assertTrue(plan.forcing)
      assert plan.obstacle_m is not None
    self.owner = ForceStopRuntime(self.params)
    for tick in range(80, 120):
      self.assertFalse(self.sample(Frame(tick), traffic_mode=True).forcing)

  def test_default_off_and_saved_choices_are_not_overwritten(self):
    path = Path(self.params.get_param_path('ForceStops'))
    for raw, expected in ((None, False), (b'0', False), (b'1', True)):
      if raw is None:
        path.unlink(missing_ok=True)
      else:
        path.write_bytes(raw)
      self.owner = ForceStopRuntime(self.params)
      for tick in range(40):
        output = self.sample(Frame(tick))
      self.assertEqual(output.forcing, expected)
      self.assertEqual(path.read_bytes() if path.exists() else None, raw)

  def test_resume_releases_current_stop_once_without_changing_longitudinal_authority(self):
    self.prime()
    self.owner.resume.observe(button_event(197), now_ns=BASE + 2_000_000_001, drive_id=DRIVE)
    self.owner.resume.observe(button_event(198, [('resumeCruise', True)]), now_ns=BASE + 2_000_000_001, drive_id=DRIVE)
    self.owner.resume.observe(button_event(199, [('resumeCruise', False)]), now_ns=BASE + 2_000_000_001, drive_id=DRIVE)
    for tick in range(40, 240):
      output = self.sample(Frame(tick))
      self.assertFalse(output.forcing)
      self.assertIsNone(output.obstacle_m)
      self.assertIsNone(output.speed_ceiling_mps)
    assert self.sample(Frame(240)).speed_ceiling_mps is not None

  def test_resume_during_manual_brake_or_aol_does_not_carry_into_long_engagement(self):
    for manual in ('brake', 'aol'):
      with self.subTest(manual=manual):
        self.owner = ForceStopRuntime(self.params)
        self.prime()
        self.owner.resume.observe(button_event(197), now_ns=BASE + 2_000_000_001, drive_id=DRIVE)
        self.owner.resume.observe(button_event(198, [('accelCruise', True)]), now_ns=BASE + 2_000_000_001, drive_id=DRIVE)
        frame = Frame(40, stopped=True)
        if manual == 'brake':
          frame['carState'].brakePressed = True
        else:
          frame['carControl'].longActive = False
          frame['carControl'].latActive = True
        self.assertFalse(self.sample(frame).forcing)
        self.assertTrue(self.sample(Frame(41, stopped=True)).should_stop)

  def test_recorded_stop_tail_invalidity_preserves_manual_hold_until_resume(self):
    self.prime()
    previous_distance = self.owner.plan.tracked_distance_m
    for tick in range(40, 70):
      sm = Frame(tick, horizon=26.18937873840332)
      # Actual predicted-stop tail: a 12 mm backwards position step, then
      # a following model event with -0.0208 m/s terminal velocity. Both
      # stay inadmissible geometry; neither is evidence that a light cleared.
      if tick % 2 == 0:
        sm['modelV2'].position.x = list(sm['modelV2'].position.x)[:-2] + [26.201595306396484, 26.18937873840332]
      else:
        sm['modelV2'].velocity.x = [10.] * 32 + [-0.02081957273185253]
      plan = self.sample(sm)
      self.assertTrue(plan.forcing)
      self.assertLessEqual(plan.tracked_distance_m, previous_distance)
      assert plan.speed_ceiling_mps is not None
      previous_distance = plan.tracked_distance_m
    for tick in range(70, 100):
      sm = Frame(tick, stopped=True)
      sm['modelV2'].position.x = [float('nan')] * 33
      plan = self.sample(sm)
      self.assertTrue(plan.should_stop)
      self.assertEqual(plan.speed_ceiling_mps, 0.)
    for tick in range(100, 116):
      plan = self.sample(Frame(tick, horizon=190., stopped=True))
    self.assertTrue(plan.forcing)
    self.assertTrue(plan.manual_hold)
    now = BASE + 5_800_000_000 + 1_000_000
    for item in (button_event(577), button_event(578, [('resumeCruise', True)]),
                 button_event(579, [('resumeCruise', False)])):
      self.owner.resume.observe(item, now_ns=now, drive_id=DRIVE)
    released = self.sample(Frame(116, horizon=190., stopped=True))
    self.assertFalse(released.manual_hold)
    self.assertFalse(released.forcing)
    self.assertIsNone(released.speed_ceiling_mps)

  def test_cold_invalid_geometry_cannot_acquire_stop(self):
    for tick in range(40):
      sm = Frame(tick)
      sm['modelV2'].velocity.x = [10.] * 32 + [-.02]
      self.assertFalse(self.sample(sm).forcing)

  def test_actual_model_and_saved_gate_reach_active_force_stop(self):
    output = self.prime()
    assert output.obstacle_m is not None
    self.assertEqual(output.jerk_scale, .32)
    self.params.put_bool('QOLLongitudinal', False, block=True)
    self.assertFalse(self.sample(Frame(60)).forcing)
    self.params.put_bool('QOLLongitudinal', True, block=True)
    self.params.put_bool('ForceStops', False, block=True)
    self.assertFalse(self.sample(Frame(80)).forcing)

  def test_bad_sources_and_driver_brake_never_hold_a_stop(self):
    for defect in ('gas', 'brake', 'can', 'off', 'stock', 'old_drive', 'stale', 'dead', 'eof', 'suspend'):
      with self.subTest(defect=defect):
        self.owner = ForceStopRuntime(self.params)
        self.prime()
        sm = Frame(40)
        args: dict = {}
        if defect in ('gas', 'brake'):
          setattr(sm['carState'], defect + 'Pressed', True)
        elif defect == 'can':
          sm['carState'].canValid = False
        elif defect == 'off':
          sm['carControl'].longActive = False
        elif defect == 'stock':
          self.cp.openpilotLongitudinalControl = False
        elif defect == 'old_drive':
          args['drive_id'] = sm.stamp + 1
        elif defect == 'stale':
          sm.logMonoTime['carState'] -= 150_000_001
        elif defect == 'dead':
          sm.alive['modelV2'] = False
        elif defect == 'eof':
          sm['modelV2'].timestampEof = 0
        elif defect == 'traffic':
          args['traffic_mode'] = True
        else:
          args['now_boot_ns'] = sm.stamp + 3_001_000_000
        plan = self.sample(sm, **args)
        self.assertFalse(plan.forcing)
        self.assertIsNone(plan.obstacle_m)
        self.cp.openpilotLongitudinalControl = True

  def test_aol_brake_approach_retains_perception_without_requesting_control(self):
    for tick in range(40):
      sm = Frame(tick)
      sm['carControl'].longActive = False
      sm['carControl'].latActive = True
      sm['selfdriveState'].enabled = False
      sm['carState'].brakePressed = True
      output = self.sample(sm)
      self.assertFalse(output.forcing)
      self.assertIsNone(output.speed_ceiling_mps)
    self.assertTrue(self.owner.light.light_detected)
    # At zero speed, the stopping-time threshold alone cannot newly detect a
    # light; the same current-drive detector carries the approach observation.
    sm = Frame(40, stopped=True, horizon=30.)
    sm['carControl'].latActive = True
    output = self.sample(sm)
    self.assertTrue(output.forcing)
    self.assertTrue(output.should_stop)
    self.assertEqual(output.speed_ceiling_mps, 0.)
    for tick in range(41, 101):
      output = self.sample(Frame(tick, stopped=True, horizon=30.))
      self.assertTrue(output.should_stop)
    for tick in range(101, 201):
      output = self.sample(Frame(tick, stopped=True, horizon=192.))
    self.assertTrue(output.forcing)
    self.assertTrue(output.manual_hold)

  def test_manual_hold_publishes_dedicated_bit_and_release_withdraws_same_cycle(self):
    from types import SimpleNamespace
    self.prime()
    planner = LongitudinalPlanner(self.cp)
    published = {}
    for tick, gas in ((40, False), (41, False), (42, True)):
      sm = Frame(tick, stopped=True, horizon=30. if tick == 40 else 192.)
      sm['carState'].gasPressed = gas
      sm.all_checks = lambda _: True
      planner.update(sm, now_ns=sm.stamp + 1_000_000, drive_id=DRIVE,
                     force_stop_provider=lambda follow, source=sm: self.sample(source, follow_seconds=follow))
      from unittest.mock import patch
      with patch('openpilot.cereal.messaging.time.monotonic', return_value=(sm.stamp + 1_000_000) / 1e9):
        planner.publish(sm, SimpleNamespace(send=lambda name, event: published.__setitem__(name, event)))
      event = published['longitudinalPlan']
      companion = published['starpilotLongitudinalPlan']
      hold = companion.starpilotLongitudinalPlan
      self.assertTrue(companion.valid)
      self.assertEqual(hold.version, 1)
      self.assertEqual(hold.sourcePlanMonoTime, event.logMonoTime)
      self.assertEqual(companion.logMonoTime, event.logMonoTime)
      self.assertEqual(hold.modelMonoTime, event.longitudinalPlan.modelMonoTime)
      self.assertEqual(hold.driveStartMonoTime, DRIVE)
      self.assertEqual(hold.forceStopHolding, not gas)
      self.assertEqual(planner.force_stop_plan.manual_hold, not gas)
      self.assertEqual(event.longitudinalPlan.modelMonoTime, sm.stamp)
      if not gas:
        self.assertTrue(event.longitudinalPlan.shouldStop)
        # The actual committed stop remains authoritative even when a legacy speed tail is positive.
        from opendbc.car.gm.tests.test_bolt_volt_configurations import ordinary_params
        from opendbc.car.gm.values import CAR as GM_CAR
        from openpilot.selfdrive.controls.lib.longcontrol import LongControl
        from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
        from openpilot.starpilot.longitudinal.tests.test_gm_volt_cc_policy import fixture
        controls, _, _, _, _, _, offset = fixture()
        controls.CP = ordinary_params(GM_CAR.CHEVROLET_VOLT, radar=True)
        controls.LoC = LongControl(controls.CP)
        controls.vehicle_startup_preferences = VehicleStartupPreferences(volt_sng=True)
        event.longitudinalPlan.speeds = [2.] * 17
        controls.sm.data['longitudinalPlan'] = event.longitudinalPlan
        controls.sm['carState'].cruiseState.enabled = controls.sm['carState'].cruiseState.standstill = True
        controls.sm['deviceState'].startedMonoTime = sm.stamp - 1_000_000_000
        for name in ('carState', 'longitudinalPlan', 'deviceState'):
          controls.sm.logMonoTime[name] = sm.stamp - 10_000_000
          controls.sm.recv_time[name] = (sm.stamp - 1_000_000) / 1e9
        from openpilot.starpilot.longitudinal.inputs import ResumeFreshness
        controls.longitudinal_inputs.resume_freshness = ResumeFreshness()
        controls.longitudinal_inputs.resume_freshness.offset_ns = offset
        controls.longitudinal_inputs.resume_freshness.floor_ns = sm.stamp - 500_000_000
        from unittest.mock import patch
        from openpilot.cereal import messaging
        cc = messaging.new_message('carControl').carControl
        cc.enabled = cc.longActive = True
        lateral = messaging.new_message('controlsState').controlsState.lateralControlState.init('torqueState')
        with patch('openpilot.starpilot.longitudinal.inputs.clock_pair_ns', return_value=(sm.stamp, sm.stamp + offset)):
          self.assertTrue(controls.longitudinal_inputs.resume_sources_current())
          controls.publish(cc, lateral)
        self.assertFalse(cc.cruiseControl.resume)
        self.assertTrue(event.longitudinalPlan.shouldStop)

  def test_saved_malformed_and_safe_mode_disable_instead_of_defaulting_on(self):
    for key, raw in (('ForceStops', b'true'), ('ForceStopDistanceOffset', b'21'),
                     ('ForceStopDistanceOffset', b'nan'), ('SafeMode', b'1'),
                     ('ConditionalModeConfig', b'{}')):
      with self.subTest(key=key, raw=raw):
        Path(self.params.get_param_path(key)).write_bytes(raw)
        self.owner = ForceStopRuntime(self.params)
        for tick in range(40):
          self.assertFalse(self.sample(Frame(tick)).forcing)
        Path(self.params.get_param_path(key)).unlink()

  def test_native_solver_gets_same_cycle_stop_and_defaults_remain_identical(self):
    for use_curve in (False, True):
      with self.subTest(use_curve=use_curve):
        self.owner = ForceStopRuntime(self.params)
        self.prime()
        selected = LongitudinalPlanner(self.cp, init_v=10)
        original = LongitudinalPlanner(self.cp, init_v=10)
        inactive = LongitudinalPlanner(self.cp, init_v=10)
        for tick in range(40, 65):
          sm = Frame(tick)
          def provider(follow, frame=sm):
            return self.sample(frame, follow_seconds=follow)
          host = CurveHost(enabled=False, replay=True) if use_curve else None
          update_curve_frame(selected, sm, self.cp, sm.stamp + 1_000_000, host=host, force_stop_provider=provider)
          original.update(sm, now_ns=sm.stamp + 1_000_000)
          inactive.update(sm, now_ns=sm.stamp + 1_000_000, force_stop_provider=lambda _: StopPlan())
          self.assertEqual(snapshot(original), snapshot(inactive))
          np.testing.assert_array_equal(original.mpc.params, inactive.mpc.params)
          np.testing.assert_array_equal(original.mpc.x_sol, inactive.mpc.x_sol)
          self.assertEqual(selected.mpc.solution_status, 0)
          assert selected.force_stop_plan.obstacle_m is not None
          np.testing.assert_array_equal(selected.mpc.params[:, 2], selected.force_stop_plan.obstacle_m + 6.)
          self.assertLessEqual(selected.output_a_target, original.output_a_target)
        # Native stop handoff is explicit even before acceleration alone reaches its threshold.
        sm = Frame(65)
        selected.update(sm, now_ns=sm.stamp + 1_000_000,
                        force_stop_provider=lambda _, stamp=sm.stamp: StopPlan(stamp, 0., None, True, True))
        self.assertTrue(selected.output_should_stop)
        self.assertEqual(selected.force_stop_plan.speed_ceiling_mps, 0.)

  def test_invalid_stop_tail_reaches_native_mpc_as_committed_obstacle(self):
    self.prime()
    selected = LongitudinalPlanner(self.cp, init_v=10)
    for tick in range(40, 55):
      sm = Frame(tick, horizon=26.18937873840332)
      sm['modelV2'].position.x = list(sm['modelV2'].position.x)[:-2] + [26.201595306396484, 26.18937873840332]
      selected.update(sm, now_ns=sm.stamp + 1_000_000,
                      force_stop_provider=lambda follow, frame=sm: self.sample(frame, follow_seconds=follow))
      plan = selected.force_stop_plan
      self.assertTrue(plan.forcing)
      assert plan.obstacle_m is not None
      np.testing.assert_array_equal(selected.mpc.params[:, 2], plan.obstacle_m + 6.)
      self.assertEqual(selected.mpc.solution_status, 0)
      self.assertLess(selected.output_a_target, 0.)

  def test_native_boundary_rejects_old_model_and_driver_override(self):
    for defect in ('old_model', 'gas', 'brake', 'off', 'force'):
      original, proposed = LongitudinalPlanner(self.cp, init_v=10), LongitudinalPlanner(self.cp, init_v=10)
      for tick in range(8):
        sm = Frame(tick)
        if defect in ('gas', 'brake'):
          setattr(sm['carState'], defect + 'Pressed', True)
        elif defect == 'off':
          sm['carControl'].longActive = False
        elif defect == 'force':
          sm['controlsState'].forceDecel = True
        stamp = sm.stamp - 1 if defect == 'old_model' else sm.stamp
        original.update(sm, now_ns=sm.stamp + 1_000_000)
        proposed.update(sm, now_ns=sm.stamp + 1_000_000,
                        force_stop_provider=lambda _, stamp=stamp: StopPlan(stamp, 0., 5., True, True, jerk_scale=.32))
        self.assertEqual(snapshot(original), snapshot(proposed))
        np.testing.assert_array_equal(original.mpc.params, proposed.mpc.params)


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