mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-20 18:03:46 +08:00
feat(dec): preserve braking through radar and model handoffs
This commit is contained in:
rav4kumar
@@ -1,17 +1,48 @@
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
|
||||
|
||||
class WMACConstants:
|
||||
# Lead detection parameters
|
||||
LEAD_WINDOW_SIZE = 6 # Stable detection window
|
||||
LEAD_PROB = 0.45 # Balanced threshold for lead detection
|
||||
TRAJECTORY_SIZE = 33
|
||||
PARAM_READ_FRAMES = max(1, int(round(1.0 / DT_MDL)))
|
||||
|
||||
# Slow down detection parameters
|
||||
SLOW_DOWN_WINDOW_SIZE = 5 # Responsive but stable
|
||||
SLOW_DOWN_PROB = 0.3 # Balanced threshold for slow down scenarios
|
||||
EMERGENCY_HOLD_FRAMES = max(1, int(round(0.75 / DT_MDL)))
|
||||
MIN_MODE_DURATION = {'acc': max(1, int(round(0.6 / DT_MDL))), 'blended': max(1, int(round(0.5 / DT_MDL)))}
|
||||
ENTER_BLENDED_FRAMES = max(1, int(round(0.4 / DT_MDL)))
|
||||
EXIT_BLENDED_FRAMES = max(1, int(round(0.35 / DT_MDL)))
|
||||
STANDSTILL_FRAMES = max(1, int(round(0.2 / DT_MDL)))
|
||||
|
||||
# Optimized slow down distance curve - smooth and progressive
|
||||
LEAD_PROB = 0.45
|
||||
LEAD_EXIT_PROB = 0.25
|
||||
LEAD_RISE_RATE = 1.0
|
||||
LEAD_FALL_RATE = 0.35
|
||||
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
|
||||
SLOW_DOWN_RISE_RATE = 0.65
|
||||
SLOW_DOWN_FALL_RATE = 0.15
|
||||
SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
|
||||
SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.]
|
||||
URGENT_SLOW_DOWN_PROB = 0.85
|
||||
|
||||
# Slowness detection parameters
|
||||
SLOWNESS_WINDOW_SIZE = 10 # Stable slowness detection
|
||||
SLOWNESS_PROB = 0.55 # Clear threshold for slowness
|
||||
SLOWNESS_CRUISE_OFFSET = 1.025 # Conservative cruise speed offset
|
||||
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
|
||||
SPEED_URGENCY_MIN = 25.0
|
||||
SPEED_URGENCY_RANGE = 80.0
|
||||
|
||||
SLOWNESS_PROB = 0.55
|
||||
SLOWNESS_EXIT_PROB = 0.45
|
||||
SLOWNESS_RISE_RATE = 0.35
|
||||
SLOWNESS_FALL_RATE = 0.5
|
||||
SLOWNESS_CRUISE_OFFSET = 1.025
|
||||
|
||||
@@ -6,129 +6,119 @@ 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 opendbc.car import structs
|
||||
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
|
||||
from typing import Literal
|
||||
|
||||
# d-e2e, from modeldata.h
|
||||
TRAJECTORY_SIZE = 33
|
||||
SET_MODE_TIMEOUT = 15
|
||||
|
||||
# Define the valid mode types
|
||||
ModeType = Literal['acc', 'blended']
|
||||
|
||||
|
||||
class SmoothKalmanFilter:
|
||||
"""Enhanced Kalman filter with smoothing for stable decision making."""
|
||||
def clip01(value: float) -> float:
|
||||
return max(0.0, min(1.0, float(value)))
|
||||
|
||||
def __init__(self, initial_value=0, measurement_noise=0.1, process_noise=0.01,
|
||||
alpha=1.0, smoothing_factor=0.85):
|
||||
self.x = initial_value
|
||||
self.P = 1.0
|
||||
self.R = measurement_noise
|
||||
self.Q = process_noise
|
||||
self.alpha = alpha
|
||||
self.smoothing_factor = smoothing_factor
|
||||
self.initialized = False
|
||||
self.history = []
|
||||
self.max_history = 10
|
||||
self.confidence = 0.0
|
||||
|
||||
def add_data(self, measurement):
|
||||
if len(self.history) >= self.max_history:
|
||||
self.history.pop(0)
|
||||
self.history.append(measurement)
|
||||
class SmoothedSignal:
|
||||
def __init__(self, rise_rate: float, fall_rate: float, initial_value: float = 0.0):
|
||||
self.rise_rate = clip01(rise_rate)
|
||||
self.fall_rate = clip01(fall_rate)
|
||||
self.value = clip01(initial_value)
|
||||
|
||||
if not self.initialized:
|
||||
self.x = measurement
|
||||
self.initialized = True
|
||||
self.confidence = 0.1
|
||||
return
|
||||
def update(self, measurement: float) -> float:
|
||||
measurement = clip01(measurement)
|
||||
rate = self.rise_rate if measurement > self.value else self.fall_rate
|
||||
self.value += (measurement - self.value) * rate
|
||||
return self.value
|
||||
|
||||
self.P = self.alpha * self.P + self.Q
|
||||
def reset(self, value: float = 0.0) -> None:
|
||||
self.value = clip01(value)
|
||||
|
||||
K = self.P / (self.P + self.R)
|
||||
effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1
|
||||
|
||||
innovation = measurement - self.x
|
||||
self.x = self.x + effective_K * innovation
|
||||
self.P = (1 - effective_K) * self.P
|
||||
class HysteresisSignal:
|
||||
def __init__(self, enter_threshold: float, exit_threshold: float, rise_rate: float, fall_rate: float):
|
||||
self.enter_threshold = clip01(enter_threshold)
|
||||
self.exit_threshold = clip01(exit_threshold)
|
||||
self.filter = SmoothedSignal(rise_rate, fall_rate)
|
||||
self.active = False
|
||||
|
||||
if abs(innovation) < 0.1:
|
||||
self.confidence = min(1.0, self.confidence + 0.05)
|
||||
else:
|
||||
self.confidence = max(0.1, self.confidence - 0.02)
|
||||
def update(self, measurement: float) -> bool:
|
||||
value = self.filter.update(measurement)
|
||||
threshold = self.exit_threshold if self.active else self.enter_threshold
|
||||
self.active = value > threshold
|
||||
return self.active
|
||||
|
||||
def get_value(self):
|
||||
return self.x if self.initialized else None
|
||||
def reset(self) -> None:
|
||||
self.filter.reset()
|
||||
self.active = False
|
||||
|
||||
def get_confidence(self):
|
||||
return self.confidence
|
||||
|
||||
def reset_data(self):
|
||||
self.initialized = False
|
||||
self.history = []
|
||||
self.confidence = 0.0
|
||||
@property
|
||||
def value(self) -> float:
|
||||
return self.filter.value
|
||||
|
||||
|
||||
class ModeTransitionManager:
|
||||
"""Manages smooth transitions between driving modes with hysteresis."""
|
||||
|
||||
def __init__(self):
|
||||
self.current_mode: ModeType = 'acc'
|
||||
self.mode_confidence = {'acc': 1.0, 'blended': 0.0}
|
||||
self.transition_timeout = 0
|
||||
self.min_mode_duration = 10
|
||||
self.mode_duration = 0
|
||||
self.emergency_override = False
|
||||
self._pending_mode: ModeType = 'acc'
|
||||
self._pending_count = 0
|
||||
self._blended_hold_frames = 0
|
||||
|
||||
def request_mode(self, mode: ModeType, confidence: float = 1.0, emergency: bool = False):
|
||||
# Emergency override for critical situations (stops, collisions)
|
||||
if emergency:
|
||||
self.emergency_override = True
|
||||
self.current_mode = mode
|
||||
self.transition_timeout = SET_MODE_TIMEOUT
|
||||
self.mode_duration = 0
|
||||
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) if mode == 'blended' else 0
|
||||
self._pending_mode = mode
|
||||
self._pending_count = 0
|
||||
self._switch_mode(mode)
|
||||
return
|
||||
|
||||
self.mode_confidence[mode] = min(1.0, self.mode_confidence[mode] + 0.1 * confidence)
|
||||
for m in self.mode_confidence:
|
||||
if m != mode:
|
||||
self.mode_confidence[m] = max(0.0, self.mode_confidence[m] - 0.05)
|
||||
if cancel_hold and mode == 'acc':
|
||||
self._blended_hold_frames = 0
|
||||
|
||||
# Require minimum duration in current mode (unless emergency)
|
||||
if self.mode_duration < self.min_mode_duration and not self.emergency_override:
|
||||
if self._blended_hold_frames > 0:
|
||||
mode = 'blended'
|
||||
|
||||
if mode == self.current_mode:
|
||||
self._pending_mode = mode
|
||||
self._pending_count = 0
|
||||
return
|
||||
|
||||
# Hysteresis: higher threshold for mode changes
|
||||
confidence_threshold = 0.6 if mode != self.current_mode else 0.3 # Lower threshold for faster response
|
||||
if mode != self._pending_mode:
|
||||
self._pending_mode = mode
|
||||
self._pending_count = 1
|
||||
else:
|
||||
self._pending_count += 1
|
||||
|
||||
if self.mode_confidence[mode] > confidence_threshold:
|
||||
if mode != self.current_mode and self.transition_timeout == 0:
|
||||
self.transition_timeout = SET_MODE_TIMEOUT
|
||||
self.current_mode = mode
|
||||
self.mode_duration = 0
|
||||
if self.mode_duration < WMACConstants.MIN_MODE_DURATION[self.current_mode]:
|
||||
return
|
||||
|
||||
def update(self):
|
||||
if self.transition_timeout > 0:
|
||||
self.transition_timeout -= 1
|
||||
required_count = WMACConstants.ENTER_BLENDED_FRAMES if mode == 'blended' else WMACConstants.EXIT_BLENDED_FRAMES
|
||||
if self._pending_count >= required_count:
|
||||
self._switch_mode(mode)
|
||||
|
||||
def update(self) -> None:
|
||||
if self._blended_hold_frames > 0:
|
||||
self._blended_hold_frames -= 1
|
||||
self.mode_duration += 1
|
||||
|
||||
# Reset emergency override after some time
|
||||
if self.emergency_override and self.mode_duration > 20:
|
||||
self.emergency_override = False
|
||||
|
||||
# Gradual confidence decay
|
||||
for mode in self.mode_confidence:
|
||||
self.mode_confidence[mode] *= 0.98
|
||||
|
||||
def get_mode(self) -> ModeType:
|
||||
return self.current_mode
|
||||
|
||||
def _switch_mode(self, mode: ModeType) -> None:
|
||||
if mode == self.current_mode:
|
||||
return
|
||||
|
||||
self.current_mode = mode
|
||||
self.mode_duration = 0
|
||||
self._pending_mode = mode
|
||||
self._pending_count = 0
|
||||
|
||||
|
||||
class DynamicExperimentalController:
|
||||
def __init__(self, CP: structs.CarParams, mpc, params=None):
|
||||
@@ -142,35 +132,32 @@ class DynamicExperimentalController:
|
||||
|
||||
self._mode_manager = ModeTransitionManager()
|
||||
|
||||
# Smooth filters for stable decision making with faster response for critical scenarios
|
||||
self._lead_filter = SmoothKalmanFilter(
|
||||
measurement_noise=0.15,
|
||||
process_noise=0.05,
|
||||
alpha=1.02,
|
||||
smoothing_factor=0.8
|
||||
self._lead_tracker = HysteresisSignal(
|
||||
enter_threshold=WMACConstants.LEAD_PROB,
|
||||
exit_threshold=WMACConstants.LEAD_EXIT_PROB,
|
||||
rise_rate=WMACConstants.LEAD_RISE_RATE,
|
||||
fall_rate=WMACConstants.LEAD_FALL_RATE,
|
||||
)
|
||||
self._slow_down_tracker = HysteresisSignal(
|
||||
enter_threshold=WMACConstants.SLOW_DOWN_PROB,
|
||||
exit_threshold=WMACConstants.SLOW_DOWN_EXIT_PROB,
|
||||
rise_rate=WMACConstants.SLOW_DOWN_RISE_RATE,
|
||||
fall_rate=WMACConstants.SLOW_DOWN_FALL_RATE,
|
||||
)
|
||||
self._slowness_tracker = HysteresisSignal(
|
||||
enter_threshold=WMACConstants.SLOWNESS_PROB,
|
||||
exit_threshold=WMACConstants.SLOWNESS_EXIT_PROB,
|
||||
rise_rate=WMACConstants.SLOWNESS_RISE_RATE,
|
||||
fall_rate=WMACConstants.SLOWNESS_FALL_RATE,
|
||||
)
|
||||
|
||||
self._slow_down_filter = SmoothKalmanFilter(
|
||||
measurement_noise=0.1,
|
||||
process_noise=0.1,
|
||||
alpha=1.05,
|
||||
smoothing_factor=0.7
|
||||
)
|
||||
|
||||
self._slowness_filter = SmoothKalmanFilter(
|
||||
measurement_noise=0.1,
|
||||
process_noise=0.06,
|
||||
alpha=1.015,
|
||||
smoothing_factor=0.92
|
||||
)
|
||||
|
||||
self._mpc_fcw_filter = SmoothKalmanFilter(
|
||||
measurement_noise=0.2,
|
||||
process_noise=0.1,
|
||||
alpha=1.1,
|
||||
smoothing_factor=0.5
|
||||
)
|
||||
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
|
||||
@@ -179,13 +166,18 @@ class DynamicExperimentalController:
|
||||
self._has_standstill = False
|
||||
self._mpc_fcw_crash_cnt = 0
|
||||
self._standstill_count = 0
|
||||
# debug
|
||||
|
||||
self._endpoint_x = float('inf')
|
||||
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 % int(1. / DT_MDL) == 0:
|
||||
if self._frame % WMACConstants.PARAM_READ_FRAMES == 0:
|
||||
self._enabled = self._params.get_bool("DynamicExperimentalControl")
|
||||
|
||||
def mode(self) -> str:
|
||||
@@ -198,191 +190,221 @@ class DynamicExperimentalController:
|
||||
return self._active
|
||||
|
||||
def set_mpc_fcw_crash_cnt(self) -> None:
|
||||
"""Set MPC FCW crash count"""
|
||||
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
|
||||
self._v_cruise_kph = car_state.vCruise
|
||||
self._has_standstill = car_state.standstill
|
||||
|
||||
# standstill detection
|
||||
if self._has_standstill:
|
||||
self._standstill_count = min(20, self._standstill_count + 1)
|
||||
self._standstill_count = min(WMACConstants.STANDSTILL_FRAMES * 3, self._standstill_count + 1)
|
||||
else:
|
||||
self._standstill_count = max(0, self._standstill_count - 1)
|
||||
|
||||
# Lead detection
|
||||
self._lead_filter.add_data(float(lead_one.present))
|
||||
lead_value = self._lead_filter.get_value() or 0.0
|
||||
self._has_lead_filtered = lead_value > WMACConstants.LEAD_PROB
|
||||
|
||||
# MPC FCW detection
|
||||
fcw_filtered_value = self._mpc_fcw_filter.get_value() or 0.0
|
||||
self._mpc_fcw_filter.add_data(float(self._mpc_fcw_crash_cnt > 0))
|
||||
self._has_mpc_fcw = fcw_filtered_value > 0.5
|
||||
|
||||
# Slow down detection
|
||||
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)
|
||||
|
||||
# Slowness detection
|
||||
if not (self._standstill_count > 5) and not self._has_slow_down:
|
||||
if self._standstill_count > WMACConstants.STANDSTILL_FRAMES or self._has_slow_down:
|
||||
self._slowness_tracker.reset()
|
||||
self._has_slowness = False
|
||||
else:
|
||||
current_slowness = float(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET))
|
||||
self._slowness_filter.add_data(current_slowness)
|
||||
slowness_value = self._slowness_filter.get_value() or 0.0
|
||||
self._has_slowness = self._slowness_tracker.update(current_slowness)
|
||||
|
||||
# Hysteresis for slowness
|
||||
threshold = WMACConstants.SLOWNESS_PROB * (0.8 if self._has_slowness else 1.1)
|
||||
self._has_slowness = slowness_value > threshold
|
||||
|
||||
def _calculate_slow_down(self, md):
|
||||
"""Calculate urgency based on trajectory endpoint vs expected distance."""
|
||||
|
||||
# Reset to safe defaults
|
||||
urgency = 0.0
|
||||
def _calculate_slow_down(self, md) -> None:
|
||||
self._endpoint_x = float('inf')
|
||||
self._expected_distance = 0.0
|
||||
self._trajectory_valid = False
|
||||
|
||||
#Require exact trajectory size
|
||||
position_valid = len(md.position.x) == TRAJECTORY_SIZE
|
||||
orientation_valid = len(md.orientation.x) == TRAJECTORY_SIZE
|
||||
self._update_model_decel_trend(md)
|
||||
urgency = self._model_action_urgency(md)
|
||||
position_valid = len(md.position.x) == WMACConstants.TRAJECTORY_SIZE
|
||||
|
||||
if not (position_valid and orientation_valid):
|
||||
# Invalid trajectory - this itself might indicate a stop scenario
|
||||
# Apply moderate urgency for incomplete trajectories at speed
|
||||
if self._v_ego_kph > 20.0:
|
||||
urgency = 0.3
|
||||
if position_valid:
|
||||
self._trajectory_valid = True
|
||||
self._endpoint_x = md.position.x[WMACConstants.TRAJECTORY_SIZE - 1]
|
||||
self._expected_distance = interp(self._v_ego_kph, WMACConstants.SLOW_DOWN_BP, WMACConstants.SLOW_DOWN_DIST)
|
||||
urgency = max(urgency, self._endpoint_urgency(self._endpoint_x, self._expected_distance))
|
||||
|
||||
self._slow_down_filter.add_data(urgency)
|
||||
urgency_filtered = self._slow_down_filter.get_value() or 0.0
|
||||
self._has_slow_down = urgency_filtered > WMACConstants.SLOW_DOWN_PROB
|
||||
self._urgency = urgency_filtered
|
||||
self._raw_urgency = clip01(urgency)
|
||||
self._has_slow_down = self._slow_down_tracker.update(self._raw_urgency)
|
||||
self._urgency = self._slow_down_tracker.value
|
||||
|
||||
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
|
||||
|
||||
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
|
||||
|
||||
# We have a valid full trajectory
|
||||
self._trajectory_valid = True
|
||||
if not self._has_any_lead:
|
||||
self._radar_acc_lead_frames = min(self._radar_acc_lead_frames, WMACConstants.RADAR_LEAD_DROPOUT_FRAMES)
|
||||
|
||||
# Use the exact endpoint (33rd point, index 32)
|
||||
endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
|
||||
self._endpoint_x = endpoint_x
|
||||
self._has_radar_acc_lead = self._radar_acc_lead_frames > 0
|
||||
self._radar_acc_lead_frames = max(0, self._radar_acc_lead_frames - 1)
|
||||
|
||||
# Get expected distance based on current speed using tuned constants
|
||||
expected_distance = interp(self._v_ego_kph,
|
||||
WMACConstants.SLOW_DOWN_BP,
|
||||
WMACConstants.SLOW_DOWN_DIST)
|
||||
self._expected_distance = expected_distance
|
||||
def _model_action_urgency(self, md) -> float:
|
||||
action = getattr(md, 'action', None)
|
||||
if action is None:
|
||||
return 0.0
|
||||
|
||||
# Calculate urgency based on trajectory shortage
|
||||
if endpoint_x < expected_distance:
|
||||
shortage = expected_distance - endpoint_x
|
||||
shortage_ratio = shortage / expected_distance
|
||||
urgency = 1.0 if getattr(action, 'shouldStop', False) else 0.0
|
||||
desired_accel = getattr(action, 'desiredAcceleration', 0.0)
|
||||
if desired_accel < WMACConstants.MODEL_DECEL_START:
|
||||
urgency = max(urgency, min(1.0, (WMACConstants.MODEL_DECEL_START - desired_accel) / WMACConstants.MODEL_DECEL_RANGE))
|
||||
return urgency
|
||||
|
||||
# Base urgency on shortage ratio
|
||||
urgency = min(1.0, shortage_ratio * 2.0)
|
||||
def _endpoint_urgency(self, endpoint_x: float, expected_distance: float) -> float:
|
||||
if endpoint_x >= expected_distance:
|
||||
return 0.0
|
||||
|
||||
# Increase urgency for very short trajectories (imminent stops)
|
||||
critical_distance = expected_distance * 0.3
|
||||
if endpoint_x < critical_distance:
|
||||
urgency = min(1.0, urgency * 2.0)
|
||||
shortage_ratio = (expected_distance - endpoint_x) / expected_distance
|
||||
urgency = min(1.0, shortage_ratio * WMACConstants.ENDPOINT_URGENCY_GAIN)
|
||||
|
||||
# Speed-based urgency adjustment
|
||||
if self._v_ego_kph > 25.0:
|
||||
speed_factor = 1.0 + (self._v_ego_kph - 25.0) / 80.0
|
||||
urgency = min(1.0, urgency * speed_factor)
|
||||
if endpoint_x < expected_distance * WMACConstants.CRITICAL_ENDPOINT_FACTOR:
|
||||
urgency = min(1.0, urgency * WMACConstants.CRITICAL_URGENCY_GAIN)
|
||||
|
||||
# Apply filtering but with less smoothing for stops
|
||||
self._slow_down_filter.add_data(urgency)
|
||||
urgency_filtered = self._slow_down_filter.get_value() or 0.0
|
||||
if self._v_ego_kph > WMACConstants.SPEED_URGENCY_MIN:
|
||||
speed_factor = 1.0 + (self._v_ego_kph - WMACConstants.SPEED_URGENCY_MIN) / WMACConstants.SPEED_URGENCY_RANGE
|
||||
urgency = min(1.0, urgency * speed_factor)
|
||||
|
||||
# Update state with lower threshold for better stop detection
|
||||
self._has_slow_down = urgency_filtered > (WMACConstants.SLOW_DOWN_PROB * 0.8)
|
||||
self._urgency = urgency_filtered
|
||||
return urgency
|
||||
|
||||
def _radarless_mode(self) -> None:
|
||||
"""Radarless mode decision logic with emergency handling."""
|
||||
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 _lead_dropout_model_handoff_ready(self) -> bool:
|
||||
if (not self._active or self._CP.radarUnavailable or not self._radar_fresh or self._has_any_lead or not self._has_radar_acc_lead
|
||||
or self._mode_manager.get_mode() != 'acc' or self._mpc.last_solution_status != 0):
|
||||
return False
|
||||
try:
|
||||
model_accel = float(self._model_accel_samples[-1])
|
||||
except (IndexError, OverflowError, TypeError, ValueError):
|
||||
return False
|
||||
return (math.isfinite(model_accel) and math.isfinite(self._planner_accel)
|
||||
and self._planner_accel <= WMACConstants.MODEL_DECEL_START
|
||||
and model_accel <= WMACConstants.MODEL_DECEL_START
|
||||
and model_accel <= self._planner_accel + WMACConstants.MODEL_DECEL_TREND_MAX_COMMAND_STEP)
|
||||
|
||||
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 self._lead_dropout_model_handoff_ready():
|
||||
self._radar_acc_lead_frames = 0
|
||||
self._has_radar_acc_lead = False
|
||||
self._model_decel_latched = True
|
||||
return 'blended', True
|
||||
|
||||
if not self._CP.radarUnavailable and self._has_radar_acc_lead:
|
||||
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
|
||||
|
||||
# EMERGENCY: MPC FCW - immediate blended mode
|
||||
if self._has_mpc_fcw:
|
||||
self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
|
||||
return
|
||||
|
||||
# Standstill: use blended
|
||||
if self._standstill_count > 3:
|
||||
self._mode_manager.request_mode('blended', confidence=0.9)
|
||||
return
|
||||
|
||||
# Slow down scenarios: emergency for high urgency, normal for lower urgency
|
||||
if self._has_slow_down:
|
||||
if self._urgency > 0.7:
|
||||
# Emergency: immediate blended mode for high urgency stops
|
||||
self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
|
||||
else:
|
||||
# Normal: blended with urgency-based confidence
|
||||
confidence = min(1.0, self._urgency * 1.5)
|
||||
self._mode_manager.request_mode('blended', confidence=confidence)
|
||||
return
|
||||
|
||||
# Driving slow: use ACC (but not if actively slowing down)
|
||||
if self._has_slowness and not self._has_slow_down:
|
||||
self._mode_manager.request_mode('acc', confidence=0.8)
|
||||
return
|
||||
|
||||
# Default: ACC
|
||||
self._mode_manager.request_mode('acc', confidence=0.7)
|
||||
|
||||
def _radar_mode(self) -> None:
|
||||
"""Radar mode with emergency handling."""
|
||||
|
||||
# EMERGENCY: MPC FCW - immediate blended mode
|
||||
if self._has_mpc_fcw:
|
||||
self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
|
||||
return
|
||||
|
||||
# If lead detected and not in standstill: always use ACC
|
||||
if self._has_lead_filtered and not (self._standstill_count > 3):
|
||||
self._mode_manager.request_mode('acc', confidence=1.0)
|
||||
return
|
||||
|
||||
# Slow down scenarios: emergency for high urgency, normal for lower urgency
|
||||
if self._has_slow_down:
|
||||
if self._urgency > 0.7:
|
||||
# Emergency: immediate blended mode for high urgency stops
|
||||
self._mode_manager.request_mode('blended', confidence=1.0, emergency=True)
|
||||
else:
|
||||
# Normal: blended with urgency-based confidence
|
||||
confidence = min(1.0, self._urgency * 1.3)
|
||||
self._mode_manager.request_mode('blended', confidence=confidence)
|
||||
return
|
||||
|
||||
# Standstill: use blended
|
||||
if self._standstill_count > 3:
|
||||
self._mode_manager.request_mode('blended', confidence=0.9)
|
||||
return
|
||||
|
||||
# Driving slow: use ACC (but not if actively slowing down)
|
||||
if self._has_slowness and not self._has_slow_down:
|
||||
self._mode_manager.request_mode('acc', confidence=0.8)
|
||||
return
|
||||
|
||||
# Default: ACC
|
||||
self._mode_manager.request_mode('acc', confidence=0.7)
|
||||
|
||||
def update(self, sm: messaging.SubMaster) -> None:
|
||||
self._read_params()
|
||||
|
||||
self.set_mpc_fcw_crash_cnt()
|
||||
|
||||
self._update_calculations(sm)
|
||||
return 'blended', True
|
||||
|
||||
if self._CP.radarUnavailable:
|
||||
self._radarless_mode()
|
||||
else:
|
||||
self._radar_mode()
|
||||
if standstill or self._has_slow_down:
|
||||
return 'blended', urgent_slow_down
|
||||
return 'acc', False
|
||||
|
||||
self._mode_manager.update()
|
||||
if standstill or self._has_slow_down:
|
||||
return 'blended', urgent_slow_down
|
||||
|
||||
return 'acc', False
|
||||
|
||||
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()
|
||||
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=not self._CP.radarUnavailable and self._has_radar_acc_lead)
|
||||
self._mode_manager.update()
|
||||
|
||||
self._frame += 1
|
||||
|
||||
@@ -1,94 +0,0 @@
|
||||
import pytest
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
|
||||
|
||||
class MockLeadOne:
|
||||
def __init__(self, status=0.0):
|
||||
self.status = status
|
||||
|
||||
class MockRadarState:
|
||||
def __init__(self, status=0.0):
|
||||
self.leadOne = MockLeadOne(status=status)
|
||||
|
||||
class MockCarState:
|
||||
def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
|
||||
self.vEgo = vEgo
|
||||
self.vCruise = vCruise
|
||||
self.standstill = standstill
|
||||
|
||||
class MockModelData:
|
||||
def __init__(self, valid=True):
|
||||
size = 33 if valid else 10 # incomplete if invalid
|
||||
self.position = type("Pos", (), {"x": [0.0] * size})()
|
||||
self.orientation = type("Ori", (), {"x": [0.0] * size})()
|
||||
|
||||
class MockSelfDriveState:
|
||||
def __init__(self, experimentalMode=False):
|
||||
self.experimentalMode = experimentalMode
|
||||
|
||||
class MockParams:
|
||||
def get_bool(self, name):
|
||||
return True
|
||||
|
||||
@pytest.fixture
|
||||
def default_sm():
|
||||
sm = {
|
||||
'carState': MockCarState(vEgo=10.0, vCruise=20.0),
|
||||
'radarState': MockRadarState(status=1.0),
|
||||
'modelV2': MockModelData(valid=True),
|
||||
'selfdriveState': MockSelfDriveState(experimentalMode=True),
|
||||
}
|
||||
return sm
|
||||
|
||||
@pytest.fixture
|
||||
def mock_cp():
|
||||
class CP:
|
||||
radarUnavailable = False
|
||||
return CP()
|
||||
|
||||
@pytest.fixture
|
||||
def mock_mpc():
|
||||
class MPC:
|
||||
crash_cnt = 0
|
||||
return MPC()
|
||||
|
||||
# Fake Kalman Filter that always returns a given value
|
||||
class FakeKalman:
|
||||
def __init__(self, value=1.0):
|
||||
self.value = value
|
||||
def add_data(self, v): pass
|
||||
def get_value(self): return self.value
|
||||
def get_confidence(self): return 1.0
|
||||
def reset_data(self): pass
|
||||
|
||||
def test_initial_mode_is_acc(mock_cp, mock_mpc):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['carState'].standstill = True
|
||||
for _ in range(10):
|
||||
controller.update(default_sm)
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
mock_mpc.crash_cnt = 1 # simulate FCW
|
||||
for _ in range(2):
|
||||
controller.update(default_sm)
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm):
|
||||
mock_cp.radarUnavailable = True
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
|
||||
# Force conditions to simulate slowdown
|
||||
controller._slow_down_filter = FakeKalman(value=1.0) # ty: ignore[invalid-assignment]
|
||||
controller._v_ego_kph = 35.0
|
||||
default_sm['modelV2'] = MockModelData(valid=False) # Incomplete trajectory
|
||||
|
||||
for _ in range(3):
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller.mode() == "blended"
|
||||
@@ -0,0 +1,685 @@
|
||||
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, 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, 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:
|
||||
def __init__(self, vEgo=0.0, vCruise=0.0, standstill=False):
|
||||
self.vEgo = vEgo
|
||||
self.vCruise = vCruise
|
||||
self.standstill = standstill
|
||||
|
||||
|
||||
class MockAction:
|
||||
def __init__(self, desiredAcceleration=0.0, shouldStop=False):
|
||||
self.desiredAcceleration = desiredAcceleration
|
||||
self.shouldStop = shouldStop
|
||||
|
||||
|
||||
class MockModelData:
|
||||
def __init__(self, valid=True, endpoint_x=200.0, orientation_valid=None, desired_acceleration=0.0, should_stop=False):
|
||||
position_size = 33 if valid else 10
|
||||
orientation_size = position_size if orientation_valid is None else (33 if orientation_valid else 10)
|
||||
position_x = [0.0] * position_size
|
||||
if position_x:
|
||||
position_x[-1] = endpoint_x
|
||||
self.position = type("Pos", (), {"x": position_x})()
|
||||
self.orientation = type("Ori", (), {"x": [0.0] * orientation_size})()
|
||||
self.acceleration = type("Accel", (), {"x": [0.0] * position_size})()
|
||||
self.action = MockAction(desired_acceleration, should_stop)
|
||||
|
||||
|
||||
class MockSelfDriveState:
|
||||
def __init__(self, experimentalMode=False):
|
||||
self.experimentalMode = experimentalMode
|
||||
|
||||
|
||||
class MockParams:
|
||||
def get_bool(self, name):
|
||||
return True
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def default_sm():
|
||||
sm = {
|
||||
'carState': MockCarState(vEgo=10.0, vCruise=20.0),
|
||||
'radarState': MockRadarState(status=1.0, radar=True, radarTrackId=7),
|
||||
'modelV2': MockModelData(valid=True),
|
||||
'selfdriveState': MockSelfDriveState(experimentalMode=True),
|
||||
}
|
||||
return sm
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def mock_cp():
|
||||
class CP:
|
||||
radarUnavailable = False
|
||||
return CP()
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def mock_mpc():
|
||||
class MPC:
|
||||
crash_cnt = 0
|
||||
a_solution = [0.0, 0.0]
|
||||
last_solution_status = 0
|
||||
return MPC()
|
||||
|
||||
|
||||
def test_initial_mode_is_acc(mock_cp, mock_mpc):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_standstill_triggers_blended(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
default_sm['carState'].standstill = True
|
||||
for _ in range(20):
|
||||
controller.update(default_sm)
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_emergency_blended_on_fcw(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
mock_mpc.crash_cnt = 1
|
||||
controller.update(default_sm)
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_radarless_slowdown_triggers_blended(mock_cp, mock_mpc, default_sm):
|
||||
mock_cp.radarUnavailable = True
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
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"
|
||||
|
||||
|
||||
def test_valid_position_with_missing_orientation_can_trigger_slowdown(mock_cp, mock_mpc, default_sm):
|
||||
mock_cp.radarUnavailable = True
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0, orientation_valid=False)
|
||||
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller._trajectory_valid
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
def test_incomplete_position_does_not_trigger_slowdown(mock_cp, mock_mpc, default_sm):
|
||||
mock_cp.radarUnavailable = True
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
default_sm['modelV2'] = MockModelData(valid=False, endpoint_x=0.0)
|
||||
|
||||
for _ in range(3):
|
||||
controller.update(default_sm)
|
||||
|
||||
assert not controller._trajectory_valid
|
||||
assert not controller._has_slow_down
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_slowdown_hysteresis_prevents_threshold_chatter():
|
||||
signal = HysteresisSignal(enter_threshold=0.5, exit_threshold=0.4, rise_rate=1.0, fall_rate=1.0)
|
||||
|
||||
assert signal.update(0.55)
|
||||
assert signal.update(0.45)
|
||||
assert not signal.update(0.35)
|
||||
|
||||
|
||||
def test_model_should_stop_triggers_blended_without_valid_trajectory(mock_cp, mock_mpc, default_sm):
|
||||
mock_cp.radarUnavailable = True
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
default_sm['modelV2'] = MockModelData(valid=False, should_stop=True)
|
||||
|
||||
controller.update(default_sm)
|
||||
|
||||
assert not controller._trajectory_valid
|
||||
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, radar=True, radarTrackId=7)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
|
||||
for _ in range(3):
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller._has_slow_down
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
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, radar=True)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller._has_lead_filtered
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
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=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=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, radarTrackId=7)
|
||||
default_sm['modelV2'] = MockModelData(valid=True, endpoint_x=0.0)
|
||||
|
||||
for _ in range(20):
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
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, 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
|
||||
|
||||
for _ in range(10):
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller._has_lead_filtered
|
||||
assert controller._has_mpc_fcw
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
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, 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)
|
||||
controller.update(default_sm)
|
||||
|
||||
assert controller._has_lead_filtered
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_braking_model_takes_over_on_the_first_fresh_full_lead_dropout(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-1.1)
|
||||
controller.update(default_sm, planner_accel=-1.2)
|
||||
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
controller.update(default_sm, planner_accel=-1.2)
|
||||
|
||||
assert not controller._has_radar_acc_lead
|
||||
assert controller._model_decel_latched
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("model_accel", "planner_accel"), ((-0.75, -1.1), (-0.3, -1.1), (-1.1, -0.3)))
|
||||
def test_lead_dropout_guard_stays_active_without_a_matching_brake(model_accel, planner_accel, mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
controller.update(default_sm, planner_accel=-1.1)
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=model_accel)
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
|
||||
controller.update(default_sm, planner_accel=planner_accel)
|
||||
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_failed_mpc_keeps_the_lead_dropout_guard(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
controller.update(default_sm, planner_accel=-1.1)
|
||||
mock_mpc.last_solution_status = 1
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-1.1)
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
|
||||
controller.update(default_sm, planner_accel=-1.1)
|
||||
|
||||
assert controller._has_radar_acc_lead
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
def test_matching_brake_without_a_prior_radar_lead_does_not_use_dropout_handoff(mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['radarState'] = MockRadarState(status=0.0)
|
||||
default_sm['modelV2'] = MockModelData(valid=False, desired_acceleration=-1.1)
|
||||
|
||||
controller.update(default_sm, planner_accel=-1.1)
|
||||
|
||||
assert not controller._has_radar_acc_lead
|
||||
assert not controller._model_decel_latched
|
||||
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"
|
||||
@@ -136,7 +136,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:
|
||||
|
||||
+55
@@ -397,6 +397,61 @@ def test_positive_e2e_to_acc_handoff_starts_from_previous_plan(actuator_delay, a
|
||||
assert plant.planner.mpc.last_solution_status == 0
|
||||
|
||||
|
||||
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.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.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