StarPilot/openpilot/selfdrive/controls/tests/test_curve_runtime_loop.py

"""The plannerd curve seam passes a current model cap, then records the winner."""

import unittest
from collections import deque
from types import SimpleNamespace as NS

from openpilot.cereal import messaging
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
from openpilot.selfdrive.controls.plannerd import (
  confirm_curve_frame, curve_for_frame, current_cruise_event, queue_cruise_event, update_curve_frame,
)
from openpilot.starpilot.curve_speed.host import CurveHost
from openpilot.starpilot.longitudinal.cruise_ceiling import CruiseCeiling, CurveCeiling
from openpilot.starpilot.longitudinal.profile_runtime import ProfileTuning
from openpilot.starpilot.speed_limits.acceptance import Authority, LongitudinalOwner, Mode
from opendbc.car.honda.interface import CarInterface
from opendbc.car.honda.values import CAR

NOW = 10_000_000_000


class Bus:
  def __init__(self, messages, stamp):
    self.messages = messages
    self.logMonoTime = dict.fromkeys(messages, stamp)
    self.valid = dict.fromkeys(messages, True)
    self.alive = dict.fromkeys(messages, True)

  def __getitem__(self, name):
    return self.messages[name]


def bus(stamp, *, lead_present=False, lead_distance=30.0):
  names = ('modelV2', 'carState', 'carControl', 'controlsState', 'selfdriveState', 'radarState', 'vehicleParameters')
  events = {name: messaging.new_message(name) for name in names}
  events['modelV2'].modelV2.orientationRate.z = [0.25] * 33
  events['modelV2'].modelV2.velocity.x = [25.0] * 33
  events['modelV2'].modelV2.position.x = [float(i * 5) for i in range(33)]
  events['modelV2'].modelV2.position.y = [0.0] * 33
  events['carState'].carState.vCruise = 108.0
  events['carState'].carState.vEgo = 25.0
  events['carState'].carState.canValid = True
  events['carControl'].carControl.longActive = True
  events['carControl'].carControl.orientationNED = [0.0, 0.0, 0.0]
  events['selfdriveState'].selfdriveState.enabled = True
  events['controlsState'].controlsState.curvature = 0.01
  events['controlsState'].controlsState.longControlState = LongCtrlState.pid
  events['radarState'].radarState.leadOne.present = lead_present
  events['radarState'].radarState.leadOne.radar = True
  events['radarState'].radarState.leadOne.dRel = lead_distance
  events['radarState'].radarState.leadOne.vLead = 25.0
  events['radarState'].radarState.leadOne.modelProb = 1.0
  return Bus({name: getattr(messaging.log_from_bytes(msg.to_bytes()), name) for name, msg in events.items()}, stamp)


