StarPilot/openpilot/starpilot/lateral/tests/test_gm_geometry_runtime.py

"""Canonical geometry and original ForceAuto precedence at actual producer/consumer seams."""
from dataclasses import replace
import json
from pathlib import Path
import unittest

from opendbc.car.gm.values import CAR
from openpilot.common.params import Params
from openpilot.common.prefix import OpenpilotPrefix
from openpilot.selfdrive.controls.controlsd import Controls
from openpilot.selfdrive.locationd.paramsd import VehicleParamsLearner
from openpilot.selfdrive.locationd.lagd import LateralLagEstimator
from openpilot.selfdrive.locationd.torqued import TorqueEstimator
from openpilot.starpilot.lateral.controller_selection import ControllerMode, learning_allowed, replace_mode
from openpilot.starpilot.lateral.gm_geometry_runtime import GeometryPublicationOwner, geometry_basis, read_geometry
from openpilot.starpilot.lateral.tests.test_gm_manual_torque import finalized_cases, stored
from openpilot.starpilot.lateral.tests.test_torque_runtime import LearnedFrame
from openpilot.starpilot.lateral.tests.test_volt_torque_policy import params as volt_params
from openpilot.starpilot.lateral.torque_runtime import TorqueHost, read_settings, runtime_enabled
from openpilot.starpilot.lateral.torque_settings import DOCUMENT_KEY, FieldChoice, parse_document, replace_field, replace_geometry, serialize_document
from openpilot.starpilot.lateral.torque_tuning import TorqueSource
from openpilot.starpilot.ui.feature_settings_owner import FeatureSettingsOwner
from openpilot.starpilot.ui.feature_settings_state import FeatureSettingsRequest


def profile(cp, learning='source'):
  tune = cp.lateralTuning.torque
  basis = (tune.latAccelFactor, tune.latAccelOffset, tune.friction)
  return replace_geometry({}, str(cp.carFingerprint), basis, geometry_basis(cp), 'learning', learning)


def edit(cp, profiles, field, value):
  tune = cp.lateralTuning.torque
  return replace_geometry(profiles, str(cp.carFingerprint), (tune.latAccelFactor, tune.latAccelOffset, tune.friction),
                          geometry_basis(cp), field, value)


