mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-23 15:23:46 +08:00
long: improve DEC radar and model braking
This commit is contained in:
rav4kumar
@@ -15,13 +15,9 @@ class WMACConstants:
|
||||
LEAD_EXIT_PROB = 0.25
|
||||
LEAD_RISE_RATE = 1.0
|
||||
LEAD_FALL_RATE = 0.35
|
||||
RADAR_LEAD_ACC_PROB = 0.5
|
||||
RADAR_LEAD_ACC_EXIT_PROB = 0.4
|
||||
RADAR_LEAD_ACC_RISE_RATE = 1.0
|
||||
RADAR_LEAD_ACC_FALL_RATE = 0.25
|
||||
RADAR_LEAD_ACC_MAX_DREL = 80.0
|
||||
RADAR_LEAD_ACC_MAX_TTC = 6.0
|
||||
RADAR_LEAD_ACC_MIN_CLOSING_SPEED = -0.5
|
||||
RADAR_LEAD_CONTINUITY_FRAMES = max(1, int(round(1.0 / DT_MDL)))
|
||||
RADAR_LEAD_DROPOUT_FRAMES = max(1, int(round(0.2 / DT_MDL)))
|
||||
RADAR_STALE_FRAMES = max(1, int(round(0.5 / DT_MDL)))
|
||||
|
||||
SLOW_DOWN_PROB = 0.5
|
||||
SLOW_DOWN_EXIT_PROB = 0.4
|
||||
@@ -33,6 +29,12 @@ class WMACConstants:
|
||||
|
||||
MODEL_DECEL_START = -0.5
|
||||
MODEL_DECEL_RANGE = 2.0
|
||||
MODEL_DECEL_TREND_FRAMES = 4
|
||||
MODEL_DECEL_TREND_ACCEL = -0.075
|
||||
MODEL_DECEL_TREND_RATE = 0.35
|
||||
MODEL_DECEL_TREND_MAX_MPC_ACCEL = 0.075
|
||||
MODEL_DECEL_TREND_MAX_COMMAND_STEP = 0.15
|
||||
MODEL_DECEL_TREND_RELEASE_ACCEL = -0.02
|
||||
ENDPOINT_URGENCY_GAIN = 1.3
|
||||
CRITICAL_ENDPOINT_FACTOR = 0.3
|
||||
CRITICAL_URGENCY_GAIN = 1.5
|
||||
|
||||
@@ -6,12 +6,15 @@ See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
# Version = 2025-6-30
|
||||
|
||||
from collections import deque
|
||||
import math
|
||||
from typing import Literal
|
||||
|
||||
from openpilot.cereal import messaging
|
||||
from numpy import interp
|
||||
from opendbc.car import structs
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
|
||||
|
||||
ModeType = Literal['acc', 'blended']
|
||||
@@ -69,7 +72,7 @@ class ModeTransitionManager:
|
||||
|
||||
def request_mode(self, mode: ModeType, immediate: bool = False, hold_frames: int = 0, cancel_hold: bool = False) -> None:
|
||||
if immediate:
|
||||
self._blended_hold_frames = max(self._blended_hold_frames, hold_frames)
|
||||
self._blended_hold_frames = max(self._blended_hold_frames, hold_frames) if mode == 'blended' else 0
|
||||
self._pending_mode = mode
|
||||
self._pending_count = 0
|
||||
self._switch_mode(mode)
|
||||
@@ -135,12 +138,6 @@ class DynamicExperimentalController:
|
||||
rise_rate=WMACConstants.LEAD_RISE_RATE,
|
||||
fall_rate=WMACConstants.LEAD_FALL_RATE,
|
||||
)
|
||||
self._radar_acc_lead_tracker = HysteresisSignal(
|
||||
enter_threshold=WMACConstants.RADAR_LEAD_ACC_PROB,
|
||||
exit_threshold=WMACConstants.RADAR_LEAD_ACC_EXIT_PROB,
|
||||
rise_rate=WMACConstants.RADAR_LEAD_ACC_RISE_RATE,
|
||||
fall_rate=WMACConstants.RADAR_LEAD_ACC_FALL_RATE,
|
||||
)
|
||||
self._slow_down_tracker = HysteresisSignal(
|
||||
enter_threshold=WMACConstants.SLOW_DOWN_PROB,
|
||||
exit_threshold=WMACConstants.SLOW_DOWN_EXIT_PROB,
|
||||
@@ -155,7 +152,12 @@ class DynamicExperimentalController:
|
||||
)
|
||||
|
||||
self._has_lead_filtered = False
|
||||
self._has_any_lead = False
|
||||
self._has_current_radar_acc_lead = False
|
||||
self._has_radar_acc_lead = False
|
||||
self._radar_acc_lead_frames = 0
|
||||
self._radar_fresh = True
|
||||
self._radar_stale_frames = 0
|
||||
self._has_slow_down = False
|
||||
self._has_slowness = False
|
||||
self._has_mpc_fcw = False
|
||||
@@ -169,6 +171,10 @@ class DynamicExperimentalController:
|
||||
self._expected_distance = 0.0
|
||||
self._trajectory_valid = False
|
||||
self._raw_urgency = 0.0
|
||||
self._model_accel_samples = deque(maxlen=WMACConstants.MODEL_DECEL_TREND_FRAMES)
|
||||
self._model_decel_trending = False
|
||||
self._model_decel_latched = False
|
||||
self._planner_accel = math.nan
|
||||
|
||||
def _read_params(self) -> None:
|
||||
if self._frame % WMACConstants.PARAM_READ_FRAMES == 0:
|
||||
@@ -186,9 +192,11 @@ class DynamicExperimentalController:
|
||||
def set_mpc_fcw_crash_cnt(self) -> None:
|
||||
self._mpc_fcw_crash_cnt = self._mpc.crash_cnt
|
||||
|
||||
def _update_calculations(self, sm: messaging.SubMaster) -> None:
|
||||
def _update_calculations(self, sm: messaging.SubMaster, radar_fresh: bool) -> None:
|
||||
car_state = sm['carState']
|
||||
lead_one = sm['radarState'].leadOne
|
||||
radar_state = sm['radarState']
|
||||
lead_one = radar_state.leadOne
|
||||
lead_two = radar_state.leadTwo
|
||||
md = sm['modelV2']
|
||||
|
||||
self._v_ego_kph = car_state.vEgo * 3.6
|
||||
@@ -200,8 +208,24 @@ class DynamicExperimentalController:
|
||||
else:
|
||||
self._standstill_count = max(0, self._standstill_count - 1)
|
||||
|
||||
self._has_lead_filtered = self._lead_tracker.update(float(lead_one.present))
|
||||
self._has_radar_acc_lead = self._radar_acc_lead_tracker.update(self._radar_acc_lead_score(lead_one))
|
||||
self._radar_fresh = bool(radar_fresh)
|
||||
if self._radar_fresh:
|
||||
self._radar_stale_frames = 0
|
||||
self._has_lead_filtered = self._lead_tracker.update(float(lead_one.present))
|
||||
self._has_any_lead = bool(lead_one.present or lead_two.present)
|
||||
self._has_current_radar_acc_lead = bool(max(self._radar_acc_lead_score(lead_one), self._radar_acc_lead_score(lead_two)))
|
||||
self._update_radar_acc_lead()
|
||||
else:
|
||||
self._radar_stale_frames += 1
|
||||
self._has_current_radar_acc_lead = False
|
||||
if self._radar_stale_frames < WMACConstants.RADAR_STALE_FRAMES:
|
||||
self._update_radar_acc_lead()
|
||||
else:
|
||||
self._lead_tracker.reset()
|
||||
self._has_lead_filtered = False
|
||||
self._has_any_lead = False
|
||||
self._has_radar_acc_lead = False
|
||||
self._radar_acc_lead_frames = 0
|
||||
self._has_mpc_fcw = self._mpc_fcw_crash_cnt > 0
|
||||
self._calculate_slow_down(md)
|
||||
|
||||
@@ -217,6 +241,7 @@ class DynamicExperimentalController:
|
||||
self._expected_distance = 0.0
|
||||
self._trajectory_valid = False
|
||||
|
||||
self._update_model_decel_trend(md)
|
||||
urgency = self._model_action_urgency(md)
|
||||
position_valid = len(md.position.x) == WMACConstants.TRAJECTORY_SIZE
|
||||
|
||||
@@ -230,17 +255,46 @@ class DynamicExperimentalController:
|
||||
self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency)
|
||||
self._urgency = self._slow_down_tracker.value
|
||||
|
||||
def _radar_acc_lead_score(self, lead_one) -> float:
|
||||
if not lead_one.present:
|
||||
return 0.0
|
||||
def _update_model_decel_trend(self, md) -> None:
|
||||
try:
|
||||
desired_accel = float(md.action.desiredAcceleration)
|
||||
except (AttributeError, OverflowError, TypeError, ValueError):
|
||||
desired_accel = math.nan
|
||||
if not math.isfinite(desired_accel):
|
||||
self._reset_model_decel_trend()
|
||||
else:
|
||||
self._model_accel_samples.append(desired_accel)
|
||||
history = tuple(self._model_accel_samples)
|
||||
self._model_decel_trending = (len(history) == self._model_accel_samples.maxlen
|
||||
and history[-1] <= WMACConstants.MODEL_DECEL_TREND_ACCEL
|
||||
and (history[0] - history[-1]) / (DT_MDL * (len(history) - 1)) > WMACConstants.MODEL_DECEL_TREND_RATE
|
||||
and all(after <= before for before, after in zip(history[:-1], history[1:], strict=True))
|
||||
and sum(after < before for before, after in zip(history[:-1], history[1:], strict=True)) >= 2)
|
||||
if len(history) == self._model_accel_samples.maxlen and all(
|
||||
accel >= WMACConstants.MODEL_DECEL_TREND_RELEASE_ACCEL for accel in history
|
||||
):
|
||||
self._model_decel_latched = False
|
||||
|
||||
d_rel = float(getattr(lead_one, 'dRel', float('inf')))
|
||||
v_rel = float(getattr(lead_one, 'vRel', 0.0))
|
||||
if d_rel <= WMACConstants.RADAR_LEAD_ACC_MAX_DREL:
|
||||
return 1.0
|
||||
if v_rel <= WMACConstants.RADAR_LEAD_ACC_MIN_CLOSING_SPEED and d_rel / max(-v_rel, 0.1) <= WMACConstants.RADAR_LEAD_ACC_MAX_TTC:
|
||||
return 1.0
|
||||
return 0.0
|
||||
def _reset_model_decel_trend(self) -> None:
|
||||
self._model_accel_samples.clear()
|
||||
self._model_decel_trending = False
|
||||
self._model_decel_latched = False
|
||||
|
||||
def _radar_acc_lead_score(self, lead_one) -> float:
|
||||
radar_track_id = int(getattr(lead_one, 'radarTrackId', -1))
|
||||
return float(lead_one.present and (bool(getattr(lead_one, 'radar', False)) or radar_track_id >= 0))
|
||||
|
||||
def _update_radar_acc_lead(self) -> None:
|
||||
if self._has_current_radar_acc_lead:
|
||||
self._radar_acc_lead_frames = WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES
|
||||
self._has_radar_acc_lead = True
|
||||
return
|
||||
|
||||
if not self._has_any_lead:
|
||||
self._radar_acc_lead_frames = min(self._radar_acc_lead_frames, WMACConstants.RADAR_LEAD_DROPOUT_FRAMES)
|
||||
|
||||
self._has_radar_acc_lead = self._radar_acc_lead_frames > 0
|
||||
self._radar_acc_lead_frames = max(0, self._radar_acc_lead_frames - 1)
|
||||
|
||||
def _model_action_urgency(self, md) -> float:
|
||||
action = getattr(md, 'action', None)
|
||||
@@ -269,16 +323,41 @@ class DynamicExperimentalController:
|
||||
|
||||
return urgency
|
||||
|
||||
def _model_decel_handoff_ready(self) -> bool:
|
||||
try:
|
||||
mpc_accel = float(self._mpc.a_solution[1])
|
||||
return (math.isfinite(mpc_accel) and mpc_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL
|
||||
and math.isfinite(self._planner_accel) and self._planner_accel <= WMACConstants.MODEL_DECEL_TREND_MAX_MPC_ACCEL
|
||||
and self._planner_accel - self._model_accel_samples[-1] <= WMACConstants.MODEL_DECEL_TREND_MAX_COMMAND_STEP)
|
||||
except (AttributeError, IndexError, OverflowError, TypeError, ValueError):
|
||||
return False
|
||||
|
||||
def _desired_mode(self) -> tuple[ModeType, bool]:
|
||||
standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES
|
||||
urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB
|
||||
|
||||
if not self._CP.radarUnavailable and self._has_current_radar_acc_lead:
|
||||
self._reset_model_decel_trend()
|
||||
return 'acc', True
|
||||
|
||||
radar_stale = not self._radar_fresh if self._has_mpc_fcw else self._radar_stale_frames > 1
|
||||
if (radar_stale or not self._has_any_lead) and (self._has_mpc_fcw or urgent_slow_down):
|
||||
self._radar_acc_lead_frames = 0
|
||||
self._has_radar_acc_lead = False
|
||||
return 'blended', True
|
||||
|
||||
if not self._CP.radarUnavailable and self._has_radar_acc_lead:
|
||||
return 'acc', False
|
||||
self._reset_model_decel_trend()
|
||||
return 'acc', True
|
||||
|
||||
entering_model_slowdown = self._model_decel_trending and self._model_decel_handoff_ready() and not self._model_decel_latched
|
||||
self._model_decel_latched |= entering_model_slowdown
|
||||
if self._model_decel_latched:
|
||||
return 'blended', entering_model_slowdown
|
||||
|
||||
if self._has_mpc_fcw:
|
||||
return 'blended', True
|
||||
|
||||
standstill = self._standstill_count > WMACConstants.STANDSTILL_FRAMES
|
||||
urgent_slow_down = self._has_slow_down and self._raw_urgency > WMACConstants.URGENT_SLOW_DOWN_PROB
|
||||
|
||||
if self._CP.radarUnavailable:
|
||||
if standstill or self._has_slow_down:
|
||||
return 'blended', urgent_slow_down
|
||||
@@ -289,15 +368,24 @@ class DynamicExperimentalController:
|
||||
|
||||
return 'acc', False
|
||||
|
||||
def update(self, sm: messaging.SubMaster) -> None:
|
||||
def update(self, sm: messaging.SubMaster, *, radar_fresh: bool = True, planner_accel: float | None = None) -> None:
|
||||
self._read_params()
|
||||
self.set_mpc_fcw_crash_cnt()
|
||||
self._update_calculations(sm)
|
||||
try:
|
||||
self._planner_accel = float(planner_accel)
|
||||
except (OverflowError, TypeError, ValueError):
|
||||
self._planner_accel = math.nan
|
||||
self._update_calculations(sm, radar_fresh)
|
||||
self._active = sm['selfdriveState'].experimentalMode and self._enabled
|
||||
if not self._active:
|
||||
model_decel_latched = self._model_decel_latched
|
||||
self._reset_model_decel_trend()
|
||||
if model_decel_latched:
|
||||
self._mode_manager.request_mode('acc', immediate=True)
|
||||
|
||||
mode, immediate = self._desired_mode()
|
||||
self._mode_manager.request_mode(mode, immediate=immediate, hold_frames=WMACConstants.EMERGENCY_HOLD_FRAMES,
|
||||
cancel_hold=self._has_radar_acc_lead)
|
||||
cancel_hold=not self._CP.radarUnavailable and self._has_radar_acc_lead)
|
||||
self._mode_manager.update()
|
||||
|
||||
self._active = sm['selfdriveState'].experimentalMode and self._enabled
|
||||
self._frame += 1
|
||||
|
||||
@@ -1,18 +1,22 @@
|
||||
import pytest
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, HysteresisSignal
|
||||
|
||||
|
||||
class MockLeadOne:
|
||||
def __init__(self, status=0.0, dRel=30.0, vRel=0.0):
|
||||
def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1):
|
||||
self.present = status
|
||||
self.dRel = dRel
|
||||
self.vRel = vRel
|
||||
self.radar = radar
|
||||
self.radarTrackId = radarTrackId
|
||||
|
||||
|
||||
class MockRadarState:
|
||||
def __init__(self, status=0.0, dRel=30.0, vRel=0.0):
|
||||
self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel)
|
||||
def __init__(self, status=0.0, dRel=30.0, vRel=0.0, radar=False, radarTrackId=-1, leadTwo=None):
|
||||
self.leadOne = MockLeadOne(status=status, dRel=dRel, vRel=vRel, radar=radar, radarTrackId=radarTrackId)
|
||||
self.leadTwo = leadTwo if leadTwo is not None else MockLeadOne()
|
||||
|
||||
|
||||
class MockCarState:
|
||||
@@ -55,7 +59,7 @@ class MockParams:
|
||||
def default_sm():
|
||||
sm = {
|
||||
'carState': MockCarState(vEgo=10.0, vCruise=20.0),
|
||||
'radarState': MockRadarState(status=1.0),
|
||||
'radarState': MockRadarState(status=1.0, radar=True, radarTrackId=7),
|
||||
'modelV2': MockModelData(valid=True),
|
||||
'selfdriveState': MockSelfDriveState(experimentalMode=True),
|
||||
}
|
||||
@@ -73,6 +77,7 @@ def mock_cp():
|
||||
def mock_mpc():
|
||||
class MPC:
|
||||
crash_cnt = 0
|
||||
a_solution = [0.0, 0.0]
|
||||
return MPC()
|
||||
|
||||
|
||||
@@ -155,9 +160,162 @@ def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mo
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_confirmed_model_decel_trend_enters_blended_before_a_large_command(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert controller._model_decel_trending
|
||||
assert not controller._has_slow_down
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_confirmed_model_decel_handoff_stays_latched_through_a_plateau(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
for _ in range(WMACConstants.EMERGENCY_HOLD_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES + 1):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-0.12)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert not controller._model_decel_trending
|
||||
assert controller._model_decel_latched
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
for _ in range(WMACConstants.MODEL_DECEL_TREND_FRAMES + WMACConstants.EXIT_BLENDED_FRAMES):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert not controller._model_decel_latched
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_model_decel_trend_never_overrides_a_radar_lead(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert not controller._model_accel_samples
|
||||
assert not controller._model_decel_latched
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_radar_acquisition_clears_a_latched_model_decel_handoff(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
assert controller._model_decel_latched
|
||||
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert not controller._model_accel_samples
|
||||
assert not controller._model_decel_latched
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_model_decel_trend_does_not_accumulate_while_dec_is_inactive(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
default_sm['selfdriveState'].experimentalMode = False
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert not controller._model_accel_samples
|
||||
assert not controller._model_decel_latched
|
||||
|
||||
default_sm['selfdriveState'].experimentalMode = True
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
assert not controller._model_decel_trending
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_disabling_dec_clears_a_latched_model_decel_mode(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
assert controller._model_decel_latched
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
default_sm['selfdriveState'].experimentalMode = False
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=0.0)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert not controller._model_decel_latched
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_model_decel_trend_waits_while_mpc_is_accelerating(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
mock_mpc.a_solution[1] = 0.5
|
||||
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.0)
|
||||
|
||||
assert controller._model_decel_trending
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_steep_model_decel_trend_defers_to_the_existing_urgent_path(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
|
||||
for desired_acceleration in (0.0, -0.2, -0.4, -0.6):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.05)
|
||||
|
||||
assert controller._model_decel_trending
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_model_decel_trend_waits_while_the_planner_is_accelerating(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
|
||||
for desired_acceleration in (-0.02, -0.05, -0.08, -0.12):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm, planner_accel=0.2)
|
||||
|
||||
assert controller._model_decel_trending
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_alternating_model_accel_noise_does_not_trigger_an_early_handoff(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
|
||||
for desired_acceleration in (0.0, -0.2, 0.0, -0.2):
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=desired_acceleration)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert not controller._model_decel_trending
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=1.0)
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
|
||||
for _ in range(3):
|
||||
@@ -168,36 +326,94 @@ def test_radar_lead_keeps_acc_over_model_slowdown(mock_cp, mock_mpc, default_sm)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_far_radar_lead_allows_blended_until_acc_relevant(mock_cp, mock_mpc, default_sm):
|
||||
def test_far_radar_lead_always_uses_acc(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0)
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0, radar=True)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller._has_lead_filtered
|
||||
assert not controller._has_radar_acc_lead
|
||||
assert controller.mode() == "blended"
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_relevant_radar_lead_smoothly_returns_to_acc(mock_cp, mock_mpc, default_sm):
|
||||
def test_radar_acquisition_immediately_returns_blended_to_acc(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=0.0)
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
controller.update(default_sm)
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, dRel=45.0, vRel=0.0)
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, radar=True, radarTrackId=7)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
default_sm['modelV2'] = MockModelData(valid=True)
|
||||
for _ in range(20):
|
||||
controller.update(default_sm)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_close_vision_only_lead_can_use_blended(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert not controller._has_radar_acc_lead
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_second_radar_lead_forces_acc(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
lead_two = MockLeadOne(status=1.0, dRel=120.0, radar=True, radarTrackId=8)
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, dRel=30.0, vRel=-5.0, leadTwo=lead_two)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_second_vision_only_lead_does_not_force_acc(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
lead_two = MockLeadOne(status=1.0, dRel=20.0, vRel=-10.0)
|
||||
default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert not controller._has_radar_acc_lead
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_inactive_lead_with_radar_marker_does_not_force_acc(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0, radar=True, radarTrackId=7)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert not controller._has_radar_acc_lead
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_radarless_car_ignores_marked_radar_track(mock_cp, mock_mpc, default_sm):
|
||||
mock_cp.radarUnavailable = True
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_closing_far_radar_lead_returns_to_acc(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=-25.0)
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, dRel=120.0, vRel=-25.0, radarTrackId=7)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
|
||||
for _ in range(20):
|
||||
@@ -209,7 +425,7 @@ def test_closing_far_radar_lead_returns_to_acc(mock_cp, mock_mpc, default_sm):
|
||||
|
||||
def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=1.0)
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
|
||||
default_sm['carState'].standstill = True
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, should_stop=True)
|
||||
mock_mpc.crash_cnt = 1
|
||||
@@ -224,12 +440,194 @@ def test_radar_lead_keeps_acc_over_fcw_and_standstill(mock_cp, mock_mpc, default
|
||||
|
||||
def test_lead_flicker_hold_prevents_one_frame_mode_flip(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=1.0)
|
||||
controller.update(default_sm)
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0)
|
||||
for _ in range(2):
|
||||
controller.update(default_sm)
|
||||
assert controller._has_slow_down
|
||||
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller._has_lead_filtered
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_radar_lead_continuity_with_vision_fallback_expires_into_confirmed_transition(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=50.0)
|
||||
for _ in range(2):
|
||||
controller.update(default_sm)
|
||||
assert controller._has_slow_down
|
||||
|
||||
default_sm['radarState'] = MockRadarState(status=1.0)
|
||||
for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES):
|
||||
controller.update(default_sm)
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
controller.update(default_sm)
|
||||
assert not controller._has_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
for _ in range(WMACConstants.ENTER_BLENDED_FRAMES - 1):
|
||||
controller.update(default_sm)
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_radar_lead_short_dropout_guard_expires_without_any_lead(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
|
||||
controller.update(default_sm)
|
||||
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
for _ in range(WMACConstants.RADAR_LEAD_DROPOUT_FRAMES):
|
||||
controller.update(default_sm)
|
||||
assert controller._has_radar_acc_lead
|
||||
|
||||
controller.update(default_sm)
|
||||
assert not controller._has_radar_acc_lead
|
||||
|
||||
|
||||
def test_one_stale_radar_frame_does_not_drop_acc_authority(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
controller.update(default_sm)
|
||||
|
||||
controller.update(default_sm, radar_fresh=False)
|
||||
|
||||
assert not controller._has_current_radar_acc_lead
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller._radar_acc_lead_frames == WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES - 1
|
||||
assert controller._radar_stale_frames == 1
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_one_stale_radar_frame_does_not_override_retained_lead_for_model_urgency(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
controller.update(default_sm)
|
||||
default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
|
||||
|
||||
controller.update(default_sm, radar_fresh=False)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
controller.update(default_sm, radar_fresh=False)
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_one_stale_radar_frame_does_not_delay_fcw(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
controller.update(default_sm)
|
||||
mock_mpc.crash_cnt = 1
|
||||
|
||||
controller.update(default_sm, radar_fresh=False)
|
||||
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_frozen_radar_marker_cannot_rearm_acc_authority(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
controller.update(default_sm)
|
||||
|
||||
for _ in range(WMACConstants.RADAR_STALE_FRAMES - 1):
|
||||
controller.update(default_sm, radar_fresh=False)
|
||||
assert controller._has_radar_acc_lead
|
||||
|
||||
controller.update(default_sm, radar_fresh=False)
|
||||
|
||||
assert not controller._has_current_radar_acc_lead
|
||||
assert not controller._has_radar_acc_lead
|
||||
assert not controller._has_any_lead
|
||||
assert not controller._has_lead_filtered
|
||||
|
||||
|
||||
def test_fresh_radar_reacquisition_after_stale_timeout_is_immediate(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
controller.update(default_sm)
|
||||
for _ in range(WMACConstants.RADAR_STALE_FRAMES):
|
||||
controller.update(default_sm, radar_fresh=False)
|
||||
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
controller.update(default_sm, radar_fresh=False)
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
|
||||
default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
|
||||
controller.update(default_sm, radar_fresh=True)
|
||||
|
||||
assert controller._radar_stale_frames == 0
|
||||
assert controller._has_current_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
@pytest.mark.parametrize("urgent_source", ["fcw", "should_stop"])
|
||||
def test_no_lead_urgent_slowdown_bypasses_radar_dropout_guard(mock_cp, mock_mpc, default_sm, urgent_source):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
|
||||
controller.update(default_sm)
|
||||
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
if urgent_source == "fcw":
|
||||
mock_mpc.crash_cnt = 1
|
||||
else:
|
||||
default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert not controller._has_radar_acc_lead
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
mock_mpc.crash_cnt = 0
|
||||
default_sm['modelV2'] = MockModelData(valid=True)
|
||||
controller.update(default_sm)
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_lead_two_radar_authority_continues_with_vision_lead_one(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
|
||||
default_sm['radarState'] = MockRadarState(status=0.0, leadTwo=lead_two)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
controller.update(default_sm)
|
||||
assert controller._has_current_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
default_sm['radarState'] = MockRadarState(status=1.0)
|
||||
for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES):
|
||||
controller.update(default_sm)
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_alternating_radar_slots_keep_acc_authority(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
|
||||
for frame in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES * 2):
|
||||
if frame % 2 == 0:
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7, leadTwo=MockLeadOne(status=1.0))
|
||||
else:
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=MockLeadOne(status=1.0, radar=True, radarTrackId=8))
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller._has_current_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_radar_reacquisition_immediately_restores_acc_after_continuity_expiry(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, radar=True, radarTrackId=7)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
controller.update(default_sm)
|
||||
|
||||
default_sm['radarState'] = MockRadarState(status=1.0)
|
||||
for _ in range(WMACConstants.RADAR_LEAD_CONTINUITY_FRAMES + 1):
|
||||
controller.update(default_sm)
|
||||
assert not controller._has_radar_acc_lead
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
lead_two = MockLeadOne(status=1.0, radar=True, radarTrackId=8)
|
||||
default_sm['radarState'] = MockRadarState(status=1.0, leadTwo=lead_two)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller._has_current_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
@@ -121,7 +121,7 @@ class LongitudinalPlannerSP:
|
||||
self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
|
||||
self.accel_controller.update_params()
|
||||
self.events_sp.clear()
|
||||
self.dec.update(sm)
|
||||
self.dec.update(sm, radar_fresh=self._radar_fresh_this_cycle, planner_accel=self.output_a_target)
|
||||
self.e2e_alerts_helper.update(sm, self.events_sp)
|
||||
|
||||
def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
|
||||
|
||||
+59
@@ -394,6 +394,65 @@ def test_e2e_to_radar_acc_handoff_keeps_braking_continuous():
|
||||
assert active[transition]
|
||||
|
||||
|
||||
def test_dec_retains_acc_through_route_like_radar_marker_dropout():
|
||||
dropout_start = 1.0
|
||||
reacquisition_time = 1.8
|
||||
|
||||
def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation:
|
||||
frame = round(current_time / DT_MDL)
|
||||
if current_time < dropout_start:
|
||||
marked_slot = "leadOne" if frame % 2 == 0 else "leadTwo"
|
||||
return truth | {"radar": lead_name == marked_slot, "radarTrackId": 985 + frame if lead_name == marked_slot else -1}
|
||||
if current_time < reacquisition_time:
|
||||
return truth | {"radar": False, "radarTrackId": -1}
|
||||
return truth | {"radar": lead_name == "leadOne", "radarTrackId": 1263 if lead_name == "leadOne" else -1}
|
||||
|
||||
trace = _run(
|
||||
duration=2.5, controller_enabled=True, dec_enabled=True, e2e=True, lead_relevancy=True, speed=20.0,
|
||||
distance_lead=35.0, v_lead=18.0, v_cruise=30.0, lead_observation_fn=observe,
|
||||
model_action_fn=lambda _current_time, _v_ego, _a_ego: (-2.0, False), actuator_delay=0.15, actuator_lag=0.20,
|
||||
)
|
||||
response = (trace.time >= dropout_start - DT_MDL) & (trace.time <= reacquisition_time + 0.5)
|
||||
|
||||
assert all(mode == "acc" for mode in trace.dec_mode)
|
||||
assert all(str(source) != "e2e" for source in trace.source)
|
||||
assert not trace.fcw.any()
|
||||
assert not _has_propulsion_brake_cycle(trace.a_target[response])
|
||||
assert np.max(np.abs(np.diff(trace.a_target[response]) / DT_MDL)) < 3.0
|
||||
assert trace.raw_radar_passthrough.all()
|
||||
assert np.all(trace.mpc_calls == 1)
|
||||
assert trace.solver_failures == 0
|
||||
|
||||
|
||||
def test_dec_uses_confirmed_model_slowdown_while_the_handoff_is_still_gentle():
|
||||
def model_action(current_time: float, _v_ego: float, _a_ego: float) -> tuple[float, bool]:
|
||||
if current_time < 1.0:
|
||||
return 0.0, False
|
||||
if current_time < 1.75:
|
||||
return -0.5 * (current_time - 1.0), False
|
||||
if current_time < 3.25:
|
||||
return -0.375, False
|
||||
return -0.375 - 0.5 * (current_time - 3.25), False
|
||||
|
||||
trace = _run(
|
||||
duration=4.5, controller_enabled=True, dec_enabled=True, e2e=True, lead_relevancy=False, speed=22.0,
|
||||
v_cruise=22.0, model_action_fn=model_action, actuator_delay=0.15, actuator_lag=0.20,
|
||||
)
|
||||
mode_changes = np.flatnonzero(np.asarray(trace.dec_mode)[1:] != np.asarray(trace.dec_mode)[:-1]) + 1
|
||||
response = trace.time >= 0.5
|
||||
|
||||
assert len(mode_changes) == 1
|
||||
assert trace.dec_mode[mode_changes[0]] == "blended"
|
||||
assert trace.time[mode_changes[0]] <= 1.20 + 1e-9
|
||||
assert -0.10 < trace.a_target[mode_changes[0]] < 0.0
|
||||
assert np.all(np.asarray(trace.dec_mode)[(trace.time >= 1.75) & (trace.time < 3.25)] == "blended")
|
||||
assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0
|
||||
assert not trace.fcw.any()
|
||||
assert trace.raw_radar_passthrough.all()
|
||||
assert np.all(trace.mpc_calls == 1)
|
||||
assert trace.solver_failures == 0
|
||||
|
||||
|
||||
def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed():
|
||||
traces = [
|
||||
_run(
|
||||
|
||||
Reference in New Issue
Block a user