mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 13:33:42 +08:00
firestar5683
168 lines
7.8 KiB
Python
168 lines
7.8 KiB
Python
import pytest
|
|
|
|
from opendbc.car.car_helpers import interfaces
|
|
from opendbc.car.structs import CarParams
|
|
from openpilot.starpilot.conditional_mode.stop_release import owner_for
|
|
|
|
|
|
BRANDS = sorted({interface.__module__.split('.')[-2] for interface in interfaces.values()})
|
|
|
|
|
|
@pytest.mark.parametrize('brand', BRANDS)
|
|
def test_cem_departure_owner_is_shared_across_active_longitudinal_configs(brand):
|
|
for identity, interface in interfaces.items():
|
|
if interface.__module__.split('.')[-2] != brand:
|
|
continue
|
|
cp = interface.get_non_essential_params(identity)
|
|
cp.openpilotLongitudinalControl = True
|
|
assert (owner_for(cp) is not None) == (not (cp.passive or cp.dashcamOnly or cp.notCar)), identity
|
|
|
|
|
|
@pytest.mark.parametrize('flag', ['passive', 'dashcamOnly', 'notCar'])
|
|
def test_inactive_profiles_never_get_departure_owner(flag):
|
|
cp = CarParams(openpilotLongitudinalControl=True)
|
|
setattr(cp, flag, True)
|
|
assert owner_for(cp) is None
|
|
|
|
|
|
def test_stock_longitudinal_never_gets_departure_owner():
|
|
assert owner_for(CarParams(openpilotLongitudinalControl=False)) is None
|
|
|
|
|
|
from types import SimpleNamespace
|
|
from functools import lru_cache
|
|
from openpilot.cereal import messaging
|
|
from openpilot.selfdrive.controls.radard import get_RadarState_from_vision
|
|
from openpilot.starpilot.conditional_mode.stop_release import StoppedLeadRelease
|
|
from openpilot.starpilot.longitudinal.tests import test_cem_green_release as green
|
|
from openpilot.starpilot.tests.test_ford_recorded_departure import _motion_payloads, _samples
|
|
|
|
|
|
@lru_cache(maxsize=1)
|
|
def recorded_motion():
|
|
return next(sample for sample in _samples() if 334.44 < sample['routeSeconds'] < 334.48)
|
|
|
|
|
|
def scene(now, vision=False):
|
|
motion = recorded_motion()
|
|
payloads = _motion_payloads(green.serialized_scene(stamp=green.BOOT + now - green.MONO - 2_000_000,
|
|
speed=0., standstill=True), motion)
|
|
for service in ('carControl', 'controlsState'):
|
|
event = messaging.new_message(service)
|
|
setattr(event, service, payloads[service])
|
|
payloads[service] = getattr(event, service)
|
|
if vision:
|
|
model = payloads['modelV2']
|
|
for slot, lead in enumerate((payloads['radarState'].leadOne, payloads['radarState'].leadTwo)):
|
|
if lead.present:
|
|
raw = model.leadsV3[slot]
|
|
lead.from_dict(get_RadarState_from_vision(raw, payloads['carState'].vEgo, model.velocity.x[0], raw.prob))
|
|
return green.CallerFrame(payloads, now - 2_000_000)
|
|
|
|
|
|
@pytest.mark.parametrize('brand', BRANDS)
|
|
@pytest.mark.parametrize('vision', [False, True])
|
|
def test_shared_release_preserves_native_target_with_radar_or_correlated_vision(brand, vision):
|
|
cp = next(interface.get_non_essential_params(identity) for identity, interface in interfaces.items()
|
|
if interface.__module__.split('.')[-2] == brand)
|
|
cp.openpilotLongitudinalControl = True
|
|
owner = owner_for(cp)
|
|
if cp.passive or cp.dashcamOnly or cp.notCar:
|
|
assert owner is None
|
|
return
|
|
assert owner is not None
|
|
detector = SimpleNamespace(committed=True, standstill_committed=True, standstill_reason='light', commit_distance_m=3.)
|
|
planner = SimpleNamespace(mpc=SimpleNamespace(solution_status=0), output_should_stop=False, output_a_target=.5,
|
|
force_stop_plan=SimpleNamespace(forcing=False, should_stop=False))
|
|
results = []
|
|
for tick in range(13):
|
|
now = green.MONO + 1_000_000_000 + tick * 50_000_000
|
|
results.append(owner.sample(scene(now, vision), cp, planner, detector, now_ns=now, drive_id=green.MONO-1_000_000_000))
|
|
assert planner.output_a_target == .5 and planner.output_should_stop is False
|
|
assert not any(results[:10]) and all(results[10:])
|
|
|
|
|
|
@pytest.mark.parametrize('defect', ['mismatched-model', 'weak-model-lead', 'native-stop', 'native-braking', 'force-stop',
|
|
'second-stopped', 'model-stop', 'stale', 'sign', 'config-change', 'source-change'])
|
|
def test_vision_release_requires_continuous_corroboration_and_native_go(defect):
|
|
cp = CarParams(openpilotLongitudinalControl=True, carFingerprint='KIA_NIRO_EV')
|
|
owner = StoppedLeadRelease()
|
|
detector = SimpleNamespace(committed=True, standstill_committed=True, standstill_reason='light', commit_distance_m=3.)
|
|
planner = SimpleNamespace(mpc=SimpleNamespace(solution_status=0), output_should_stop=False, output_a_target=.5,
|
|
force_stop_plan=SimpleNamespace(forcing=False, should_stop=False))
|
|
results = []
|
|
for tick in range(13):
|
|
now = green.MONO + 1_000_000_000 + tick * 50_000_000
|
|
sm = scene(now, True)
|
|
if defect == 'mismatched-model':
|
|
sm['radarState'].leadOne.dRel += 1.
|
|
elif defect == 'weak-model-lead':
|
|
sm['modelV2'].leadsV3[0].prob = .5
|
|
elif defect == 'native-stop':
|
|
planner.output_should_stop = True
|
|
elif defect == 'native-braking':
|
|
planner.output_a_target = -.1
|
|
elif defect == 'force-stop':
|
|
planner.force_stop_plan.forcing = True
|
|
elif defect == 'second-stopped':
|
|
sm['radarState'].leadTwo.vLead = 0.
|
|
elif defect == 'model-stop':
|
|
sm['modelV2'].action.shouldStop = True
|
|
elif defect == 'stale':
|
|
sm.logMonoTime['radarState'] = now - 150_000_001
|
|
elif defect == 'sign':
|
|
detector.standstill_reason = 'sign'
|
|
elif defect == 'config-change':
|
|
cp.carFingerprint = str(tick)
|
|
elif defect == 'source-change' and tick % 2:
|
|
sm = scene(now, False)
|
|
results.append(owner.sample(sm, cp, planner, detector, now_ns=now, drive_id=green.MONO-1_000_000_000))
|
|
assert not any(results)
|
|
|
|
|
|
@pytest.mark.parametrize('vision', [False, True])
|
|
def test_niro_actual_cem_hold_releases_only_after_native_go(monkeypatch, vision):
|
|
from opendbc.car.hyundai.values import CAR
|
|
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
|
case = green.CemGreenCallerTests(methodName='runTest')
|
|
case.setUp()
|
|
case.cp = interfaces[CAR.KIA_NIRO_EV].get_non_essential_params(CAR.KIA_NIRO_EV)
|
|
case.cp.openpilotLongitudinalControl = True
|
|
case.cp.pcmCruise = False
|
|
case.planner = LongitudinalPlanner(case.cp, init_v=4.27)
|
|
try:
|
|
case.prime()
|
|
original = green.serialized_scene
|
|
samples = [sample for sample in _samples() if 334.29 <= sample['routeSeconds'] <= 335.3]
|
|
motion = samples[0]
|
|
def moving_lead(**kwargs):
|
|
payloads = _motion_payloads(original(**kwargs), motion)
|
|
if vision:
|
|
model = payloads['modelV2']
|
|
for slot, lead in enumerate((payloads['radarState'].leadOne, payloads['radarState'].leadTwo)):
|
|
if lead.present:
|
|
raw = model.leadsV3[slot]
|
|
lead.from_dict(get_RadarState_from_vision(raw, payloads['carState'].vEgo, model.velocity.x[0], raw.prob))
|
|
return payloads
|
|
monkeypatch.setattr(green, 'serialized_scene', moving_lead)
|
|
observations = []
|
|
original_sample = StoppedLeadRelease.sample
|
|
def observed(owner, sm, cp, planner, detector, **kwargs):
|
|
before = float(planner.output_a_target), bool(planner.output_should_stop)
|
|
released = original_sample(owner, sm, cp, planner, detector, **kwargs)
|
|
observations.append((released, before, float(planner.output_a_target), bool(planner.output_should_stop)))
|
|
return released
|
|
monkeypatch.setattr(StoppedLeadRelease, 'sample', observed)
|
|
results = []
|
|
for motion in samples:
|
|
_, _, held, plan = case.cycle(stopped=True, position=motion['model']['position']['x'],
|
|
velocity=motion['model']['velocity']['x'], should_stop=False,
|
|
desired_accel=motion['model']['action']['desiredAcceleration'], lead=True)
|
|
results.append((held, plan.shouldStop, plan.aTarget))
|
|
assert any(released for released, _, _, _ in observations), (observations, results)
|
|
assert all(before == (target, stop) for _, before, target, stop in observations)
|
|
assert any(not held and not stop and target > .1 for held, stop, target in results)
|
|
assert not case.owner.mode.projector.stop_detector.committed
|
|
finally:
|
|
case.doCleanups()
|