class TestGmGeometryRuntime(unittest.TestCase):
  def test_document_backward_compatibility_exact_basis_and_rejection(self):
    cp = volt_params(CAR.CHEVROLET_VOLT_CAMERA)
    profiles = profile(cp)
    identity = str(cp.carFingerprint)
    raw = serialize_document(profiles)
    self.assertEqual(parse_document(raw), profiles)
    self.assertEqual(json.loads(raw)['schemaVersion'], 3)
    old = {identity: replace(profiles[identity], geometry=None)}
    self.assertEqual(json.loads(serialize_document(old))['schemaVersion'], 1)
    self.assertEqual(parse_document(serialize_document(old)), old)
    for field, value in [('ratio', FieldChoice('custom', cp.steerRatio*1.51)),
                         ('ratio', FieldChoice('custom', float('nan'))),
                         ('full_delay', FieldChoice('custom', 1.01)), ('automatic_delay', 1), ('learning', 'unknown')]:
      with self.assertRaises(ValueError):
        edit(cp, profiles, field, value)
    with OpenpilotPrefix():
      saved = Params()
      stored(saved, profiles)
      self.assertIsNotNone(read_geometry(saved, cp).profile)
      bad = cp.as_reader().as_builder()
      bad.steerActuatorDelay += .01
      self.assertIsNone(read_geometry(saved, bad).profile)
      bad = cp.as_reader().as_builder()
      bad.safetyConfigs[0].safetyParam = 0xffff
      self.assertIsNone(read_geometry(saved, bad).profile)
      for raw in (b'{', json.dumps({'schemaVersion':3,'vehicles':{}}).encode()):
        Path(saved.get_param_path(DOCUMENT_KEY)).write_bytes(raw)
        self.assertIsNone(read_geometry(saved, cp).profile)
        self.assertEqual(Path(saved.get_param_path(DOCUMENT_KEY)).read_bytes(), raw)

  def test_force_auto_admits_default_custom_and_standard_only_on_exact_intent(self):
    for policy, cp in finalized_cases():
      with OpenpilotPrefix():
        saved = Params()
        saved.put('CarParams', cp.to_bytes(), block=True)
        default = Controls()
        self.assertFalse(default.torque_learning_allowed, (policy, 'default unchanged'))
        self.assertIsNone(default.torque_host)
        profiles = profile(cp, 'force_auto')
        stored(saved, profiles)
        for mode in ControllerMode:
          saved.put('LateralControllerSelection', json.loads(replace_mode(None,cp,mode)), block=True)
          self.assertTrue(learning_allowed(saved, cp), (policy, mode))
          controls = Controls()
          self.assertEqual(controls.lateral_controller_selection.mode, mode)
          self.assertTrue(controls.torque_learning_allowed)
          self.assertIsNotNone(controls.torque_host)
          estimator = TorqueEstimator(cp.as_reader(), allow_learning=learning_allowed(saved, cp))
          self.assertTrue(estimator.learning_allowed)
          self.assertFalse(estimator.use_params, 'brand eligibility semantics remain unchanged')
          self.assertFalse(estimator.get_msg().lateralTorqueParameters.valid, 'empty fit is never fabricated valid')
        saved.put_bool('ForceAutoTuneOff', True, block=True)
        self.assertFalse(learning_allowed(saved, cp))
        self.assertFalse(read_geometry(saved, cp).force_auto)
        self.assertFalse(runtime_enabled(cp, saved))
        saved.put_bool('ForceAutoTuneOff', False, block=True)
        stored(saved, edit(cp, profiles, 'learning', 'force_off'))
        self.assertFalse(learning_allowed(saved, cp))

  def test_force_auto_validity_and_original_custom_suppression_resume(self):
    cp = volt_params(CAR.CHEVROLET_VOLT_CAMERA)
    with OpenpilotPrefix():
      saved = Params()
      host = TorqueHost(saved, cp, allow_learning=True)
      vehicle = host.vehicle
      profiles = replace_field(profile(cp,'force_auto'), str(cp.carFingerprint),
                               (vehicle.lat_accel_factor,vehicle.lat_accel_offset,vehicle.friction), 'friction','custom',vehicle.friction*1.2)
      stored(saved, profiles)
      now = 1_000_000_000
      frame = LearnedFrame(now, factor=vehicle.lat_accel_factor*1.1, offset=.02, friction=vehicle.friction*1.1)
      frame.state.useParams = False
      host.settings = read_settings(saved, vehicle, allow_learning=True, CP=cp)
      self.assertTrue(host.settings.force_auto)
      self.assertIsNone(host.settings.user_friction)
      self.assertEqual(host._select(frame, now).source, TorqueSource.LEARNED)
      for attribute, value in [('valid',False),('version',2),('latAccelFactorFiltered',float('nan')),
                               ('latAccelFactorFiltered',vehicle.lat_accel_factor*2),('latAccelOffsetFiltered',1.),
                               ('frictionCoefficientFiltered',vehicle.friction*2)]:
        good = getattr(frame.state, attribute)
        setattr(frame.state, attribute, value)
        self.assertIsNone(host._learned(frame, now), attribute)
        setattr(frame.state, attribute, good)
      frame.checked = False
      self.assertIsNone(host._learned(frame, now))
      frame.checked = True
      for stamp in (0, now+1, now-3_000_000_000):
        frame.logMonoTime['lateralTorqueParameters'] = stamp
        self.assertIsNone(host._learned(frame, now))
      frame.logMonoTime['lateralTorqueParameters'] = now
      source = edit(cp, profiles, 'learning', 'source')
      stored(saved, source)
      host.settings = read_settings(saved, vehicle, allow_learning=True, CP=cp)
      self.assertFalse(host.settings.force_auto)
      self.assertEqual(host._select(frame, now).source, TorqueSource.USER)
      self.assertAlmostEqual(host._select(frame, now).friction, vehicle.friction*1.2)
      self.assertEqual(parse_document(Path(saved.get_param_path(DOCUMENT_KEY)).read_bytes())[str(cp.carFingerprint)].friction,
                       profiles[str(cp.carFingerprint)].friction)
      stored(saved, edit(cp, profiles, 'learning', 'force_off'))
      host.settings = read_settings(saved, vehicle, allow_learning=True, CP=cp)
      self.assertTrue(host.settings.force_auto_off)
      self.assertEqual(host._select(frame, now).source, TorqueSource.USER)

  def test_actual_paramsd_and_lagd_project_copy_preserve_estimator_cache_and_validity(self):
    cp = volt_params(CAR.CHEVROLET_VOLT_CAMERA)
    with OpenpilotPrefix():
      saved = Params()
      ratio = cp.steerRatio*1.1
      profiles = edit(cp, profile(cp), 'ratio', FieldChoice('custom',ratio))
      profiles = edit(cp, profiles, 'full_delay', FieldChoice('custom',.7))
      profiles = edit(cp, profiles, 'automatic_delay', False)
      stored(saved, profiles)
      owner = GeometryPublicationOwner(saved, cp)
      learner = VehicleParamsLearner(cp.as_reader(), cp.steerRatio, .8, 0.)
      original = learner.get_msg(False, debug=True)
      lag = LateralLagEstimator(cp.as_reader(), .05)
      delay = lag.get_msg(False, debug=True)
      raw, lag_raw = original.to_bytes(), delay.to_bytes()
      state = learner.kf.x.copy()
      p = learner.kf.P.copy()
      projected = owner.parameters(original)
      delayed = owner.delay(delay)
      self.assertAlmostEqual(projected.vehicleParameters.steerRatio,ratio,delta=1e-5)
      self.assertEqual(projected.vehicleParameters.stiffnessFactor, original.vehicleParameters.stiffnessFactor)
      self.assertAlmostEqual(delayed.lateralDelay.lateralDelay,.7)
      self.assertFalse(projected.valid or delayed.valid)
      expected = original.to_dict()
      expected['vehicleParameters']['steerRatio'] = projected.vehicleParameters.steerRatio
      self.assertEqual(projected.to_dict(),expected)
      expected_delay = delay.to_dict()
      expected_delay['lateralDelay']['lateralDelay'] = delayed.lateralDelay.lateralDelay
      self.assertEqual(delayed.to_dict(),expected_delay)
      self.assertEqual(original.as_reader().as_builder().to_bytes(), raw)
      self.assertEqual(delay.as_reader().as_builder().to_bytes(), lag_raw)
      self.assertTrue((learner.kf.x==state).all() and (learner.kf.P==p).all())
      self.assertAlmostEqual(lag.initial_lag, cp.steerActuatorDelay+.2)
      stored(saved, edit(cp,profiles,'learning','force_auto'))
      owner.last_read_ns = None
      self.assertIs(owner.parameters(original),original,'ForceAuto suppresses custom ratio')
      self.assertAlmostEqual(owner.delay(delay).lateralDelay.lateralDelay,.7,msg='full delay independent')
      stored(saved, edit(cp,profiles,'learning','force_off'))
      owner.last_read_ns = None
      off = owner.parameters(original)
      self.assertAlmostEqual(off.vehicleParameters.steerRatio,ratio,delta=1e-5)
      self.assertEqual(off.vehicleParameters.stiffnessFactor,1.)
      reset = edit(cp,profiles,'ratio',FieldChoice('source',ratio))
      reset = edit(cp,reset,'full_delay',FieldChoice('source',.7))
      stored(saved, reset)
      owner.last_read_ns = None
      self.assertIs(owner.parameters(original),original)
      self.assertAlmostEqual(owner.delay(delay).lateralDelay.lateralDelay,cp.steerActuatorDelay+.2)
      self.assertEqual(original.as_reader().as_builder().to_bytes(),raw)
      self.assertEqual(delay.as_reader().as_builder().to_bytes(),lag_raw)

  def test_ui_guarded_geometry_choices_and_source_staleness(self):
    cp = volt_params(CAR.CHEVROLET_VOLT_CAMERA)
    with OpenpilotPrefix():
      saved=Params()
      owner=FeatureSettingsOwner(saved,lambda _:True,vehicle_fingerprint=lambda:cp.carFingerprint,vehicle_params=lambda:cp)
      def rows():
        return {row.key:row for row in owner.snapshot('torque',parked=True,system_long=True,lateral_context=True,metric=False).rows}
      row=rows()['torque:learning:value']
      request=FeatureSettingsRequest(row.key,row.source,'Force auto',vehicle_fingerprint=cp.carFingerprint,
                                    capability=row.capability,dependencies=row.dependencies,related_source=row.related_source)
      self.assertTrue(owner.apply(request))
      self.assertTrue(read_geometry(saved,cp).force_auto)
      ratio=rows()['torque:ratio:value']
      self.assertIn('temporarily ignored',ratio.reason)
      self.assertEqual(Path(saved.get_param_path('LateralControllerSelection')).exists(),False)
      request=FeatureSettingsRequest(ratio.key,ratio.source,str(cp.steerRatio*1.1),vehicle_fingerprint=cp.carFingerprint,
                                    capability=ratio.capability,dependencies=ratio.dependencies,related_source=ratio.related_source)
      changed=cp.as_reader().as_builder()
      changed.steerRatio+=.1
      owner.vehicle_params=lambda:changed
      self.assertFalse(owner.apply(request))

  def test_ui_geometry_custom_source_roundtrip_and_narrow_stale_basis_reset(self):
    from openpilot.cereal import messaging
    from openpilot.starpilot.lateral.gain_runtime import gain_basis
    from openpilot.starpilot.lateral.torque_settings import replace_gain
    from openpilot.starpilot.ui.feature_settings_state import row_default

    cp = volt_params(CAR.CHEVROLET_VOLT_CAMERA)
    identity = str(cp.carFingerprint)
    tune = cp.lateralTuning.torque
    torque_basis = (tune.latAccelFactor, tune.latAccelOffset, tune.friction)
    with OpenpilotPrefix():
      saved = Params()
      saved.put('CarParams', cp.to_bytes(), block=True)
      controller = Controls()
      profiles = replace_field({}, identity, torque_basis, 'factor', 'custom', torque_basis[0] * 1.1)
      profiles = replace_field(profiles, identity, torque_basis, 'friction', 'custom', torque_basis[2] * 1.1)
      profiles = replace_gain(profiles, identity, gain_basis(cp, controller.LaC), 'custom', .7)
      stored(saved, profiles)
      original_manual = profiles[identity]
      owner = FeatureSettingsOwner(saved, lambda _: True, vehicle_fingerprint=lambda: cp.carFingerprint,
                                   vehicle_params=lambda: cp)

      def rows():
        return {row.key: row for row in owner.snapshot('torque', parked=True, system_long=True,
                                                       lateral_context=True, metric=False).rows}

      def apply_value(key, value):
        row = rows()[key]
        request = FeatureSettingsRequest(row.key, row.source, value, vehicle_fingerprint=cp.carFingerprint,
                                         capability=row.capability, dependencies=row.dependencies,
                                         related_source=row.related_source)
        self.assertTrue(owner.apply(request), key)

      ratio = cp.steerRatio * 1.1
      apply_value('torque:ratio:value', str(ratio))
      apply_value('torque:full_delay:value', '.7')
      stored_profile = parse_document(Path(saved.get_param_path(DOCUMENT_KEY)).read_bytes())[identity]
      self.assertEqual(stored_profile.geometry.ratio, FieldChoice('custom', ratio))
      self.assertEqual(stored_profile.geometry.full_delay, FieldChoice('custom', .7))
      self.assertFalse(stored_profile.geometry.automatic_delay)
      parameters = messaging.new_message('vehicleParameters', valid=True)
      parameters.vehicleParameters.steerRatio = cp.steerRatio
      delay = messaging.new_message('lateralDelay', valid=True)
      delay.lateralDelay.lateralDelay = .6
      publisher = GeometryPublicationOwner(saved, cp)
      self.assertAlmostEqual(publisher.parameters(parameters).vehicleParameters.steerRatio, ratio, delta=1e-5)
      self.assertAlmostEqual(publisher.delay(delay).lateralDelay.lateralDelay, .7, delta=1e-5)
      for field in ('ratio', 'full_delay'):
        row = rows()[f'torque:{field}:value']
        self.assertEqual(row.default_key, f'torque:{field}:mode')
        request = row_default(row)
        self.assertIsNotNone(request, field)
        assert request is not None
        self.assertEqual(request.key, row.default_key)
        self.assertTrue(owner.apply(request), field)
      publisher.last_read_ns = None
      self.assertIs(publisher.parameters(parameters), parameters, 'source ratio withdraws custom projection')
      self.assertAlmostEqual(publisher.delay(delay).lateralDelay.lateralDelay, cp.steerActuatorDelay + .2, delta=1e-5)
      source = parse_document(Path(saved.get_param_path(DOCUMENT_KEY)).read_bytes())[identity]
      self.assertEqual(source.geometry.ratio, FieldChoice('source', ratio))
      self.assertEqual(source.geometry.full_delay, FieldChoice('source', .7))
      self.assertFalse(source.geometry.automatic_delay)
      apply_value('torque:ratio:value', str(ratio))
      apply_value('torque:full_delay:value', '.7')
      before = parse_document(Path(saved.get_param_path(DOCUMENT_KEY)).read_bytes())
      changed = cp.as_reader().as_builder()
      changed.steerActuatorDelay += .01
      owner.vehicle_params = lambda: changed
      self.assertIsNone(read_geometry(saved, changed).profile, 'changed basis pauses saved geometry')
      stale_rows = rows()
      self.assertNotIn('torque:ratio:value', stale_rows)
      reset = stale_rows['torque:geometry_reset:value']
      self.assertTrue(owner.apply(FeatureSettingsRequest(reset.key, reset.source, 'Reset',
                         vehicle_fingerprint=cp.carFingerprint, capability=reset.capability,
                         dependencies=reset.dependencies, related_source=reset.related_source)))
      after = parse_document(Path(saved.get_param_path(DOCUMENT_KEY)).read_bytes())
      self.assertEqual(after, {**before, identity: replace(before[identity], geometry=None)})
      self.assertEqual(after[identity], original_manual, 'narrow reset retains factor, friction and KP basis/value')
      self.assertFalse(Path(saved.get_param_path('LateralControllerSelection')).exists())
      self.assertFalse(Path(saved.get_param_path('AdvancedLateralTune')).exists())

  def test_absent_geometry_preserves_ioniq_and_five_bolt_consumers(self):
    from opendbc.car.car_helpers import interfaces
    from opendbc.car.hyundai.values import CAR as HYUNDAI
    from opendbc.car.gm.tests.test_bolt_cc import params as bolt_params
    from openpilot.starpilot.lateral.torque_supported import BOLT_VEHICLES
    from openpilot.cereal import messaging
    cps = [interfaces[HYUNDAI.HYUNDAI_IONIQ_6].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_6)]
    cps += [bolt_params(identity,alpha=True) for identity in sorted(BOLT_VEHICLES)]
    for cp in cps:
      with OpenpilotPrefix():
        saved = Params()
        tune = cp.lateralTuning.torque
        document = replace_field({},str(cp.carFingerprint),(tune.latAccelFactor,tune.latAccelOffset,tune.friction),
                                 'friction','custom',tune.friction*1.1)
        raw = serialize_document(document)
        stored(saved,document)
        saved_raw=Path(saved.get_param_path(DOCUMENT_KEY)).read_bytes()
        saved.put_bool('ForceAutoTuneOff',True,block=True)
        owner = GeometryPublicationOwner(saved,cp)
        parameters=messaging.new_message('vehicleParameters',valid=False)
        parameters.vehicleParameters.steerRatio=cp.steerRatio*1.01
        parameters.vehicleParameters.stiffnessFactor=.8
        delay=messaging.new_message('lateralDelay',valid=False)
        delay.lateralDelay.lateralDelay=.6
        self.assertIs(owner.parameters(parameters),parameters)
        self.assertIs(owner.delay(delay),delay)
        self.assertFalse(read_geometry(saved,cp).force_auto)
        self.assertEqual(serialize_document(parse_document(raw)),raw)
        self.assertEqual(Path(saved.get_param_path(DOCUMENT_KEY)).read_bytes(),saved_raw)