mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-28 09:03:46 +08:00
firestar5683
181 lines
8.8 KiB
Python
181 lines
8.8 KiB
Python
import tempfile
|
|
import unittest
|
|
from types import SimpleNamespace
|
|
from unittest.mock import patch
|
|
|
|
from openpilot.starpilot.controller_extensions import ManualTurnInputs, configure_controller
|
|
from openpilot.starpilot.tests import test_vehicle_preferences as preferences_tests
|
|
from opendbc.car.ford.values import CAR
|
|
|
|
|
|
class TestControllerExtensions(unittest.TestCase):
|
|
def test_real_saved_preference_default_and_disabled(self):
|
|
from openpilot.common.params import Params
|
|
with tempfile.TemporaryDirectory() as directory:
|
|
params = Params(directory)
|
|
self.assertTrue(params.get('FordHumanTurnDetection', return_default=True))
|
|
params.put_bool('FordHumanTurnDetection', False, block=True)
|
|
self.assertFalse(params.get('FordHumanTurnDetection', return_default=True))
|
|
|
|
def test_actual_model_owner_and_cached_preference(self):
|
|
import openpilot.cereal.messaging as messaging
|
|
from openpilot.common.params import Params
|
|
message = messaging.new_message('modelV2')
|
|
message.modelV2.meta.laneChangeState = 'laneChangeStarting'
|
|
sm = SimpleNamespace(alive={'modelV2': True}, valid={'modelV2': True}, update=lambda timeout: None)
|
|
|
|
class ModelReader:
|
|
alive = sm.alive
|
|
valid = sm.valid
|
|
|
|
def update(self, timeout):
|
|
sm.update(timeout)
|
|
|
|
def __getitem__(self, key):
|
|
return message.modelV2
|
|
with patch.object(messaging, 'SubMaster', return_value=ModelReader()), patch.object(Params, 'get', return_value=True) as read:
|
|
inputs = ManualTurnInputs(Params())
|
|
self.assertEqual(inputs.update(), (True, True, True))
|
|
for _ in range(99):
|
|
inputs.update()
|
|
self.assertEqual(read.call_count, 8)
|
|
inputs.update()
|
|
self.assertEqual(read.call_count, 12)
|
|
sm.alive['modelV2'] = False
|
|
self.assertEqual(inputs.update(), (True, True, False))
|
|
|
|
def test_model_preview_freshness_delay_and_invalid_output(self):
|
|
import openpilot.cereal.messaging as messaging
|
|
from openpilot.selfdrive.modeld.constants import ModelConstants
|
|
now = 10_000_000_000
|
|
model = messaging.new_message('modelV2')
|
|
model.modelV2.orientationRate.z = [20. * (value / 100.) for value in ModelConstants.T_IDXS]
|
|
delay = messaging.new_message('lateralDelay')
|
|
delay.lateralDelay.lateralDelay = .3
|
|
class Reader:
|
|
alive = {'modelV2': True, 'lateralDelay': True}
|
|
valid = dict(alive)
|
|
logMonoTime = {'modelV2': now, 'lateralDelay': now}
|
|
def __getitem__(self, key):
|
|
return model.modelV2 if key == 'modelV2' else delay.lateralDelay
|
|
inputs = ManualTurnInputs.__new__(ManualTurnInputs)
|
|
vars(inputs)['sm'] = Reader()
|
|
with patch('openpilot.starpilot.controller_extensions.time.monotonic_ns', return_value=now):
|
|
curvature = inputs.preview_curvature(20.)
|
|
assert curvature is not None
|
|
self.assertAlmostEqual(curvature, .003)
|
|
inputs.sm.logMonoTime['lateralDelay'] = now - 1_000_000_001
|
|
curvature = inputs.preview_curvature(20.)
|
|
assert curvature is not None
|
|
self.assertAlmostEqual(curvature, .002)
|
|
for stamp in (0, now + 1, now - 100_000_001):
|
|
inputs.sm.logMonoTime['modelV2'] = stamp
|
|
self.assertIsNone(inputs.preview_curvature(20.))
|
|
inputs.sm.logMonoTime['modelV2'] = now
|
|
self.assertIsNone(inputs.preview_curvature(0.))
|
|
model.modelV2.orientationRate.z = [float('nan')] * 33
|
|
self.assertIsNone(inputs.preview_curvature(20.))
|
|
|
|
def test_assist_permission_tracks_panda_health_without_affecting_preview(self):
|
|
import openpilot.cereal.messaging as messaging
|
|
from openpilot.common.params import Params
|
|
|
|
class Reader(dict):
|
|
def __init__(self):
|
|
super().__init__(pandaStates=[SimpleNamespace(controlsAllowed=True)])
|
|
self.alive = self.valid = self.freq_ok = True
|
|
|
|
def all_checks(self, services):
|
|
self.asserted_services = services
|
|
return self.alive and self.valid and self.freq_ok
|
|
|
|
reader = Reader()
|
|
with patch.object(messaging, 'SubMaster', return_value=reader) as factory, patch.object(Params, 'get', return_value=True):
|
|
inputs = ManualTurnInputs(Params())
|
|
self.assertNotIn('pandaStates', factory.call_args.args[0])
|
|
self.assertEqual(inputs.assist_permission(), ((), False))
|
|
inputs = ManualTurnInputs(Params(), track_assist_permission=True)
|
|
self.assertIn('pandaStates', factory.call_args.args[0])
|
|
self.assertEqual(inputs.assist_permission(), (reader['pandaStates'], True))
|
|
for health in ('alive', 'valid', 'freq_ok'):
|
|
setattr(reader, health, False)
|
|
self.assertEqual(inputs.assist_permission(), (reader['pandaStates'], False))
|
|
setattr(reader, health, True)
|
|
self.assertEqual(reader.asserted_services, ['pandaStates'])
|
|
|
|
def test_real_card_finalizes_then_binds_only_admitted_controller(self):
|
|
harness = preferences_tests.TestVehicleStartupPreferences(methodName='test_only_exact_saved_opt_in_is_loaded')
|
|
harness.setUp()
|
|
self.addCleanup(harness.doCleanups)
|
|
with patch('openpilot.starpilot.controller_extensions.messaging.SubMaster', return_value=SimpleNamespace()):
|
|
host, _, published = harness.start(CAR.FORD_MUSTANG_MACH_E_MK1)
|
|
self.assertFalse(published.passive)
|
|
self.assertIsInstance(host.CI.CC.manual_turn_inputs, ManualTurnInputs)
|
|
self.assertIs(host.CI.CC.manual_turn_inputs.params, harness.params)
|
|
self.assertTrue(host.CI.CC.manual_turn_inputs.track_assist_permission)
|
|
host, _, published = harness.start(CAR.FORD_MUSTANG_MACH_E_MK1, enabled=False)
|
|
self.assertTrue(published.passive)
|
|
self.assertIsNone(host.CI.CC.manual_turn_inputs)
|
|
|
|
def test_standalone_controller_has_no_host_owner(self):
|
|
from opendbc.car.ford.tests.test_three_ports import params
|
|
from opendbc.car.ford.carcontroller import CarController
|
|
from opendbc.car.ford.values import DBC
|
|
from opendbc.car.ford.carstate import CarState
|
|
from opendbc.car import structs
|
|
cp = params(CAR.FORD_MUSTANG_MACH_E_MK1)
|
|
controller = CarController(DBC[cp.carFingerprint], cp)
|
|
state = CarState(cp)
|
|
state.update(state.get_can_parsers(cp))
|
|
state.out = structs.CarState().as_reader()
|
|
controller.update(structs.CarControl().as_reader(), state, 0)
|
|
self.assertIsNone(controller.manual_turn_inputs)
|
|
cp.passive = True
|
|
configure_controller(SimpleNamespace(CP=cp, CC=controller), SimpleNamespace())
|
|
self.assertIsNone(controller.manual_turn_inputs)
|
|
|
|
|
|
class TestAngleSnapshotParity(unittest.TestCase):
|
|
def test_angle_delay_reaches_pinned_bp7_without_changing_curvature_snapshot(self):
|
|
import openpilot.cereal.messaging as messaging
|
|
from openpilot.selfdrive.modeld.constants import ModelConstants
|
|
from opendbc.bluepilot_lateral.core.angle import AngleController, Inputs
|
|
from opendbc.bluepilot_lateral.tests import reference
|
|
from dataclasses import replace
|
|
|
|
now = 10_000_000_000
|
|
model = messaging.new_message('modelV2')
|
|
model.modelV2.orientationRate.z = [5. * (.001 + .004 * value) for value in ModelConstants.T_IDXS]
|
|
delay = messaging.new_message('lateralDelay')
|
|
class Reader:
|
|
alive = {'modelV2': True, 'lateralDelay': True}
|
|
valid = dict(alive)
|
|
logMonoTime = {'modelV2': now, 'lateralDelay': now}
|
|
def __getitem__(self, key):
|
|
return model.modelV2 if key == 'modelV2' else delay.lateralDelay
|
|
provider = ManualTurnInputs.__new__(ManualTurnInputs)
|
|
vars(provider)['sm'] = Reader()
|
|
provider.enabled = True
|
|
with patch('openpilot.starpilot.controller_extensions.time.monotonic_ns', return_value=now):
|
|
for value in (.05, .10, .125, .15, .30):
|
|
delay.lateralDelay.lateralDelay = value
|
|
snapshot = provider.lateral_snapshot(5., angle=True)
|
|
self.assertIsNotNone(snapshot)
|
|
self.assertAlmostEqual(snapshot[2], min(.15, max(.10, value)))
|
|
self.assertAlmostEqual(provider.lateral_snapshot(5.)[2], min(.4, max(.2, value)))
|
|
adapted, upstream = AngleController(low_gain=1., high_gain=1.05), reference.Angle(CAR.FORD_MUSTANG_MACH_E_MK1)
|
|
for _ in range(20):
|
|
common = Inputs(active=True, speed=5., measured_curvature=.001, desired_curvature=.001,
|
|
yaw_predictions=tuple(snapshot[0].orientationRate.z), time_indices=tuple(snapshot[1]), lateral_delay=snapshot[2])
|
|
command = adapted.step(common)
|
|
expected = upstream.step(replace(common, lateral_delay=delay.lateralDelay.lateralDelay))
|
|
self.assertTrue(command.active)
|
|
self.assertAlmostEqual(command.path_angle, expected.path_angle, places=12)
|
|
provider.sm.logMonoTime['lateralDelay'] = now - 1_000_000_001
|
|
self.assertAlmostEqual(provider.lateral_snapshot(5., angle=True)[2], .15)
|
|
self.assertAlmostEqual(provider.lateral_snapshot(5.)[2], .2)
|
|
for stamp in (0, now + 1, now - 100_000_001):
|
|
provider.sm.logMonoTime['modelV2'] = stamp
|
|
self.assertIsNone(provider.lateral_snapshot(5., angle=True))
|
|
self.assertIsNone(provider.lateral_snapshot(5.))
|