mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-21 13:33:47 +08:00
dec: rewrite acc/blended
This commit is contained in:
rav4kumar
@@ -209,6 +209,10 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
state @0 :DynamicExperimentalControlState;
|
||||
enabled @1 :Bool;
|
||||
active @2 :Bool;
|
||||
decelIntent @3 :Float32;
|
||||
curveDetected @4 :Bool;
|
||||
wantBlended @5 :Bool;
|
||||
leadVeto @6 :Bool;
|
||||
|
||||
enum DynamicExperimentalControlState {
|
||||
acc @0;
|
||||
|
||||
@@ -132,6 +132,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.output_a_target)
|
||||
self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality)
|
||||
self.update_dec(sm)
|
||||
|
||||
self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
|
||||
self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
|
||||
|
||||
@@ -1,17 +0,0 @@
|
||||
class WMACConstants:
|
||||
# Lead detection parameters
|
||||
LEAD_WINDOW_SIZE = 6 # Stable detection window
|
||||
LEAD_PROB = 0.45 # Balanced threshold for lead detection
|
||||
|
||||
# Slow down detection parameters
|
||||
SLOW_DOWN_WINDOW_SIZE = 5 # Responsive but stable
|
||||
SLOW_DOWN_PROB = 0.3 # Balanced threshold for slow down scenarios
|
||||
|
||||
# Optimized slow down distance curve - smooth and progressive
|
||||
SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.]
|
||||
SLOW_DOWN_DIST = [32., 46., 64., 86., 108., 130., 145., 165.]
|
||||
|
||||
# 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
|
||||
@@ -4,192 +4,116 @@ Copyright (c) 2021-, rav4kumar, sunnypilot, and a number of other contributors.
|
||||
This file is part of sunnypilot and is licensed under the MIT License.
|
||||
See the LICENSE.md file in the root directory for more details.
|
||||
"""
|
||||
# Version = 2025-6-30
|
||||
from dataclasses import dataclass
|
||||
from typing import Literal
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.cereal import messaging
|
||||
from opendbc.car import structs
|
||||
from numpy import interp
|
||||
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
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
|
||||
# d-e2e, from modeldata.h
|
||||
TRAJECTORY_SIZE = 33
|
||||
SET_MODE_TIMEOUT = 15
|
||||
|
||||
# Define the valid mode types
|
||||
ModeType = Literal['acc', 'blended']
|
||||
|
||||
_DECEL_LOOKAHEAD_MIN_T = 1.0
|
||||
_DECEL_LOOKAHEAD_MAX_T = 6.0
|
||||
_T_IDXS = np.array(ModelConstants.T_IDXS)
|
||||
_DECEL_IDX = np.where((_T_IDXS >= _DECEL_LOOKAHEAD_MIN_T) & (_T_IDXS <= _DECEL_LOOKAHEAD_MAX_T))[0]
|
||||
_DECEL_INV_T = 1.0 / _T_IDXS[_DECEL_IDX]
|
||||
|
||||
class SmoothKalmanFilter:
|
||||
"""Enhanced Kalman filter with smoothing for stable decision making."""
|
||||
DECEL_INTENT_A_HINT = 0.35
|
||||
DECEL_INTENT_A_FULL = 1.30
|
||||
DECEL_INTENT_TRIGGER = 0.5
|
||||
|
||||
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
|
||||
CURVE_Y_MAX = 5.0
|
||||
|
||||
def add_data(self, measurement):
|
||||
if len(self.history) >= self.max_history:
|
||||
self.history.pop(0)
|
||||
self.history.append(measurement)
|
||||
LEAD_FUTURE_PROB_VANISH = 0.35
|
||||
|
||||
if not self.initialized:
|
||||
self.x = measurement
|
||||
self.initialized = True
|
||||
self.confidence = 0.1
|
||||
return
|
||||
MODEL_DROP_TRUST_FULL = 5.0
|
||||
MODEL_DROP_TRUST_NONE = 30.0
|
||||
MODEL_TRUST_MIN = 0.5
|
||||
|
||||
self.P = self.alpha * self.P + self.Q
|
||||
CREEP_SPEED_ENTER = 2.0
|
||||
CREEP_SPEED_EXIT = 3.0
|
||||
|
||||
K = self.P / (self.P + self.R)
|
||||
effective_K = K * (1.0 - self.smoothing_factor) + self.smoothing_factor * 0.1
|
||||
ENTER_FRAMES = 3
|
||||
EXIT_FRAMES = 16
|
||||
MIN_BLENDED_FRAMES = 20
|
||||
|
||||
innovation = measurement - self.x
|
||||
self.x = self.x + effective_K * innovation
|
||||
self.P = (1 - effective_K) * self.P
|
||||
|
||||
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 get_value(self):
|
||||
return self.x if self.initialized else None
|
||||
|
||||
def get_confidence(self):
|
||||
return self.confidence
|
||||
|
||||
def reset_data(self):
|
||||
self.initialized = False
|
||||
self.history = []
|
||||
self.confidence = 0.0
|
||||
PARAM_READ_FRAMES = 5
|
||||
|
||||
|
||||
class ModeTransitionManager:
|
||||
"""Manages smooth transitions between driving modes with hysteresis."""
|
||||
@dataclass
|
||||
class DecSignals:
|
||||
decel_intent: float = 0.0
|
||||
curve_detected: bool = False
|
||||
model_trust: float = 1.0
|
||||
creeping: bool = False
|
||||
|
||||
|
||||
def should_blend(s: DecSignals) -> bool:
|
||||
degraded = s.model_trust < MODEL_TRUST_MIN
|
||||
slowdown_detected = not degraded and s.decel_intent >= DECEL_INTENT_TRIGGER and not s.curve_detected
|
||||
return slowdown_detected or s.creeping
|
||||
|
||||
|
||||
class ModeHysteresis:
|
||||
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.mode: ModeType = 'acc'
|
||||
self.above = 0
|
||||
self.below = 0
|
||||
self.blended_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
|
||||
return
|
||||
def update(self, want_blended: bool, override: bool, veto: bool) -> ModeType:
|
||||
self.above = self.above + 1 if want_blended else 0
|
||||
self.below = 0 if want_blended else self.below + 1
|
||||
|
||||
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 override:
|
||||
self.mode, self.blended_frames = 'blended', 0
|
||||
elif veto:
|
||||
self.mode = 'acc'
|
||||
elif self.mode == 'acc':
|
||||
if self.above >= ENTER_FRAMES:
|
||||
self.mode, self.blended_frames = 'blended', 0
|
||||
else:
|
||||
self.blended_frames += 1
|
||||
if self.blended_frames >= MIN_BLENDED_FRAMES and self.below >= EXIT_FRAMES:
|
||||
self.mode = 'acc'
|
||||
return self.mode
|
||||
|
||||
# Require minimum duration in current mode (unless emergency)
|
||||
if self.mode_duration < self.min_mode_duration and not self.emergency_override:
|
||||
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 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
|
||||
|
||||
def update(self):
|
||||
if self.transition_timeout > 0:
|
||||
self.transition_timeout -= 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 reset(self) -> None:
|
||||
self.mode = 'acc'
|
||||
self.above = 0
|
||||
self.below = 0
|
||||
self.blended_frames = 0
|
||||
|
||||
|
||||
class DynamicExperimentalController:
|
||||
def __init__(self, CP: structs.CarParams, mpc, params=None):
|
||||
self._CP = CP
|
||||
self._mpc = mpc
|
||||
self._params = params or Params()
|
||||
self._enabled: bool = self._params.get_bool("DynamicExperimentalControl")
|
||||
self._active: bool = False
|
||||
self._frame: int = 0
|
||||
self._urgency = 0.0
|
||||
|
||||
self._mode_manager = ModeTransitionManager()
|
||||
self._hysteresis = ModeHysteresis()
|
||||
self._creeping = False
|
||||
|
||||
# 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.signals = DecSignals()
|
||||
self.want_blended = False
|
||||
self.lead_veto = False
|
||||
|
||||
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_slow_down = False
|
||||
self._has_slowness = False
|
||||
self._has_mpc_fcw = False
|
||||
self._v_ego_kph = 0.0
|
||||
self._v_cruise_kph = 0.0
|
||||
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
|
||||
def _update_creeping(self, v_ego: float) -> bool:
|
||||
self._creeping = v_ego < CREEP_SPEED_EXIT if self._creeping else v_ego <= CREEP_SPEED_ENTER
|
||||
return self._creeping
|
||||
|
||||
def _read_params(self) -> None:
|
||||
if self._frame % int(1. / DT_MDL) == 0:
|
||||
if self._frame % PARAM_READ_FRAMES == 0:
|
||||
self._enabled = self._params.get_bool("DynamicExperimentalControl")
|
||||
|
||||
def mode(self) -> str:
|
||||
return self._mode_manager.get_mode()
|
||||
return self._hysteresis.mode
|
||||
|
||||
def enabled(self) -> bool:
|
||||
return self._enabled
|
||||
@@ -197,192 +121,61 @@ class DynamicExperimentalController:
|
||||
def active(self) -> bool:
|
||||
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
|
||||
@staticmethod
|
||||
def _decel_intent(md) -> float:
|
||||
v = np.asarray(md.velocity.x)
|
||||
if len(v) != len(_T_IDXS):
|
||||
return 0.0
|
||||
a_req = float(np.min((v[_DECEL_IDX] - v[0]) * _DECEL_INV_T))
|
||||
return float(np.interp(-a_req, [DECEL_INTENT_A_HINT, DECEL_INTENT_A_FULL], [0.0, 1.0]))
|
||||
|
||||
def _update_calculations(self, sm: messaging.SubMaster) -> None:
|
||||
car_state = sm['carState']
|
||||
lead_one = sm['radarState'].leadOne
|
||||
md = sm['modelV2']
|
||||
@staticmethod
|
||||
def _curve_detected(md) -> bool:
|
||||
y = md.position.y
|
||||
if len(y) < 1:
|
||||
return False
|
||||
return abs(y[-1]) >= CURVE_Y_MAX
|
||||
|
||||
self._v_ego_kph = car_state.vEgo * 3.6
|
||||
self._v_cruise_kph = car_state.vCruise
|
||||
self._has_standstill = car_state.standstill
|
||||
@staticmethod
|
||||
def _model_trust(md) -> float:
|
||||
if len(md.velocity.x) != len(_T_IDXS):
|
||||
return 0.0
|
||||
return float(np.interp(md.frameDropPerc, [MODEL_DROP_TRUST_FULL, MODEL_DROP_TRUST_NONE], [1.0, 0.0]))
|
||||
|
||||
# standstill detection
|
||||
if self._has_standstill:
|
||||
self._standstill_count = min(20, 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._calculate_slow_down(md)
|
||||
|
||||
# Slowness detection
|
||||
if not (self._standstill_count > 5) and not self._has_slow_down:
|
||||
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
|
||||
|
||||
# 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
|
||||
self._endpoint_x = float('inf')
|
||||
self._trajectory_valid = False
|
||||
|
||||
#Require exact trajectory size
|
||||
position_valid = len(md.position.x) == TRAJECTORY_SIZE
|
||||
orientation_valid = len(md.orientation.x) == 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
|
||||
|
||||
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
|
||||
return
|
||||
|
||||
# We have a valid full trajectory
|
||||
self._trajectory_valid = True
|
||||
|
||||
# Use the exact endpoint (33rd point, index 32)
|
||||
endpoint_x = md.position.x[TRAJECTORY_SIZE - 1]
|
||||
self._endpoint_x = endpoint_x
|
||||
|
||||
# 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
|
||||
|
||||
# Calculate urgency based on trajectory shortage
|
||||
if endpoint_x < expected_distance:
|
||||
shortage = expected_distance - endpoint_x
|
||||
shortage_ratio = shortage / expected_distance
|
||||
|
||||
# Base urgency on shortage ratio
|
||||
urgency = min(1.0, shortage_ratio * 2.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)
|
||||
|
||||
# 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)
|
||||
|
||||
# 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
|
||||
|
||||
# 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
|
||||
|
||||
def _radarless_mode(self) -> None:
|
||||
"""Radarless mode decision logic 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
|
||||
|
||||
# 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)
|
||||
@staticmethod
|
||||
def _lead_veto(radar_state, md) -> bool:
|
||||
lead_one, lead_two = radar_state.leadOne, radar_state.leadTwo
|
||||
lead_now = lead_one.present or lead_two.present
|
||||
probs = md.leadsV3
|
||||
future = min(probs[1].prob, probs[2].prob) if len(probs) >= 3 else 1.0
|
||||
return bool(lead_now and future > LEAD_FUTURE_PROB_VANISH)
|
||||
|
||||
def update(self, sm: messaging.SubMaster) -> None:
|
||||
self._read_params()
|
||||
|
||||
self.set_mpc_fcw_crash_cnt()
|
||||
car_state = sm['carState']
|
||||
md = sm['modelV2']
|
||||
radar_state = sm['radarState']
|
||||
|
||||
self._update_calculations(sm)
|
||||
is_creeping = self._update_creeping(car_state.vEgo)
|
||||
self.lead_veto = self._lead_veto(radar_state, md)
|
||||
|
||||
if self._CP.radarUnavailable:
|
||||
self._radarless_mode()
|
||||
self.signals = DecSignals(
|
||||
decel_intent=self._decel_intent(md),
|
||||
curve_detected=self._curve_detected(md),
|
||||
model_trust=self._model_trust(md),
|
||||
creeping=is_creeping,
|
||||
)
|
||||
self.want_blended = should_blend(self.signals)
|
||||
|
||||
crash_override = self._mpc.crash_cnt >= 1
|
||||
hard_brake_override = bool(md.meta.hardBrakePredicted)
|
||||
override = (crash_override or hard_brake_override) and not self.lead_veto
|
||||
|
||||
if self._enabled:
|
||||
self._hysteresis.update(self.want_blended, override, self.lead_veto)
|
||||
else:
|
||||
self._radar_mode()
|
||||
self._hysteresis.reset()
|
||||
|
||||
self._mode_manager.update()
|
||||
self._active = sm['selfdriveState'].experimentalMode and self._enabled
|
||||
self._frame += 1
|
||||
|
||||
@@ -1,91 +1,285 @@
|
||||
import numpy as np
|
||||
|
||||
from openpilot.cereal import messaging
|
||||
from opendbc.car import structs
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import (
|
||||
DecSignals,
|
||||
DynamicExperimentalController,
|
||||
ModeHysteresis,
|
||||
should_blend,
|
||||
ENTER_FRAMES,
|
||||
MIN_BLENDED_FRAMES,
|
||||
)
|
||||
|
||||
class MockLeadOne:
|
||||
def __init__(self, present=0.0):
|
||||
self.present = present
|
||||
T_IDXS = np.array(ModelConstants.T_IDXS)
|
||||
|
||||
class MockRadarState:
|
||||
def __init__(self, present=0.0):
|
||||
self.leadOne = MockLeadOne(present=present)
|
||||
|
||||
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 __init__(self, enabled=True):
|
||||
self._enabled = enabled
|
||||
|
||||
def get_bool(self, name):
|
||||
return True
|
||||
return self._enabled
|
||||
|
||||
def default_sm():
|
||||
sm = {
|
||||
'carState': MockCarState(vEgo=10.0, vCruise=20.0),
|
||||
'radarState': MockRadarState(present=1.0),
|
||||
'modelV2': MockModelData(valid=True),
|
||||
'selfdriveState': MockSelfDriveState(experimentalMode=True),
|
||||
|
||||
class MockMpc:
|
||||
def __init__(self, crash_cnt=0):
|
||||
self.crash_cnt = crash_cnt
|
||||
|
||||
|
||||
def flat_velocity(v):
|
||||
return [float(v)] * len(T_IDXS)
|
||||
|
||||
|
||||
def decel_velocity(v0, a):
|
||||
return [float(max(0.0, v0 + a * t)) for t in T_IDXS]
|
||||
|
||||
|
||||
def make_car_state(v_ego=10.0, v_cruise=20.0):
|
||||
msg = messaging.new_message('carState')
|
||||
msg.carState.vEgo = v_ego
|
||||
msg.carState.vCruise = v_cruise
|
||||
return msg.carState.as_reader()
|
||||
|
||||
|
||||
def make_selfdrive_state(experimental_mode=True):
|
||||
msg = messaging.new_message('selfdriveState')
|
||||
msg.selfdriveState.experimentalMode = experimental_mode
|
||||
return msg.selfdriveState.as_reader()
|
||||
|
||||
|
||||
def make_radar_state(lead_present=False, lead_radar=False, lead_two_present=False):
|
||||
msg = messaging.new_message('radarState')
|
||||
msg.radarState.leadOne.present = lead_present
|
||||
msg.radarState.leadOne.radar = lead_radar
|
||||
msg.radarState.leadTwo.present = lead_two_present
|
||||
return msg.radarState.as_reader()
|
||||
|
||||
|
||||
def make_model_v2(velocity=None, position_y=None, hard_brake=False, lead_probs=None, frame_drop_perc=0.0):
|
||||
msg = messaging.new_message('modelV2')
|
||||
msg.modelV2.velocity.x = velocity if velocity is not None else flat_velocity(0.0)
|
||||
msg.modelV2.position.y = position_y if position_y is not None else [0.0] * len(T_IDXS)
|
||||
msg.modelV2.frameDropPerc = frame_drop_perc
|
||||
msg.modelV2.meta.hardBrakePredicted = hard_brake
|
||||
if lead_probs is not None:
|
||||
msg.modelV2.init('leadsV3', 3)
|
||||
for i, (prob, prob_time) in enumerate(zip(lead_probs, (0.0, 2.0, 4.0), strict=True)):
|
||||
msg.modelV2.leadsV3[i].prob = prob
|
||||
msg.modelV2.leadsV3[i].probTime = prob_time
|
||||
return msg.modelV2.as_reader()
|
||||
|
||||
|
||||
def make_sm(v_ego=10.0, v_cruise=20.0, velocity=None, position_y=None, hard_brake=False,
|
||||
lead_present=False, lead_radar=False, lead_two_present=False, lead_probs=None,
|
||||
frame_drop_perc=0.0, experimental_mode=True):
|
||||
return {
|
||||
'carState': make_car_state(v_ego, v_cruise),
|
||||
'radarState': make_radar_state(lead_present, lead_radar, lead_two_present),
|
||||
'modelV2': make_model_v2(velocity, position_y, hard_brake, lead_probs, frame_drop_perc),
|
||||
'selfdriveState': make_selfdrive_state(experimental_mode),
|
||||
}
|
||||
return sm
|
||||
|
||||
def mock_cp():
|
||||
class CP:
|
||||
radarUnavailable = False
|
||||
return CP()
|
||||
|
||||
def mock_mpc():
|
||||
class MPC:
|
||||
crash_cnt = 0
|
||||
return MPC()
|
||||
def make_controller(cp=None, mpc=None, enabled=True):
|
||||
return DynamicExperimentalController(cp or structs.CarParams(), mpc or MockMpc(), params=MockParams(enabled))
|
||||
|
||||
# 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
|
||||
|
||||
class TestDynamicExperimentalController(OpenpilotTestCase):
|
||||
def test_initial_mode_is_acc(self, mock_cp, mock_mpc):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
def test_initial_mode_is_acc(self):
|
||||
controller = make_controller()
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
def test_standstill_triggers_blended(self, mock_cp, mock_mpc, default_sm):
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
default_sm['carState'].standstill = True
|
||||
def test_flat_plan_never_blends_at_any_speed(self):
|
||||
for v_ego in (2.5, 5.6, 8.3, 13.9, 22.2, 30.6):
|
||||
controller = make_controller()
|
||||
sm = make_sm(v_ego=v_ego, velocity=flat_velocity(v_ego))
|
||||
for _ in range(100):
|
||||
controller.update(sm)
|
||||
assert controller.mode() == "acc", f"false blend on a flat plan at v_ego={v_ego}"
|
||||
|
||||
def test_highway_slowdown_without_lead_blends(self):
|
||||
v0 = 110 / 3.6
|
||||
a = (70 / 3.6 - v0) / 6.0
|
||||
controller = make_controller()
|
||||
sm = make_sm(v_ego=v0, velocity=decel_velocity(v0, a))
|
||||
for _ in range(10):
|
||||
controller.update(default_sm)
|
||||
controller.update(sm)
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
def test_emergency_blended_on_fcw(self, 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)
|
||||
def test_curve_exclusion_prevents_false_blend(self):
|
||||
controller = make_controller()
|
||||
sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), position_y=[6.0] * len(T_IDXS))
|
||||
for _ in range(30):
|
||||
controller.update(sm)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
def test_any_lead_forces_acc_even_with_strong_model_signal(self):
|
||||
for lead_radar in (True, False):
|
||||
controller = make_controller()
|
||||
sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
|
||||
lead_present=True, lead_radar=lead_radar, lead_probs=[1.0, 1.0, 1.0])
|
||||
for _ in range(60):
|
||||
controller.update(sm)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
def test_veto_releases_without_rebuild_lag(self):
|
||||
controller = make_controller()
|
||||
lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0),
|
||||
lead_present=True, lead_probs=[1.0, 1.0, 1.0])
|
||||
for _ in range(30):
|
||||
controller.update(lead_sm)
|
||||
assert controller.mode() == "acc"
|
||||
assert controller.lead_veto
|
||||
|
||||
no_lead_sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -2.0), lead_present=False)
|
||||
for _ in range(ENTER_FRAMES + 2):
|
||||
controller.update(no_lead_sm)
|
||||
if controller.mode() == "blended":
|
||||
break
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
def test_radarless_slowdown_triggers_blended(self, mock_cp, mock_mpc, default_sm):
|
||||
mock_cp.radarUnavailable = True
|
||||
controller = DynamicExperimentalController(mock_cp, mock_mpc, params=MockParams())
|
||||
def test_lead_gone_with_no_underlying_slowdown_stays_acc(self):
|
||||
controller = make_controller()
|
||||
lead_sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0])
|
||||
for _ in range(30):
|
||||
controller.update(lead_sm)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
# 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
|
||||
no_lead_sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=False)
|
||||
for _ in range(20):
|
||||
controller.update(no_lead_sm)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
for _ in range(3):
|
||||
controller.update(default_sm)
|
||||
def test_creep_does_not_release_lead_veto(self):
|
||||
controller = make_controller()
|
||||
sm = make_sm(v_ego=1.0, velocity=flat_velocity(1.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0])
|
||||
for _ in range(10):
|
||||
controller.update(sm)
|
||||
assert controller.mode() == "acc"
|
||||
assert controller.lead_veto
|
||||
|
||||
def test_creep_hysteresis_band_without_lead(self):
|
||||
controller = make_controller()
|
||||
controller.update(make_sm(v_ego=1.5, velocity=flat_velocity(1.5)))
|
||||
assert controller.signals.creeping
|
||||
|
||||
controller.update(make_sm(v_ego=2.5, velocity=flat_velocity(2.5)))
|
||||
assert controller.signals.creeping, "a small excursion above CREEP_SPEED_ENTER should not exit creeping"
|
||||
|
||||
controller.update(make_sm(v_ego=5.0, velocity=flat_velocity(5.0)))
|
||||
assert not controller.signals.creeping, "should exit creeping once genuinely above CREEP_SPEED_EXIT"
|
||||
|
||||
def test_crash_cnt_override_inert_while_lead_present(self):
|
||||
mpc = MockMpc(crash_cnt=0)
|
||||
controller = make_controller(mpc=mpc)
|
||||
sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=True, lead_probs=[1.0, 1.0, 1.0])
|
||||
for _ in range(30):
|
||||
controller.update(sm)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
mpc.crash_cnt = 1
|
||||
controller.update(sm)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
def test_crash_cnt_blends_within_one_frame_without_lead(self):
|
||||
mpc = MockMpc(crash_cnt=1)
|
||||
controller = make_controller(mpc=mpc)
|
||||
sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), lead_present=False)
|
||||
controller.update(sm)
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
def test_hard_brake_predicted_blends_within_one_frame_without_lead(self):
|
||||
controller = make_controller()
|
||||
sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), hard_brake=True, lead_present=False)
|
||||
controller.update(sm)
|
||||
assert controller.mode() == "blended"
|
||||
|
||||
def test_hard_brake_override_inert_while_lead_present(self):
|
||||
controller = make_controller()
|
||||
sm = make_sm(v_ego=20.0, velocity=flat_velocity(20.0), hard_brake=True,
|
||||
lead_present=True, lead_probs=[1.0, 1.0, 1.0])
|
||||
controller.update(sm)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
def test_degraded_model_does_not_blend(self):
|
||||
controller = make_controller()
|
||||
sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -3.0), frame_drop_perc=60.0)
|
||||
for _ in range(30):
|
||||
controller.update(sm)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
def test_short_plan_arrays_do_not_blend(self):
|
||||
controller = make_controller()
|
||||
sm = make_sm(v_ego=20.0, velocity=[20.0] * 5)
|
||||
for _ in range(30):
|
||||
controller.update(sm)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
def test_disabled_param_holds_acc(self):
|
||||
controller = make_controller(enabled=False)
|
||||
sm = make_sm(v_ego=20.0, velocity=decel_velocity(20.0, -3.0))
|
||||
for _ in range(30):
|
||||
controller.update(sm)
|
||||
assert controller.mode() == "acc"
|
||||
|
||||
|
||||
class TestModeHysteresis(OpenpilotTestCase):
|
||||
def test_entry_requires_enter_frames(self):
|
||||
h = ModeHysteresis()
|
||||
for _ in range(ENTER_FRAMES - 1):
|
||||
assert h.update(want_blended=True, override=False, veto=False) == "acc"
|
||||
assert h.update(want_blended=True, override=False, veto=False) == "blended"
|
||||
|
||||
def test_override_beats_veto(self):
|
||||
h = ModeHysteresis()
|
||||
assert h.update(want_blended=False, override=True, veto=True) == "blended"
|
||||
|
||||
def test_veto_forces_acc_even_when_reason_active(self):
|
||||
h = ModeHysteresis()
|
||||
for _ in range(ENTER_FRAMES + 5):
|
||||
assert h.update(want_blended=True, override=False, veto=True) == "acc"
|
||||
|
||||
def test_counter_accumulates_under_veto_then_releases_instantly(self):
|
||||
h = ModeHysteresis()
|
||||
for _ in range(ENTER_FRAMES + 5):
|
||||
h.update(want_blended=True, override=False, veto=True)
|
||||
assert h.mode == "acc"
|
||||
assert h.update(want_blended=True, override=False, veto=False) == "blended"
|
||||
|
||||
def test_exit_requires_min_dwell_and_sustained_absence(self):
|
||||
h = ModeHysteresis()
|
||||
for _ in range(ENTER_FRAMES):
|
||||
h.update(want_blended=True, override=False, veto=False)
|
||||
assert h.mode == "blended"
|
||||
for _ in range(MIN_BLENDED_FRAMES - 1):
|
||||
assert h.update(want_blended=False, override=False, veto=False) == "blended"
|
||||
assert h.update(want_blended=False, override=False, veto=False) == "acc"
|
||||
|
||||
def test_no_flapping_on_alternating_reason(self):
|
||||
h = ModeHysteresis()
|
||||
changes = 0
|
||||
prev = h.mode
|
||||
for i in range(200):
|
||||
mode = h.update(want_blended=i % 2 == 0, override=False, veto=False)
|
||||
changes += mode != prev
|
||||
prev = mode
|
||||
assert changes == 0
|
||||
|
||||
|
||||
class TestShouldBlend(OpenpilotTestCase):
|
||||
def test_slowdown_detected_triggers(self):
|
||||
assert should_blend(DecSignals(decel_intent=1.0))
|
||||
assert not should_blend(DecSignals(decel_intent=0.0))
|
||||
|
||||
def test_curve_exclusion_suppresses_slowdown(self):
|
||||
assert not should_blend(DecSignals(decel_intent=1.0, curve_detected=True))
|
||||
|
||||
def test_degraded_model_suppresses_model_based_reasons(self):
|
||||
s = DecSignals(decel_intent=1.0, model_trust=0.0)
|
||||
assert not should_blend(s)
|
||||
|
||||
def test_creep_bypasses_everything(self):
|
||||
assert should_blend(DecSignals(model_trust=0.0, creeping=True))
|
||||
|
||||
@@ -93,9 +93,11 @@ class LongitudinalPlannerSP:
|
||||
def update(self, sm: messaging.SubMaster) -> None:
|
||||
self.accel_controller.update(sm)
|
||||
self.events_sp.clear()
|
||||
self.dec.update(sm)
|
||||
self.e2e_alerts_helper.update(sm, self.events_sp)
|
||||
|
||||
def update_dec(self, sm: messaging.SubMaster) -> None:
|
||||
self.dec.update(sm)
|
||||
|
||||
def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None:
|
||||
plan_sp_send = messaging.new_message('longitudinalPlanSP')
|
||||
|
||||
@@ -112,6 +114,10 @@ class LongitudinalPlannerSP:
|
||||
dec.state = DecState.blended if self.dec.mode() == 'blended' else DecState.acc
|
||||
dec.enabled = self.dec.enabled()
|
||||
dec.active = self.dec.active()
|
||||
dec.decelIntent = float(self.dec.signals.decel_intent)
|
||||
dec.curveDetected = bool(self.dec.signals.curve_detected)
|
||||
dec.wantBlended = bool(self.dec.want_blended)
|
||||
dec.leadVeto = bool(self.dec.lead_veto)
|
||||
|
||||
accel_controller = longitudinalPlanSP.accelController
|
||||
accel_controller.enabled = self.accel_controller.is_enabled()
|
||||
|
||||
@@ -7,7 +7,7 @@ See the LICENSE.md file in the root directory for more details.
|
||||
|
||||
from collections import deque
|
||||
from collections.abc import Callable
|
||||
from dataclasses import dataclass
|
||||
from dataclasses import asdict, dataclass
|
||||
import math
|
||||
import time
|
||||
from typing import Any
|
||||
@@ -28,6 +28,11 @@ LeadObservation = dict[str, Any]
|
||||
LeadObservationFn = Callable[[float, str, LeadObservation], LeadObservation | None]
|
||||
ModelActionFn = Callable[[float, float, float], tuple[float, bool]]
|
||||
EgoObservationFn = Callable[[float, float, float], tuple[float, float]]
|
||||
ModelPlanFn = Callable[[float, float, float], list[float]]
|
||||
ModelMetaFn = Callable[[float], tuple[list[float], bool, float]]
|
||||
LeadFutureProbsFn = Callable[[float], tuple[float, float, float]]
|
||||
PositionYFn = Callable[[float], list[float]]
|
||||
ExperimentalModeFn = Callable[[float], bool]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
@@ -85,6 +90,11 @@ class PlantSP(Plant):
|
||||
lead_observation_fn: LeadObservationFn | None = None,
|
||||
model_action_fn: ModelActionFn | None = None,
|
||||
ego_observation_fn: EgoObservationFn | None = None,
|
||||
model_plan_fn: ModelPlanFn | None = None,
|
||||
model_meta_fn: ModelMetaFn | None = None,
|
||||
lead_future_probs_fn: LeadFutureProbsFn | None = None,
|
||||
position_y_fn: PositionYFn | None = None,
|
||||
experimental_mode_fn: ExperimentalModeFn | None = None,
|
||||
actuator_delay: float | None = None,
|
||||
actuator_lag: float = 0.0,
|
||||
actuator_model: ActuatorModel | None = None,
|
||||
@@ -129,6 +139,11 @@ class PlantSP(Plant):
|
||||
self.lead_observation_fn = lead_observation_fn
|
||||
self.model_action_fn = model_action_fn
|
||||
self.ego_observation_fn = ego_observation_fn
|
||||
self.model_plan_fn = model_plan_fn
|
||||
self.model_meta_fn = model_meta_fn
|
||||
self.lead_future_probs_fn = lead_future_probs_fn
|
||||
self.position_y_fn = position_y_fn
|
||||
self.experimental_mode_fn = experimental_mode_fn
|
||||
self.actuator_model = actuator_model
|
||||
self.actuator_delay = actuator_model.planner_delay if actuator_model is not None else actuator_delay
|
||||
self.transport_delay = actuator_model.transport_delay if actuator_model is not None else actuator_delay
|
||||
@@ -282,6 +297,10 @@ class PlantSP(Plant):
|
||||
# does not predict slowdown in e2e mode
|
||||
position = log.XYZTData.new_message()
|
||||
position.x = [float(x) for x in (self.speed + 0.5) * np.array(ModelConstants.T_IDXS)]
|
||||
if self.position_y_fn is None:
|
||||
position.y = [0.0] * len(ModelConstants.T_IDXS)
|
||||
else:
|
||||
position.y = [float(y) for y in self.position_y_fn(self.current_time)]
|
||||
model.modelV2.position = position
|
||||
if self.model_action_fn is None:
|
||||
model_acceleration, model_should_stop = self.acceleration + 0.5, False
|
||||
@@ -290,17 +309,34 @@ class PlantSP(Plant):
|
||||
model.modelV2.action.desiredAcceleration = float(model_acceleration)
|
||||
model.modelV2.action.shouldStop = bool(model_should_stop)
|
||||
velocity = log.XYZTData.new_message()
|
||||
velocity.x = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
|
||||
velocity.x[0] = float(self.speed) # always start at current speed
|
||||
if self.model_plan_fn is None:
|
||||
velocity_plan = [float(x) for x in (self.speed + 0.5) * np.ones_like(ModelConstants.T_IDXS)]
|
||||
velocity_plan[0] = float(self.speed) # always start at current speed
|
||||
else:
|
||||
velocity_plan = [float(x) for x in self.model_plan_fn(self.current_time, self.speed, self.acceleration)]
|
||||
velocity.x = velocity_plan
|
||||
model.modelV2.velocity = velocity
|
||||
acceleration = log.XYZTData.new_message()
|
||||
acceleration.x = [float(x) for x in np.zeros_like(ModelConstants.T_IDXS)]
|
||||
model.modelV2.acceleration = acceleration
|
||||
model.modelV2.meta.disengagePredictions.gasPressProbs = [float(prob_throttle) for _ in range(6)]
|
||||
if self.model_meta_fn is None:
|
||||
brake3_probs, hard_brake_predicted, frame_drop_perc = [0.0] * 5, False, 0.0
|
||||
else:
|
||||
brake3_probs, hard_brake_predicted, frame_drop_perc = self.model_meta_fn(self.current_time)
|
||||
model.modelV2.meta.disengagePredictions.brake3MetersPerSecondSquaredProbs = [float(p) for p in brake3_probs]
|
||||
model.modelV2.meta.hardBrakePredicted = bool(hard_brake_predicted)
|
||||
model.modelV2.frameDropPerc = float(frame_drop_perc)
|
||||
if self.lead_future_probs_fn is not None:
|
||||
model.modelV2.init('leadsV3', 3)
|
||||
lead_future_probs = self.lead_future_probs_fn(self.current_time)
|
||||
for i, (prob, prob_time) in enumerate(zip(lead_future_probs, (0.0, 2.0, 4.0), strict=True)):
|
||||
model.modelV2.leadsV3[i].prob = float(prob)
|
||||
model.modelV2.leadsV3[i].probTime = prob_time
|
||||
|
||||
control.controlsState.longControlState = self.long_control.long_control_state if self.long_control is not None else (
|
||||
LongCtrlState.pid if self.enabled else LongCtrlState.off)
|
||||
ss.selfdriveState.experimentalMode = self.e2e
|
||||
ss.selfdriveState.experimentalMode = self.e2e if self.experimental_mode_fn is None else bool(self.experimental_mode_fn(self.current_time))
|
||||
ss.selfdriveState.personality = self.personality
|
||||
control.controlsState.forceDecel = self.force_decel
|
||||
true_v_ego = self.speed
|
||||
@@ -383,6 +419,9 @@ class PlantSP(Plant):
|
||||
"fcw": fcw,
|
||||
"mpc_source": self.planner.mpc.source,
|
||||
"dec_mode": self.planner.dec.mode(),
|
||||
"dec_want_blended": self.planner.dec.want_blended,
|
||||
"dec_signals": asdict(self.planner.dec.signals),
|
||||
"dec_lead_veto": self.planner.dec.lead_veto,
|
||||
"controller_active": self.planner.accel_controller_active,
|
||||
"model_action": {
|
||||
"desiredAcceleration": float(model_acceleration),
|
||||
|
||||
+179
@@ -0,0 +1,179 @@
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.common.test import OpenpilotTestCase
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import ENTER_FRAMES, MIN_BLENDED_FRAMES
|
||||
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
|
||||
|
||||
T_IDXS = np.array(ModelConstants.T_IDXS)
|
||||
|
||||
|
||||
def decel_plan(a):
|
||||
def fn(_current_time, speed, _acceleration):
|
||||
return [float(max(0.0, speed + a * t)) for t in T_IDXS]
|
||||
return fn
|
||||
|
||||
|
||||
def flat_plan():
|
||||
def fn(_current_time, speed, _acceleration):
|
||||
return [float(speed)] * len(T_IDXS)
|
||||
return fn
|
||||
|
||||
|
||||
def alternating_plan(a):
|
||||
def fn(current_time, speed, _acceleration):
|
||||
frame_a = a if round(current_time / DT_MDL) % 2 == 0 else 0.0
|
||||
return [float(max(0.0, speed + frame_a * t)) for t in T_IDXS]
|
||||
return fn
|
||||
|
||||
|
||||
def persistent_lead_probs(_current_time):
|
||||
return (1.0, 0.95, 0.9)
|
||||
|
||||
|
||||
def _run(plant, steps, v_lead=0.0, v_cruise=50.0):
|
||||
solver_failures = 0
|
||||
original_reset = plant.planner.mpc.reset
|
||||
|
||||
def counting_reset(*args, **kw):
|
||||
nonlocal solver_failures
|
||||
if plant.planner.mpc.solution_status != 0:
|
||||
solver_failures += 1
|
||||
return original_reset(*args, **kw)
|
||||
|
||||
plant.planner.mpc.reset = counting_reset
|
||||
return [plant.step(v_lead=v_lead, v_cruise=v_cruise) for _ in range(steps)], solver_failures
|
||||
|
||||
|
||||
def mode_changes(results):
|
||||
modes = [r["dec_mode"] for r in results]
|
||||
return sum(a != b for a, b in zip(modes, modes[1:], strict=False))
|
||||
|
||||
|
||||
class TestDecManeuvers(OpenpilotTestCase):
|
||||
def setUp(self):
|
||||
super().setUp()
|
||||
self.params = Params()
|
||||
self.params.put_bool("DynamicExperimentalControl", True, block=True)
|
||||
|
||||
def test_s1_lead_clears_with_underlying_slowdown_blends_quickly(self):
|
||||
clear_t = 1.0
|
||||
|
||||
def lead_obs(current_time, _lead_name, truth):
|
||||
return None if current_time >= clear_t else dict(truth)
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True,
|
||||
lead_observation_fn=lead_obs, model_plan_fn=decel_plan(-2.5),
|
||||
lead_future_probs_fn=persistent_lead_probs)
|
||||
clear_frame = round(clear_t / DT_MDL)
|
||||
results, _ = _run(plant, steps=clear_frame + ENTER_FRAMES + 5, v_lead=20.0, v_cruise=20.0)
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results[:clear_frame])
|
||||
assert all(r["dec_lead_veto"] for r in results[:clear_frame])
|
||||
post_clear = [r["dec_mode"] for r in results[clear_frame:clear_frame + ENTER_FRAMES + 2]]
|
||||
assert "blended" in post_clear
|
||||
|
||||
def test_s1b_lead_clears_with_no_underlying_slowdown_stays_acc(self):
|
||||
clear_t = 1.0
|
||||
|
||||
def lead_obs(current_time, _lead_name, truth):
|
||||
return None if current_time >= clear_t else dict(truth)
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True,
|
||||
lead_observation_fn=lead_obs, model_plan_fn=flat_plan(),
|
||||
lead_future_probs_fn=persistent_lead_probs)
|
||||
clear_frame = round(clear_t / DT_MDL)
|
||||
results, _ = _run(plant, steps=clear_frame + MIN_BLENDED_FRAMES, v_lead=20.0, v_cruise=20.0)
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
|
||||
def test_s2_steady_highway_following_never_blends(self):
|
||||
v = 80.0 / 3.6
|
||||
plant = PlantSP(lead_relevancy=True, speed=v, distance_lead=40.0, e2e=True, only_radar=True,
|
||||
model_plan_fn=flat_plan(), lead_future_probs_fn=persistent_lead_probs)
|
||||
results, failures = _run(plant, steps=100, v_lead=v, v_cruise=v)
|
||||
|
||||
assert failures <= 1
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
|
||||
def test_s3_low_speed_cruise_no_lead_never_blends(self):
|
||||
v = 15.0 / 3.6
|
||||
plant = PlantSP(lead_relevancy=False, speed=v, e2e=True, model_plan_fn=flat_plan())
|
||||
results, _ = _run(plant, steps=100, v_cruise=v)
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
|
||||
def test_s4_highway_slowdown_without_lead_blends(self):
|
||||
v0 = 110.0 / 3.6
|
||||
a = (70.0 / 3.6 - v0) / 6.0
|
||||
plant = PlantSP(lead_relevancy=False, speed=v0, e2e=True, model_plan_fn=decel_plan(a))
|
||||
results, _ = _run(plant, steps=10, v_cruise=v0)
|
||||
|
||||
assert any(r["dec_mode"] == "blended" for r in results)
|
||||
|
||||
def test_s5_stop_then_depart_with_lead_present_stays_acc_throughout(self):
|
||||
def departing_lead(current_time):
|
||||
return 0.0 if current_time < 1.0 else min(15.0, 3.0 * (current_time - 1.0))
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=0.0, distance_lead=6.0, e2e=True)
|
||||
results = []
|
||||
solver_failures = 0
|
||||
original_reset = plant.planner.mpc.reset
|
||||
|
||||
def counting_reset(*args, **kw):
|
||||
nonlocal solver_failures
|
||||
if plant.planner.mpc.solution_status != 0:
|
||||
solver_failures += 1
|
||||
return original_reset(*args, **kw)
|
||||
|
||||
plant.planner.mpc.reset = counting_reset
|
||||
for _ in range(200):
|
||||
results.append(plant.step(v_lead=departing_lead(plant.current_time), v_cruise=15.0))
|
||||
|
||||
assert solver_failures <= 1
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
assert all(r["dec_lead_veto"] for r in results)
|
||||
|
||||
def test_s6_creep_cycles_behind_lead_stay_acc(self):
|
||||
def creep_cycle_lead(current_time):
|
||||
return 1.5 + 1.5 * np.sin(current_time * 2.0)
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=1.0, distance_lead=8.0, e2e=True, only_radar=True,
|
||||
model_plan_fn=flat_plan(), lead_future_probs_fn=persistent_lead_probs)
|
||||
results = [plant.step(v_lead=creep_cycle_lead(plant.current_time), v_cruise=5.0) for _ in range(200)]
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
|
||||
def test_s7_oscillating_near_threshold_demand_does_not_flap(self):
|
||||
plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=alternating_plan(-2.5))
|
||||
results, _ = _run(plant, steps=200, v_cruise=20.0)
|
||||
|
||||
assert mode_changes(results) <= 2
|
||||
|
||||
def test_s8_degraded_model_holds_acc_through_a_slowdown(self):
|
||||
def degraded_meta(_current_time):
|
||||
return [0.0] * 5, False, 60.0
|
||||
|
||||
plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-3.0), model_meta_fn=degraded_meta)
|
||||
results, _ = _run(plant, steps=30, v_cruise=20.0)
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
|
||||
def test_s9_curve_exclusion_prevents_false_blend_on_a_bend(self):
|
||||
plant = PlantSP(lead_relevancy=False, speed=20.0, e2e=True, model_plan_fn=decel_plan(-2.5),
|
||||
position_y_fn=lambda _t: [6.0] * len(T_IDXS))
|
||||
results, _ = _run(plant, steps=30, v_cruise=20.0)
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
|
||||
def test_s10_hard_brake_override_inert_while_lead_present(self):
|
||||
def hard_brake_meta(_current_time):
|
||||
return [0.0] * 5, True, 0.0
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=20.0, distance_lead=40.0, e2e=True, only_radar=True,
|
||||
model_plan_fn=flat_plan(), model_meta_fn=hard_brake_meta, lead_future_probs_fn=persistent_lead_probs)
|
||||
results, _ = _run(plant, steps=10, v_lead=20.0, v_cruise=20.0)
|
||||
|
||||
assert all(r["dec_mode"] == "acc" for r in results)
|
||||
Reference in New Issue
Block a user