mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-08 05:23:41 +08:00
firestar5683
198 lines
11 KiB
Python
198 lines
11 KiB
Python
from collections import deque
|
|
from contextlib import ExitStack
|
|
from unittest import mock
|
|
|
|
import pytest
|
|
|
|
from openpilot.cereal import log, messaging
|
|
from openpilot.common.prefix import OpenpilotPrefix
|
|
from opendbc.car.structs import car
|
|
from openpilot.starpilot.aol.tests import test_ioniq_availability as fixture
|
|
from openpilot.starpilot.aol.tests.test_carstate_queue import message
|
|
from openpilot.starpilot.aol.wire import IntentState, encode_intent
|
|
|
|
|
|
BASE = 10_020_000_000
|
|
|
|
|
|
def intent(stamp, sequence, *, latched=True):
|
|
wire = messaging.new_message('aolIntentWire', 0, valid=True, logMonoTime=stamp)
|
|
wire.aolIntentWire = encode_intent(IntentState('authority-card', sequence, stamp, stamp,
|
|
stamp + 200_000_000, latched, False, False, True, latched))
|
|
return wire
|
|
|
|
|
|
@pytest.mark.parametrize('pulse', ['healthy', 'cancel', 'brake', 'permanent_eps', 'temporary_eps', 'invalid_envelope', 'reordered', 'aol_gas_permanent_eps',
|
|
'stock_angle_gas_invalid_envelope', 'stock_angle_gas_permanent_eps'])
|
|
def test_late_carstate_during_environment_work_reaches_final_owners_once(pulse):
|
|
case = fixture.IoniqAvailabilityTests(methodName='runTest')
|
|
with ExitStack() as cleanup:
|
|
cleanup.callback(case.doCleanups)
|
|
with OpenpilotPrefix():
|
|
stock_angle = pulse.startswith('stock_angle_gas_')
|
|
held_gas = pulse == 'aol_gas_permanent_eps' or stock_angle
|
|
sd, _, step, _, _ = case._ioniq_availability_callers(full_op=not held_gas, stock_angle=stock_angle)
|
|
initial, _, initial_command = step(2, offset_ns=4_012_592, state_stamp=BASE, intent_message=intent(BASE, 3), gas=held_gas)
|
|
if held_gas:
|
|
assert initial.lateralActive and initial_command.latActive and sd.CS_prev.gasPressed
|
|
assert not initial.longitudinalActive and not initial_command.longActive and not sd.enabled
|
|
a, b, now = BASE, BASE + 27_819_143, BASE + 30_721_946
|
|
late = fixture.car_state()
|
|
late.vEgo = 25.0
|
|
late.vEgoRaw = 25.0
|
|
late.gasPressed = held_gas
|
|
if pulse == 'cancel':
|
|
late.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.cancel, pressed=True)]
|
|
elif pulse == 'brake':
|
|
late.brakePressed = True
|
|
elif pulse in ('permanent_eps', 'aol_gas_permanent_eps', 'stock_angle_gas_permanent_eps'):
|
|
late.steerFaultPermanent = True
|
|
elif pulse == 'temporary_eps':
|
|
late.steerFaultTemporary = late.standstill = True
|
|
queued = deque()
|
|
original_status = sd.update_process_health
|
|
original_receive = messaging.recv_one_or_none
|
|
|
|
def finish_preamble():
|
|
result = original_status()
|
|
queued.append(message(a - 1 if pulse == 'reordered' else b, late,
|
|
valid=pulse not in ('invalid_envelope', 'stock_angle_gas_invalid_envelope')))
|
|
return result
|
|
|
|
def receive(sock):
|
|
return queued.popleft() if sock is sd.car_state_sock and queued else (
|
|
None if sock is sd.car_state_sock else original_receive(sock))
|
|
|
|
with mock.patch.object(sd, 'update_process_health', side_effect=finish_preamble), \
|
|
mock.patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one_or_none', side_effect=receive), \
|
|
mock.patch.object(sd.car_events, 'update', wraps=sd.car_events.update) as car_events, \
|
|
mock.patch.object(sd.excessive_actuation_check, 'update', wraps=sd.excessive_actuation_check.update) as excessive, \
|
|
mock.patch.object(sd.state_machine, 'update', wraps=sd.state_machine.update) as transition:
|
|
axis, _, command = step(5, offset_ns=721_946, state_stamp=a, intent_message=intent(b, 4), gas=held_gas)
|
|
assert now - a == 30_721_946 and now - b == 2_902_803
|
|
assert not queued and sd.CS_prev.vEgo == 25.0
|
|
assert sd.CS_prev.gasPressed == held_gas
|
|
assert car_events.call_count == 2 and transition.call_count == 1
|
|
assert excessive.call_count == (1 if sd.calibrated_pose is not None else 0)
|
|
if pulse == 'healthy':
|
|
assert axis.sourceCarStateMonoTime == b and sd.aol_last_intent.carStateLogMonoTime == b
|
|
assert axis.lateralActive and axis.longitudinalActive and command.latActive and command.longActive
|
|
assert sd.enabled and sd.active and not sd.aol_authority_lost
|
|
else:
|
|
if pulse == 'temporary_eps':
|
|
assert axis.longitudinalActive and command.longActive and sd.enabled and sd.active
|
|
else:
|
|
assert not axis.longitudinalActive and not command.longActive
|
|
if pulse not in ('brake',):
|
|
assert not axis.lateralActive and not command.latActive
|
|
if pulse in ('invalid_envelope', 'reordered', 'stock_angle_gas_invalid_envelope'):
|
|
assert axis.sourceCarStateMonoTime == 0 and not sd.conditional_car_state_valid
|
|
if pulse == 'temporary_eps':
|
|
assert log.OnroadEvent.EventName.steerTempUnavailableSilent in sd.events.names
|
|
assert not sd.aol_authority_lost and sd.aol_last_intent.allowedLatch
|
|
if pulse in ('cancel', 'brake'):
|
|
assert not sd.enabled and not sd.events.contains(fixture.ET.ENABLE)
|
|
|
|
|
|
@pytest.mark.parametrize('operation', ['ordinary', 'aol_only', 'aol_only_gas', 'stock_angle_gas'])
|
|
@pytest.mark.parametrize('timing', ['timely', 'late_poll', 'processing_crosses_deadline'])
|
|
def test_fresh_source_replaces_expired_output_without_restoring_disengagement(operation, timing):
|
|
case = fixture.IoniqAvailabilityTests(methodName='runTest')
|
|
with ExitStack() as cleanup:
|
|
cleanup.callback(case.doCleanups)
|
|
with OpenpilotPrefix():
|
|
full_op = operation == 'ordinary'
|
|
state = fixture.car_state()
|
|
held_gas = operation in ('aol_only_gas', 'stock_angle_gas')
|
|
state.gasPressed = held_gas
|
|
state.vEgoRaw = state.vEgo
|
|
sd, controls, step, _, _ = case._ioniq_availability_callers(full_op=full_op, stock_angle=operation == 'stock_angle_gas')
|
|
original, _, command = step(2, offset_ns=4_266_961, state_stamp=BASE, intent_message=intent(BASE, 3), observed_state=state, gas=held_gas)
|
|
assert original.lateralActive and command.latActive
|
|
assert original.longitudinalActive == full_op and command.longActive == full_op
|
|
assert sd.enabled == full_op and sd.aol_last_intent.allowedLatch
|
|
assert sd.CS_prev.gasPressed == held_gas
|
|
deadline = BASE + 34_266_961
|
|
assert original.validUntilMonoTime == deadline
|
|
now = [BASE + 30_254_367]
|
|
next_stamp = BASE + 30_582_763
|
|
queued, wires = deque(), deque()
|
|
original_receive = messaging.recv_one_or_none
|
|
original_car_update = sd._update_car_event_sample
|
|
|
|
def receive(sock):
|
|
if sock is sd.car_state_sock:
|
|
return queued.popleft() if queued else None
|
|
if sock is sd.aol_intent_companion.sock:
|
|
return wires.popleft() if wires else None
|
|
return original_receive(sock)
|
|
|
|
def poll(timeout):
|
|
assert 0 < timeout <= (deadline - now[0]) // 1_000_000
|
|
held, _ = controls.state_control()
|
|
assert held.latActive and held.longActive == full_op
|
|
now[0] = deadline + 1 if timing == 'late_poll' else next_stamp + 100_000
|
|
if timing == 'late_poll':
|
|
expired, _ = controls.state_control()
|
|
assert not expired.latActive and not expired.longActive
|
|
queued.append(message(next_stamp, state))
|
|
wires.append(intent(next_stamp, 4).as_reader())
|
|
return [sd.car_state_sock]
|
|
|
|
def car_update(sample, previous):
|
|
result = original_car_update(sample, previous)
|
|
if timing == 'processing_crosses_deadline':
|
|
now[0] = deadline + 1
|
|
expired, _ = controls.state_control()
|
|
assert not expired.latActive and not expired.longActive
|
|
return result
|
|
|
|
poller = mock.Mock()
|
|
poller.poll.side_effect = poll
|
|
sd.car_state_poller = poller
|
|
with mock.patch('openpilot.selfdrive.selfdrived.selfdrived.time.monotonic_ns', side_effect=lambda: now[0]), \
|
|
mock.patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.recv_one_or_none', side_effect=receive), \
|
|
mock.patch.object(sd, '_update_car_event_sample', side_effect=car_update), \
|
|
mock.patch.object(sd.state_machine, 'update', wraps=sd.state_machine.update) as transition:
|
|
axis, _, command = step(5, timeout=True, offset_ns=254_367, intent_message=intent(BASE, 3), gas=held_gas)
|
|
assert sd.CS_prev.gasPressed == held_gas
|
|
assert poller.poll.call_count == 1 and transition.call_count == 1 and not queued
|
|
assert sd.aol_last_intent is not None and sd.aol_last_intent.allowedLatch and sd.aol_last_intent.lateralArmed
|
|
assert now[0] - next_stamp < 30_000_000
|
|
assert axis.sourceCarStateMonoTime == next_stamp and axis.lateralActive and command.latActive
|
|
assert axis.longitudinalActive == full_op and command.longActive == full_op
|
|
assert sd.enabled == full_op and not sd.aol_authority_lost
|
|
assert axis.validUntilMonoTime > deadline
|
|
if operation == 'aol_only_gas':
|
|
for tick in range(7, 107):
|
|
continued, _, continued_command = step(tick, observed_state=state, gas=held_gas, native_request_long=False)
|
|
assert continued.lateralActive and continued_command.latActive
|
|
assert sd.CS_prev.gasPressed
|
|
assert not continued.longitudinalActive and not continued_command.longActive and not sd.enabled
|
|
assert sd.aol_last_intent.allowedLatch and sd.aol_last_intent.lateralArmed
|
|
|
|
|
|
@pytest.mark.parametrize('latched', [False, True])
|
|
def test_route_4f_new_source_after_previous_output_deadline(latched):
|
|
case = fixture.IoniqAvailabilityTests(methodName='runTest')
|
|
with ExitStack() as cleanup:
|
|
cleanup.callback(case.doCleanups)
|
|
with OpenpilotPrefix():
|
|
sd, controls, step, _, _ = case._ioniq_availability_callers()
|
|
prior, _, _ = step(2, offset_ns=4_322_384, state_stamp=BASE,
|
|
intent_message=intent(BASE, 3, latched=latched), latched=latched)
|
|
now = BASE + 35_936_883
|
|
source = BASE + 32_971_113
|
|
assert now - prior.validUntilMonoTime == 1_614_499
|
|
assert now - source == 2_965_770
|
|
with mock.patch('openpilot.selfdrive.controls.controlsd.time.monotonic_ns', return_value=now):
|
|
expired, _ = controls.state_control()
|
|
assert not expired.latActive and not expired.longActive
|
|
axis, state, command = step(5, offset_ns=5_936_883, state_stamp=source,
|
|
intent_message=intent(source, 4, latched=latched), latched=latched)
|
|
assert axis.sourceCarStateMonoTime == source
|
|
assert axis.lateralActive and axis.longitudinalActive and command.latActive and command.longActive
|
|
assert state.enabled and sd.enabled and sd.active
|
|
assert not sd.aol_authority_lost
|
|
assert log.OnroadEvent.EventName.controlsMismatch not in sd.events.names
|