class CurveLoopTests(unittest.TestCase):
  def cruise_message(self, kind, stamp, event_id=1, **fields):
    event = messaging.new_message('slcCruiseEvent')
    event.valid = True
    event.logMonoTime = stamp
    event.slcCruiseEvent = dict(eventId=event_id, observedMonoTime=stamp, kind=kind, button='accel',
                               producerSessionId='card-session', previousMps=30.0, selectedMps=30.0, **fields)
    return messaging.log_from_bytes(event.to_bytes())

  def test_one_ordered_stream_keeps_curve_press_and_slc_speed_change(self):
    slc, curve = deque(maxlen=64), deque(maxlen=64)
    queue_cruise_event(self.cruise_message('curveAccelPress', NOW, 10), slc, curve)
    queue_cruise_event(self.cruise_message('driverChange', NOW, 11), slc, curve)
    self.assertEqual(current_cruise_event(curve, NOW).event_id, 10)
    self.assertEqual(current_cruise_event(slc, NOW).event_id, 11)
    self.assertIsNone(current_cruise_event(curve, NOW))
    self.assertIsNone(current_cruise_event(slc, NOW))

  def test_confirmations_and_ambiguous_or_expired_presses_cannot_reach_curve(self):
    slc, curve = deque(maxlen=64), deque(maxlen=64)
    queue_cruise_event(self.cruise_message('confirmationAccept', NOW), slc, curve)
    self.assertEqual(len(slc), 1)
    self.assertFalse(curve)
    for fields in ({'longPress': True}, {'sessionId': 'slc-session'}, {'decisionId': 1}, {'commandId': 1}, {'presentationId': 1}):
      queue_cruise_event(self.cruise_message('curveAccelPress', NOW, **fields), slc, curve)
      self.assertFalse(curve)
    queue_cruise_event(self.cruise_message('curveAccelPress', NOW), slc, curve)
    self.assertIsNone(current_cruise_event(curve, NOW - 1))
    self.assertEqual(len(curve), 1)
    self.assertIsNone(current_cruise_event(curve, NOW + 150_000_000))
    self.assertFalse(curve)

  def test_serialized_physical_press_releases_only_confirmed_curve_owner(self):
    cp = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
    cp.pcmCruise = False
    host = CurveHost(enabled=True, replay=True)
    planner = LongitudinalPlanner(cp, init_v=25.0)
    for n in range(1, 31):
      stamp = NOW + n * 50_000_000
      sm = bus(stamp)
      update_curve_frame(planner, sm, cp, stamp, host=host)
    self.assertTrue(host.runtime.was_controlling)
    slc, curve = deque(maxlen=64), deque(maxlen=64)
    stamp += 50_000_000
    queue_cruise_event(self.cruise_message('curveAccelPress', stamp), slc, curve)
    sm = bus(stamp)
    result = update_curve_frame(planner, sm, cp, stamp, host=host, event=current_cruise_event(curve, stamp))
    self.assertIsNone(result.ceiling_mps)
    self.assertTrue(host.runtime.override)
    self.assertFalse(planner.last_curve_ceiling_applied)

  def test_exact_model_cap_and_post_composition_winner(self):
    cp = NS(openpilotLongitudinalControl=True, pcmCruise=False)
    host = CurveHost(enabled=True, replay=True)
    planner = LongitudinalPlanner.__new__(LongitudinalPlanner)
    planner.last_curve_ceiling_applied = False
    for n in range(1, 27):
      stamp = NOW + n * 50_000_000
      ceiling, result = curve_for_frame(host, bus(stamp), cp, stamp)
      self.assertIsNotNone(ceiling)
      self.assertIsNotNone(result)
      self.assertEqual(ceiling.model_ns, stamp)
      # SLC/lead can win despite a curve candidate. Feedback remains clear.
      confirm_curve_frame(host, planner, stamp)
      self.assertFalse(host.runtime.was_controlling)
    planner.last_curve_ceiling_applied = True
    stamp += 50_000_000
    self.assertIsNotNone(curve_for_frame(host, bus(stamp), cp, stamp)[0])
    confirm_curve_frame(host, planner, stamp)
    self.assertTrue(host.runtime.was_controlling)

  def test_no_host_has_no_cap(self):
    self.assertEqual(curve_for_frame(None, None, None, NOW), (None, None))

  def test_actual_native_planner_update_provides_feedback(self):
    cp = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
    cp.pcmCruise = False
    host = CurveHost(enabled=True, replay=True)
    planner = LongitudinalPlanner(cp, init_v=25.0)
    applied = []
    states = []
    for n in range(1, 31):
      stamp = NOW + n * 50_000_000
      sm = bus(stamp)
      result = update_curve_frame(planner, sm, cp, stamp, host=host)
      applied.append(planner.last_curve_ceiling_applied)
      states.append((planner.last_curve_ceiling_status, float(planner.a_cruise), float(planner.output_a_target),
                     str(planner.mpc.source), result.ceiling_mps if result else None, result.reason if result else None))
      self.assertEqual(host.runtime.confirmed_ns, stamp)
    self.assertIn(True, applied, states[-5:])

  def test_actual_native_slc_lower_cap_denies_curve_feedback(self):
    cp = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
    cp.pcmCruise = False
    host = CurveHost(enabled=True, replay=True)
    planner = LongitudinalPlanner(cp, init_v=25.0)
    slc = CruiseCeiling(10.0, Authority(Mode.LONGITUDINAL_ONLY, LongitudinalOwner.SYSTEM,
                                       False, True, False, False))
    for n in range(1, 27):
      stamp = NOW + n * 50_000_000
      sm = bus(stamp)
      result = update_curve_frame(planner, sm, cp, stamp, host=host, cruise_ceiling=slc)
      self.assertIsNotNone(result.ceiling_mps)
      self.assertFalse(planner.last_curve_ceiling_applied)
      self.assertFalse(host.runtime.was_controlling)
    self.assertEqual(host.runtime.curve.revision, 0)

  def test_same_cycle_mpc_headway_filters_following_lead(self):
    cp = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
    cp.pcmCruise = False
    host = CurveHost(enabled=True, no_lead=True, replay=True)
    planner = LongitudinalPlanner(cp, init_v=25.0)
    allowed = []
    selected_headways = []
    for n in range(1, 65):
      stamp = NOW + n * 50_000_000
      # Current radar lead enters then leaves the filtered tracking/following
      # window. A single raw absence cannot immediately authorize NoLead.
      sm = bus(stamp, lead_present=16 <= n <= 42)
      output = []
      def provider(follow_time_s, *, _sm=sm, _stamp=stamp, _output=output):
        selected_headways.append(follow_time_s)
        ceiling, result = curve_for_frame(host, _sm, cp, _stamp, follow_time_s=follow_time_s)
        _output.append((ceiling, result))
        return ceiling
      planner.update(sm, curve_provider=provider)
      self.assertEqual(len(output), 1)
      self.assertAlmostEqual(selected_headways[-1], float(planner.mpc.params[0, 4]))
      confirm_curve_frame(host, planner, stamp)
      allowed.append(output[0][0] is not None)
    self.assertTrue(all(allowed[:10]))
    self.assertFalse(any(allowed[30:42]))
    self.assertTrue(any(allowed[-10:]))
    with self.assertRaises(ValueError):
      planner.update(bus(NOW + 65 * 50_000_000), curve_ceiling=CurveCeiling(20.0, NOW), curve_provider=lambda _headway: None)

  def test_selected_profile_headway_changes_following_on_same_cycle(self):
    cp = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
    cp.pcmCruise = False
    stock_host = CurveHost(enabled=True, no_lead=True, replay=True)
    tuned_host = CurveHost(enabled=True, no_lead=True, replay=True)
    stock = LongitudinalPlanner(cp, init_v=25.0)
    tuned = LongitudinalPlanner(cp, init_v=25.0)
    profile = ProfileTuning('aggressive', 2.5, 1.0, 1.0, 1.0, 1.0, 1.0)
    stock_caps, tuned_caps, headways = [], [], []
    for n in range(1, 32):
      stamp = NOW + n * 50_000_000
      sm = bus(stamp, lead_present=True, lead_distance=80.0)
      def stock_provider(headway, *, _sm=sm, _stamp=stamp):
        ceiling, _ = curve_for_frame(stock_host, _sm, cp, _stamp, follow_time_s=headway)
        stock_caps.append(ceiling)
        return ceiling
      def tuned_provider(headway, *, _sm=sm, _stamp=stamp):
        headways.append(headway)
        ceiling, _ = curve_for_frame(tuned_host, _sm, cp, _stamp, follow_time_s=headway)
        tuned_caps.append(ceiling)
        return ceiling
      stock.update(sm, curve_provider=stock_provider)
      tuned.update(sm, profile_tuning=profile, curve_provider=tuned_provider)
      confirm_curve_frame(stock_host, stock, stamp)
      confirm_curve_frame(tuned_host, tuned, stamp)
    self.assertAlmostEqual(float(stock.mpc.params[0, 4]), 1.25)
    self.assertGreater(headways[-1], 1.6)
    self.assertAlmostEqual(headways[-1], float(tuned.mpc.params[0, 4]))
    self.assertIsNotNone(stock_caps[-1])
    self.assertIsNone(tuned_caps[-1])


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