mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-08-23 15:23:46 +08:00
feat: accel controller
This commit is contained in:
rav4kumar
@@ -194,6 +194,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
aTarget @5 :Float32;
|
||||
events @6 :List(OnroadEventSP.Event);
|
||||
e2eAlerts @7 :E2eAlerts;
|
||||
accelController @8 :AccelController;
|
||||
|
||||
struct DynamicExperimentalControl {
|
||||
state @0 :DynamicExperimentalControlState;
|
||||
@@ -296,6 +297,35 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 {
|
||||
greenLightAlert @0 :Bool;
|
||||
leadDepartAlert @1 :Bool;
|
||||
}
|
||||
|
||||
struct AccelController {
|
||||
enabled @0 :Bool;
|
||||
active @1 :Bool;
|
||||
shadowOnlyDEPRECATED @2 :Bool;
|
||||
profile @3 :Profile;
|
||||
state @4 :State;
|
||||
|
||||
enum Profile {
|
||||
eco @0;
|
||||
normal @1;
|
||||
sport @2;
|
||||
}
|
||||
|
||||
enum State {
|
||||
inactive @0;
|
||||
free @1;
|
||||
restrict @2;
|
||||
hold @3;
|
||||
release @4;
|
||||
stopHold @5;
|
||||
}
|
||||
}
|
||||
|
||||
enum AccelerationPersonality {
|
||||
eco @0;
|
||||
normal @1;
|
||||
sport @2;
|
||||
}
|
||||
}
|
||||
|
||||
struct OnroadEventSP @0xda96579883444c35 {
|
||||
|
||||
@@ -235,6 +235,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
|
||||
{"DynamicExperimentalControl", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"BlindSpot", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
|
||||
// Accel Controller profiles (Eco / Normal / Sport)
|
||||
{"AccelPersonalityEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
|
||||
{"AccelPersonality", {PERSISTENT | BACKUP, INT, "1"}},
|
||||
|
||||
// sunnypilot model params
|
||||
{"CameraOffset", {PERSISTENT | BACKUP, FLOAT, "0.0"}},
|
||||
{"LagdToggle", {PERSISTENT | BACKUP, BOOL, "1"}},
|
||||
|
||||
@@ -112,12 +112,16 @@ class TestParams:
|
||||
def test_params_default_value(self):
|
||||
self.params.remove("LanguageSetting")
|
||||
self.params.remove("LongitudinalPersonality")
|
||||
self.params.remove("AccelPersonalityEnabled")
|
||||
self.params.remove("AccelPersonality")
|
||||
self.params.remove("LiveParameters")
|
||||
|
||||
assert self.params.get("LanguageSetting") is None
|
||||
assert self.params.get("LanguageSetting", return_default=False) is None
|
||||
assert isinstance(self.params.get("LanguageSetting", return_default=True), str)
|
||||
assert isinstance(self.params.get("LongitudinalPersonality", return_default=True), int)
|
||||
assert self.params.get("AccelPersonalityEnabled", return_default=True) is False
|
||||
assert self.params.get("AccelPersonality", return_default=True) == 1
|
||||
assert self.params.get("LiveParameters") is None
|
||||
assert self.params.get("LiveParameters", return_default=True) is None
|
||||
|
||||
|
||||
@@ -9,6 +9,7 @@ from openpilot.common.swaglog import cloudlog
|
||||
# WARNING: imports outside of constants will not trigger a rebuild
|
||||
from openpilot.selfdrive.modeld.constants import index_function
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpcSP
|
||||
|
||||
if __name__ == '__main__': # generating code
|
||||
from acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
|
||||
@@ -213,8 +214,9 @@ def gen_long_ocp():
|
||||
return ocp
|
||||
|
||||
|
||||
class LongitudinalMpc:
|
||||
class LongitudinalMpc(LongitudinalMpcSP):
|
||||
def __init__(self, dt=DT_MDL):
|
||||
LongitudinalMpcSP.__init__(self)
|
||||
self.dt = dt
|
||||
self.solver = AcadosOcpSolverCython(MODEL_NAME, ACADOS_SOLVER_TYPE, N)
|
||||
self.reset()
|
||||
@@ -270,7 +272,8 @@ class LongitudinalMpc:
|
||||
def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
|
||||
jerk_factor = get_jerk_factor(personality)
|
||||
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
|
||||
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
|
||||
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost,
|
||||
LongitudinalMpcSP.scale_jerk_cost(self, jerk_factor * J_EGO_COST)]
|
||||
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
|
||||
self.set_cost_weights(cost_weights, constraint_cost_weights)
|
||||
|
||||
@@ -345,6 +348,7 @@ class LongitudinalMpc:
|
||||
|
||||
self.params[:,0] = ACCEL_MIN
|
||||
self.params[:,1] = ACCEL_MAX
|
||||
LongitudinalMpcSP.apply_accel_limits(self)
|
||||
self.params[:,2] = np.min(x_obstacles, axis=1)
|
||||
self.params[:,3] = np.copy(self.a_prev)
|
||||
self.params[:,4] = t_follow
|
||||
@@ -364,6 +368,7 @@ class LongitudinalMpc:
|
||||
self.solver.constraints_set(0, "ubx", self.x0)
|
||||
|
||||
self.solution_status = self.solver.solve()
|
||||
LongitudinalMpcSP.save_solution_status(self)
|
||||
self.solve_time = float(self.solver.get_stats('time_tot')[0])
|
||||
self.time_qp_solution = float(self.solver.get_stats('time_qp')[0])
|
||||
self.time_linearization = float(self.solver.get_stats('time_lin')[0])
|
||||
|
||||
@@ -51,7 +51,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
def __init__(self, CP, CP_SP, init_v=0.0, init_a=0.0, dt=DT_MDL):
|
||||
self.CP = CP
|
||||
self.mpc = LongitudinalMpc(dt=dt)
|
||||
LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc)
|
||||
LongitudinalPlannerSP.__init__(self, self.CP, CP_SP, self.mpc, dt=dt)
|
||||
self.fcw = False
|
||||
self.dt = dt
|
||||
self.allow_throttle = True
|
||||
@@ -129,16 +129,12 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
clipped_accel_coast = max(accel_coast, accel_clip[0])
|
||||
clipped_accel_coast_interp = np.interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_clip[1], clipped_accel_coast])
|
||||
accel_clip[1] = min(accel_clip[1], clipped_accel_coast_interp)
|
||||
|
||||
# Get new v_cruise and a_desired from Smart Cruise Control and Speed Limit Assist
|
||||
v_cruise, self.a_desired = LongitudinalPlannerSP.update_targets(self, sm, self.v_desired_filter.x, self.a_desired, v_cruise)
|
||||
|
||||
if force_slow_decel:
|
||||
v_cruise = 0.0
|
||||
|
||||
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
|
||||
self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
|
||||
is_e2e = LongitudinalPlannerSP.update_mpc(self, sm, v_cruise, prev_accel_constraint, accel_clip[1], reset_state)
|
||||
|
||||
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)
|
||||
@@ -154,13 +150,14 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
self.a_desired = float(np.interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
|
||||
self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
|
||||
|
||||
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
|
||||
output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
|
||||
action_t=action_t, vEgoStopping=self.CP.vEgoStopping)
|
||||
action_t = self.CP.longitudinalActuatorDelay + DT_MDL
|
||||
output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(
|
||||
self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, action_t=action_t, vEgoStopping=self.CP.vEgoStopping,
|
||||
)
|
||||
output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
|
||||
output_should_stop_e2e = sm['modelV2'].action.shouldStop
|
||||
|
||||
if self.is_e2e(sm):
|
||||
if is_e2e:
|
||||
output_a_target = min(output_a_target_e2e, output_a_target_mpc)
|
||||
self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
|
||||
if output_a_target < output_a_target_mpc:
|
||||
@@ -168,6 +165,7 @@ class LongitudinalPlanner(LongitudinalPlannerSP):
|
||||
else:
|
||||
output_a_target = output_a_target_mpc
|
||||
self.output_should_stop = output_should_stop_mpc
|
||||
self.output_should_stop = LongitudinalPlannerSP.update_should_stop(self, self.output_should_stop)
|
||||
|
||||
for idx in range(2):
|
||||
accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)
|
||||
|
||||
@@ -11,6 +11,14 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
|
||||
|
||||
class PlannerSM(dict):
|
||||
def __init__(self, radar_frame: int, services: dict):
|
||||
super().__init__(services)
|
||||
self.logMonoTime = {"radarState": radar_frame}
|
||||
self.valid = {"radarState": True}
|
||||
self.alive = {"radarState": True}
|
||||
|
||||
|
||||
class Plant:
|
||||
messaging_initialized = False
|
||||
|
||||
@@ -132,7 +140,7 @@ class Plant:
|
||||
car_control.carControl.orientationNED = [0., float(pitch), 0.]
|
||||
|
||||
# ******** get controlsState messages for plotting ***
|
||||
sm = {'radarState': radar.radarState,
|
||||
sm = PlannerSM(self.rk.frame, {'radarState': radar.radarState,
|
||||
'carState': car_state.carState,
|
||||
'carControl': car_control.carControl,
|
||||
'controlsState': control.controlsState,
|
||||
@@ -141,7 +149,7 @@ class Plant:
|
||||
'modelV2': model.modelV2,
|
||||
'carStateSP': car_state_sp.carStateSP,
|
||||
'liveMapDataSP': live_map_data_sp.liveMapDataSP,
|
||||
'gpsLocation': gps_data.gpsLocation}
|
||||
'gpsLocation': gps_data.gpsLocation})
|
||||
self.planner.update(sm)
|
||||
self.acceleration = self.planner.output_a_target
|
||||
if self.planner.output_should_stop:
|
||||
|
||||
@@ -27,6 +27,12 @@ DESCRIPTIONS = {
|
||||
"In relaxed mode sunnypilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with " +
|
||||
"your steering wheel distance button."
|
||||
),
|
||||
"AccelPersonalityEnabled": tr_noop(
|
||||
"Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority."
|
||||
),
|
||||
"AccelPersonality": tr_noop(
|
||||
"Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly."
|
||||
),
|
||||
"IsLdwEnabled": tr_noop(
|
||||
"Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line " +
|
||||
"without a turn signal activated while driving over 31 mph (50 km/h)."
|
||||
@@ -106,6 +112,24 @@ class TogglesLayout(Widget):
|
||||
icon="speed_limit.png"
|
||||
)
|
||||
|
||||
self._accel_personality_enabled = toggle_item(
|
||||
lambda: tr("Enable Accel Controller"),
|
||||
lambda: tr(DESCRIPTIONS["AccelPersonalityEnabled"]),
|
||||
self._params.get_bool("AccelPersonalityEnabled"),
|
||||
callback=self._set_accel_personality_enabled,
|
||||
icon="speed_limit.png",
|
||||
)
|
||||
|
||||
self._accel_personality_setting = multiple_button_item(
|
||||
lambda: tr("Acceleration Profile"),
|
||||
lambda: tr(DESCRIPTIONS["AccelPersonality"]),
|
||||
buttons=[lambda: tr("Eco"), lambda: tr("Normal"), lambda: tr("Sport")],
|
||||
button_width=300,
|
||||
callback=self._set_accel_personality,
|
||||
selected_index=self._params.get("AccelPersonality", return_default=True),
|
||||
icon="speed_limit.png"
|
||||
)
|
||||
|
||||
self._toggles = {}
|
||||
self._locked_toggles = set()
|
||||
for param, (title, desc, icon, needs_restart) in self._toggle_defs.items():
|
||||
@@ -135,9 +159,11 @@ class TogglesLayout(Widget):
|
||||
|
||||
self._toggles[param] = toggle
|
||||
|
||||
# insert longitudinal personality after NDOG toggle
|
||||
# insert longitudinal personality and Accel Controller settings after NDOG toggle
|
||||
if param == "DisengageOnAccelerator":
|
||||
self._toggles["LongitudinalPersonality"] = self._long_personality_setting
|
||||
self._toggles["AccelPersonalityEnabled"] = self._accel_personality_enabled
|
||||
self._toggles["AccelPersonality"] = self._accel_personality_setting
|
||||
|
||||
self._update_experimental_mode_icon()
|
||||
self._scroller = Scroller(list(self._toggles.values()), line_separator=True, spacing=0)
|
||||
@@ -158,6 +184,7 @@ class TogglesLayout(Widget):
|
||||
|
||||
def _update_toggles(self):
|
||||
ui_state.update_params()
|
||||
accel_personality_enabled = self._params.get_bool("AccelPersonalityEnabled")
|
||||
|
||||
e2e_description = tr(
|
||||
"sunnypilot defaults to driving in chill mode. Experimental mode enables alpha-level features that aren't ready for chill mode. " +
|
||||
@@ -176,11 +203,15 @@ class TogglesLayout(Widget):
|
||||
self._toggles["ExperimentalMode"].action_item.set_enabled(True)
|
||||
self._toggles["ExperimentalMode"].set_description(e2e_description)
|
||||
self._long_personality_setting.action_item.set_enabled(True)
|
||||
self._accel_personality_enabled.action_item.set_enabled(True)
|
||||
self._accel_personality_setting.action_item.set_enabled(accel_personality_enabled)
|
||||
else:
|
||||
# no long for now
|
||||
self._toggles["ExperimentalMode"].action_item.set_enabled(False)
|
||||
self._toggles["ExperimentalMode"].action_item.set_state(False)
|
||||
self._long_personality_setting.action_item.set_enabled(False)
|
||||
self._accel_personality_enabled.action_item.set_enabled(False)
|
||||
self._accel_personality_setting.action_item.set_enabled(False)
|
||||
self._params.remove("ExperimentalMode")
|
||||
|
||||
unavailable = tr("Experimental mode is currently unavailable on this car since the car's stock ACC is used for longitudinal control.")
|
||||
@@ -203,6 +234,10 @@ class TogglesLayout(Widget):
|
||||
# refresh toggles from params to mirror external changes
|
||||
for param in self._toggle_defs:
|
||||
self._toggles[param].action_item.set_state(self._params.get_bool(param))
|
||||
self._accel_personality_enabled.action_item.set_state(accel_personality_enabled)
|
||||
self._accel_personality_setting.action_item.set_selected_button(
|
||||
self._params.get("AccelPersonality", return_default=True)
|
||||
)
|
||||
|
||||
# these toggles need restart, block while engaged
|
||||
for toggle_def in self._toggle_defs:
|
||||
@@ -247,3 +282,10 @@ class TogglesLayout(Widget):
|
||||
|
||||
def _set_longitudinal_personality(self, button_index: int):
|
||||
self._params.put("LongitudinalPersonality", button_index, block=True)
|
||||
|
||||
def _set_accel_personality(self, button_index: int):
|
||||
self._params.put("AccelPersonality", button_index, block=True)
|
||||
|
||||
def _set_accel_personality_enabled(self, state: bool):
|
||||
self._params.put_bool("AccelPersonalityEnabled", state, block=True)
|
||||
self._accel_personality_setting.action_item.set_enabled(state and ui_state.has_longitudinal_control)
|
||||
|
||||
@@ -14,6 +14,8 @@ class TogglesLayoutMici(NavScroller):
|
||||
super().__init__()
|
||||
|
||||
self._personality_toggle = BigMultiParamToggle("driving personality", "LongitudinalPersonality", ["aggressive", "standard", "relaxed"])
|
||||
self._accel_personality_enabled = BigParamControl("enable accel controller", "AccelPersonalityEnabled")
|
||||
self._accel_personality_toggle = BigMultiParamToggle("acceleration profile", "AccelPersonality", ["eco", "normal", "sport"])
|
||||
self._experimental_btn = BigParamControl("experimental mode", "ExperimentalMode")
|
||||
is_metric_toggle = BigParamControl("use metric units", "IsMetric")
|
||||
ldw_toggle = BigParamControl("lane departure warnings", "IsLdwEnabled")
|
||||
@@ -24,6 +26,8 @@ class TogglesLayoutMici(NavScroller):
|
||||
|
||||
self._scroller.add_widgets([
|
||||
self._personality_toggle,
|
||||
self._accel_personality_enabled,
|
||||
self._accel_personality_toggle,
|
||||
self._experimental_btn,
|
||||
is_metric_toggle,
|
||||
ldw_toggle,
|
||||
@@ -36,6 +40,7 @@ class TogglesLayoutMici(NavScroller):
|
||||
# Toggle lists
|
||||
self._refresh_toggles = (
|
||||
("ExperimentalMode", self._experimental_btn),
|
||||
("AccelPersonalityEnabled", self._accel_personality_enabled),
|
||||
("IsMetric", is_metric_toggle),
|
||||
("IsLdwEnabled", ldw_toggle),
|
||||
("AlwaysOnDM", always_on_dm_toggle),
|
||||
@@ -45,6 +50,9 @@ class TogglesLayoutMici(NavScroller):
|
||||
)
|
||||
|
||||
enable_openpilot.set_enabled(lambda: not ui_state.engaged)
|
||||
self._accel_personality_toggle.set_enabled(
|
||||
lambda: ui_state.has_longitudinal_control and ui_state.params.get_bool("AccelPersonalityEnabled")
|
||||
)
|
||||
record_front.set_enabled(False if ui_state.params.get_bool("RecordFrontLock") else (lambda: not ui_state.engaged))
|
||||
record_mic.set_enabled(lambda: not ui_state.engaged)
|
||||
|
||||
@@ -75,13 +83,18 @@ class TogglesLayoutMici(NavScroller):
|
||||
if ui_state.has_longitudinal_control:
|
||||
self._experimental_btn.set_visible(True)
|
||||
self._personality_toggle.set_visible(True)
|
||||
self._accel_personality_enabled.set_visible(True)
|
||||
self._accel_personality_toggle.set_visible(True)
|
||||
else:
|
||||
# no long for now
|
||||
self._experimental_btn.set_visible(False)
|
||||
self._experimental_btn.set_checked(False)
|
||||
self._personality_toggle.set_visible(False)
|
||||
self._accel_personality_enabled.set_visible(False)
|
||||
self._accel_personality_toggle.set_visible(False)
|
||||
ui_state.params.remove("ExperimentalMode")
|
||||
|
||||
# Refresh toggles from params to mirror external changes
|
||||
for key, item in self._refresh_toggles:
|
||||
item.set_checked(ui_state.params.get_bool(key))
|
||||
self._accel_personality_toggle.refresh()
|
||||
|
||||
@@ -382,13 +382,18 @@ class BigMultiParamToggle(BigMultiToggle):
|
||||
self._load_value()
|
||||
|
||||
def _load_value(self):
|
||||
self.set_value(self._options[self._params.get(self._param) or 0])
|
||||
value = self._params.get(self._param, return_default=True)
|
||||
index = value if isinstance(value, int) else 0
|
||||
self.set_value(self._options[max(0, min(index, len(self._options) - 1))])
|
||||
|
||||
def _handle_mouse_release(self, mouse_pos: MousePos):
|
||||
super()._handle_mouse_release(mouse_pos)
|
||||
new_idx = self._options.index(self.value)
|
||||
self._params.put(self._param, new_idx)
|
||||
|
||||
def refresh(self):
|
||||
self._load_value()
|
||||
|
||||
|
||||
class BigParamControl(BigToggle):
|
||||
def __init__(self, text: str, param: str, toggle_callback: Callable | None = None):
|
||||
|
||||
@@ -0,0 +1,384 @@
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car.interfaces import ACCEL_MAX
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource
|
||||
from openpilot.sunnypilot import get_sanitize_int_param
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
|
||||
CAP_FILTER_FRAMES, COMFORT_DECEL, DEPARTURE_MOTION_NOISE_FLOOR, LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW,
|
||||
LEAD_BRAKING_ACCEL_THRESHOLD, LEAD_LOSS_HOLD_TIME, LEAD_MATCH_ACCEL_SLEW, LEAD_MATCH_GAP_GAIN, LEAD_MATCH_SPEED_HEADROOM,
|
||||
MATCHED_SPEED_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE,
|
||||
MPC_DECEL_JERK_MAX_TARGET_REDUCTION, MPC_DECEL_TREND_FRAMES, SPEED_RELIEF_DEADBAND, SPEED_RESTRICT_DEADBAND, TARGET_SPEED_ARM_MARGIN,
|
||||
TARGET_SPEED_RESERVE, PLANNER_BRAKING_ACCEL_THRESHOLD, RADAR_STALE_TIMEOUT, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_EGO_SPEED,
|
||||
STOP_HOLD_EXIT_FRAMES, STOP_HOLD_EXIT_SPEED, STOP_HOLD_MAX_LEAD_DISTANCE, VEGO_NOISE_TOLERANCE, PARAM_READ_INTERVAL, AccelProfile,
|
||||
profile_accel_max, sanitize_profile,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.helpers import build_accel_ceiling, is_valid_context
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan, calculate_lead_plan, has_radar_lead, is_lead_source
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.state import AccelControllerState, TargetState
|
||||
|
||||
|
||||
class AccelController:
|
||||
def __init__(self, CP, dt: float = DT_MDL):
|
||||
if not math.isfinite(dt) or dt <= 0.0:
|
||||
raise ValueError("dt must be finite and positive")
|
||||
|
||||
self.dt = dt
|
||||
self.delay = float(CP.longitudinalActuatorDelay) + DT_MDL
|
||||
self.lead_loss_hold_frames = max(CAP_FILTER_FRAMES, math.ceil(LEAD_LOSS_HOLD_TIME / dt))
|
||||
self.radar_stale_frames = max(1, math.ceil(RADAR_STALE_TIMEOUT / dt))
|
||||
self.params = Params()
|
||||
self.available = bool(CP.openpilotLongitudinalControl)
|
||||
self.enabled = False
|
||||
self.profile = AccelProfile.normal
|
||||
self._param_read_frames = max(1, int(round(PARAM_READ_INTERVAL / dt)))
|
||||
self._param_frame = 0
|
||||
self._jerk_smoothing_blocked = False
|
||||
self._required_decel_samples: list[float] = []
|
||||
self._required_decel_lead = -1
|
||||
self._required_decel_lead_track_id = -1
|
||||
self._lead_trend_warmup = False
|
||||
self.target_state = TargetState()
|
||||
self._held_lead_plan: LeadPlan | None = None
|
||||
self.is_active = self.launching = self.departure_launching = False
|
||||
self.output_v_target = 0.0
|
||||
self.mpc_accel_max: tuple[float, ...] | None = None
|
||||
self.state = AccelControllerState.inactive
|
||||
self.selected_lead = -1
|
||||
self.selected_lead_track_id = -1
|
||||
self.required_decel = 0.0
|
||||
|
||||
@property
|
||||
def is_enabled(self) -> bool:
|
||||
return self.available and self.enabled
|
||||
|
||||
def update_params(self) -> None:
|
||||
if self._param_frame % self._param_read_frames == 0:
|
||||
self.enabled = self.params.get_bool("AccelPersonalityEnabled")
|
||||
self.profile = get_sanitize_int_param("AccelPersonality", AccelProfile.eco, AccelProfile.sport, self.params)
|
||||
self._param_frame += 1
|
||||
|
||||
@staticmethod
|
||||
def _profile(profile: int) -> int:
|
||||
return sanitize_profile(profile)
|
||||
|
||||
@staticmethod
|
||||
def get_profile_accel_max(profile: int, v_ego: float) -> float:
|
||||
return profile_accel_max(profile, v_ego)
|
||||
|
||||
def _update_target(self, lead_plan: LeadPlan, base_speed: float, v_ego: float, profile: int, profile_max_accel: float,
|
||||
previous_should_stop: bool, previous_mpc_source, planner_speed: float, planner_accel: float) -> float:
|
||||
state = self.target_state
|
||||
lead_filter_ready = state.update_samples(lead_plan, self.dt)
|
||||
state.active_frames += 1
|
||||
has_lead = lead_plan.selected_lead >= 0
|
||||
filtered_cap = state.filtered_cap
|
||||
slot_changed = has_lead and state.selected_lead >= 0 and lead_plan.selected_lead != state.selected_lead
|
||||
track_changed = (has_lead and state.selected_lead >= 0 and lead_plan.selected_lead == state.selected_lead
|
||||
and lead_plan.selected_lead_track_id != state.selected_lead_track_id
|
||||
and (state.selected_lead_track_id >= 0 or lead_plan.selected_lead_track_id >= 0))
|
||||
false_relief = has_lead and math.isfinite(filtered_cap) and lead_plan.cap >= filtered_cap + SPEED_RELIEF_DEADBAND
|
||||
if (slot_changed or track_changed) and false_relief and state.lead_switch_guard_frames == 0 and planner_accel <= PLANNER_BRAKING_ACCEL_THRESHOLD:
|
||||
state.lead_switch_guard_frames = self.lead_loss_hold_frames
|
||||
elif state.lead_switch_guard_frames > 0:
|
||||
state.lead_switch_guard_frames -= 1
|
||||
if slot_changed or track_changed:
|
||||
state.matched_lead = False
|
||||
state.matched_accel_limit = None
|
||||
if has_lead:
|
||||
state.selected_lead = lead_plan.selected_lead
|
||||
state.selected_lead_track_id = lead_plan.selected_lead_track_id
|
||||
elif state.lead_loss_frames >= self.lead_loss_hold_frames:
|
||||
state.lead_switch_guard_frames = 0
|
||||
state.selected_lead = state.selected_lead_track_id = -1
|
||||
departure_separation = (lead_plan.departure_lead_separations[lead_plan.departure_lead_index]
|
||||
if lead_plan.departure_lead_index >= 0 else math.inf)
|
||||
stopped_lead_hold = (has_lead and lead_plan.has_nearly_stopped_lead
|
||||
and (lead_plan.departure_cap < 0.50 or (state.lead_braking and departure_separation <= STOP_HOLD_MAX_LEAD_DISTANCE)))
|
||||
invalid_lead = lead_plan.lead_status and not has_lead
|
||||
prior_lead_context = is_lead_source(previous_mpc_source) or math.isfinite(filtered_cap) or state.lead_braking
|
||||
previous_stop = previous_should_stop and prior_lead_context and (not has_lead or lead_plan.departure_lead_speed < STOP_HOLD_EXIT_SPEED)
|
||||
stop_evidence = stopped_lead_hold or lead_plan.cap < 0.50 or filtered_cap < 0.50 or (previous_stop and not state.launching) or invalid_lead
|
||||
departure_motion_confirmed = (state.launching and state.departure_launch and has_lead
|
||||
and (state.departure.progress(lead_plan, DEPARTURE_MOTION_NOISE_FLOOR) or state.departure.recent_motion()))
|
||||
if state.active_frames >= self.lead_loss_hold_frames and math.isfinite(filtered_cap) and has_lead and planner_accel <= PLANNER_BRAKING_ACCEL_THRESHOLD:
|
||||
state.lead_braking = True
|
||||
elif not has_lead and state.lead_loss_frames >= self.lead_loss_hold_frames:
|
||||
state.lead_braking = False
|
||||
|
||||
if state.target_speed is None:
|
||||
e2e_handoff = previous_mpc_source == LongitudinalPlanSource.e2e
|
||||
seed_from_ego = has_lead and planner_accel > PLANNER_BRAKING_ACCEL_THRESHOLD and not e2e_handoff
|
||||
state.target_speed = min(base_speed, v_ego) if seed_from_ego else base_speed
|
||||
state.e2e_braking_handoff = e2e_handoff and planner_accel < 0.0
|
||||
state.state = AccelControllerState.free
|
||||
if v_ego < STOP_HOLD_EGO_SPEED and not stop_evidence:
|
||||
state.target_speed = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
|
||||
state.state = AccelControllerState.release
|
||||
state.launching = True
|
||||
state.departure_launch = False
|
||||
elif state.e2e_braking_handoff and planner_accel >= 0.0:
|
||||
state.e2e_braking_handoff = False
|
||||
|
||||
state.target_speed = min(state.target_speed, base_speed)
|
||||
if v_ego < STOP_HOLD_EGO_SPEED and stop_evidence and not departure_motion_confirmed and state.state != AccelControllerState.stopHold:
|
||||
state.enter_stop_hold(lead_plan)
|
||||
return state.target_speed
|
||||
|
||||
if state.state == AccelControllerState.stopHold:
|
||||
state.departure.backfill_references()
|
||||
fast_departure = (has_lead and min(lead_plan.selected_lead_speed, lead_plan.departure_lead_speed) > STOP_HOLD_EXIT_SPEED
|
||||
and lead_plan.departure_cap > STOP_HOLD_EXIT_SPEED)
|
||||
raw_departure = fast_departure or not lead_plan.lead_status and state.lead_loss_frames >= self.lead_loss_hold_frames
|
||||
departed = state.departure.progress(lead_plan, STOP_HOLD_CREEP_DISTANCE) or raw_departure
|
||||
if fast_departure and state.departure_frames == 0:
|
||||
state.departure.keep_latest_motion_sample()
|
||||
state.departure_frames = state.departure_frames + 1 if departed else 0
|
||||
state.target_speed = 0.0
|
||||
fast_departure_confirmed = fast_departure and state.departure.recent_motion()
|
||||
if state.departure_frames < STOP_HOLD_EXIT_FRAMES or fast_departure and not fast_departure_confirmed:
|
||||
return state.target_speed
|
||||
state.target_speed = base_speed
|
||||
state.state = AccelControllerState.release
|
||||
state.departure_frames = 0
|
||||
state.launching = True
|
||||
state.departure_launch = has_lead
|
||||
return state.target_speed
|
||||
|
||||
if state.launching:
|
||||
renewed_stop = (has_lead and not departure_motion_confirmed
|
||||
and (lead_plan.cap < STOP_HOLD_EXIT_SPEED
|
||||
or (lead_plan.has_nearly_stopped_lead and lead_plan.departure_cap < STOP_HOLD_EXIT_SPEED)))
|
||||
guarded_departure_loss = state.departure_launch and not lead_plan.lead_status and state.lead_loss_frames < self.lead_loss_hold_frames
|
||||
if invalid_lead:
|
||||
state.launching = state.departure_launch = False
|
||||
if v_ego < STOP_HOLD_EGO_SPEED:
|
||||
state.enter_stop_hold(lead_plan)
|
||||
return state.target_speed
|
||||
state.state = AccelControllerState.hold
|
||||
return state.target_speed
|
||||
if guarded_departure_loss:
|
||||
state.state = AccelControllerState.hold
|
||||
return state.target_speed
|
||||
if state.departure_launch and not has_lead:
|
||||
state.departure_launch = False
|
||||
if renewed_stop:
|
||||
state.launching = state.departure_launch = False
|
||||
if v_ego < STOP_HOLD_EGO_SPEED:
|
||||
state.enter_stop_hold(lead_plan)
|
||||
return state.target_speed
|
||||
if state.launching:
|
||||
if state.departure_launch:
|
||||
state.target_speed = base_speed
|
||||
else:
|
||||
launch_target = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM)
|
||||
state.target_speed = min(base_speed, max(state.target_speed, launch_target) + LAUNCH_TARGET_SLEW * self.dt)
|
||||
if v_ego >= LAUNCH_END_SPEED:
|
||||
state.launching = state.departure_launch = False
|
||||
|
||||
comfort_decel = COMFORT_DECEL[profile]
|
||||
if (has_lead and not state.launching and state.state == AccelControllerState.restrict
|
||||
and lead_plan.closing_speed <= 0.0 and v_ego >= state.filtered_lead_speed - VEGO_NOISE_TOLERANCE):
|
||||
state.matched_lead = True
|
||||
elif not has_lead and state.lead_loss_frames >= self.lead_loss_hold_frames:
|
||||
state.matched_lead = False
|
||||
|
||||
lost_lead_source = is_lead_source(previous_mpc_source) and not has_lead and planner_speed < state.target_speed
|
||||
if not has_lead and (state.matched_lead or lost_lead_source):
|
||||
if lost_lead_source:
|
||||
state.target_speed = max(planner_speed, state.target_speed - MATCHED_SPEED_DECEL_RATE * self.dt)
|
||||
state.state = AccelControllerState.hold
|
||||
return state.target_speed
|
||||
|
||||
if state.matched_lead:
|
||||
if math.isfinite(state.filtered_lead_speed):
|
||||
recovery_speed = min(base_speed, state.filtered_lead_speed + min(LEAD_MATCH_SPEED_HEADROOM, LEAD_MATCH_GAP_GAIN * lead_plan.usable_gap))
|
||||
desired_accel_limit = min(profile_max_accel, max(recovery_speed - v_ego, 0.0))
|
||||
else:
|
||||
desired_accel_limit = 0.0
|
||||
if state.filtered_lead_accel < LEAD_BRAKING_ACCEL_THRESHOLD:
|
||||
desired_accel_limit = profile_max_accel
|
||||
if state.matched_accel_limit is None:
|
||||
state.matched_accel_limit = profile_max_accel
|
||||
if state.lead_switch_guard_frames > 0:
|
||||
desired_accel_limit = min(desired_accel_limit, state.matched_accel_limit)
|
||||
state.matched_accel_limit = min(profile_max_accel, float(np.clip(
|
||||
desired_accel_limit, state.matched_accel_limit - LEAD_MATCH_ACCEL_SLEW * self.dt,
|
||||
state.matched_accel_limit + LEAD_MATCH_ACCEL_SLEW * self.dt,
|
||||
)))
|
||||
matched_ceiling = min(base_speed, filtered_cap)
|
||||
if matched_ceiling <= state.target_speed - SPEED_RESTRICT_DEADBAND:
|
||||
state.target_speed = max(matched_ceiling, state.target_speed - MATCHED_SPEED_DECEL_RATE * self.dt)
|
||||
state.state = AccelControllerState.restrict
|
||||
elif state.lead_switch_guard_frames == 0 and matched_ceiling >= state.target_speed + SPEED_RELIEF_DEADBAND:
|
||||
state.target_speed = min(matched_ceiling, state.target_speed + profile_max_accel * self.dt)
|
||||
state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.release
|
||||
else:
|
||||
state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.hold
|
||||
return state.target_speed
|
||||
state.matched_accel_limit = None
|
||||
|
||||
ceiling = min(base_speed, filtered_cap)
|
||||
if lead_filter_ready and state.active_frames == CAP_FILTER_FRAMES // 2 + 1 and not state.launching and planner_speed < state.target_speed:
|
||||
state.target_speed = max(planner_speed, state.target_speed - comfort_decel * self.dt)
|
||||
|
||||
if ceiling <= state.target_speed - SPEED_RESTRICT_DEADBAND or (state.state == AccelControllerState.restrict and ceiling < state.target_speed):
|
||||
state.target_speed = max(ceiling, state.target_speed - comfort_decel * self.dt)
|
||||
state.state = AccelControllerState.restrict
|
||||
return state.target_speed
|
||||
|
||||
filter_warmup = has_lead and not math.isfinite(filtered_cap)
|
||||
guarded_lead_loss = not has_lead and state.lead_loss_frames < self.lead_loss_hold_frames
|
||||
if (filter_warmup or guarded_lead_loss) and state.target_speed < base_speed - SPEED_RESTRICT_DEADBAND:
|
||||
state.state = AccelControllerState.hold
|
||||
return state.target_speed
|
||||
|
||||
confirmed_clear_road = not math.isfinite(filtered_cap) and not guarded_lead_loss
|
||||
relief = not has_lead or lead_plan.closing_speed <= 0.0
|
||||
if relief and (ceiling >= state.target_speed + SPEED_RELIEF_DEADBAND or (confirmed_clear_road and ceiling > state.target_speed)):
|
||||
if state.lead_switch_guard_frames == 0:
|
||||
state.target_speed = ceiling
|
||||
state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.release
|
||||
else:
|
||||
state.state = AccelControllerState.free if state.target_speed >= base_speed - SPEED_RESTRICT_DEADBAND else AccelControllerState.hold
|
||||
return state.target_speed
|
||||
|
||||
def _update_freshness(self, radar_fresh: bool) -> None:
|
||||
self.target_state.stale_frames = 0 if radar_fresh else self.target_state.stale_frames + 1
|
||||
if self.target_state.stale_frames >= self.radar_stale_frames:
|
||||
self.target_state = TargetState()
|
||||
|
||||
def reset(self) -> None:
|
||||
self.target_state = TargetState()
|
||||
self._held_lead_plan = None
|
||||
self._jerk_smoothing_blocked = False
|
||||
self._required_decel_samples.clear()
|
||||
self._required_decel_lead = self._required_decel_lead_track_id = -1
|
||||
self._lead_trend_warmup = False
|
||||
self.is_active = self.launching = self.departure_launching = False
|
||||
self.output_v_target = 0.0
|
||||
self.mpc_accel_max = None
|
||||
self.state = AccelControllerState.inactive
|
||||
self.selected_lead = -1
|
||||
self.selected_lead_track_id = -1
|
||||
self.required_decel = 0.0
|
||||
|
||||
def update(self, radar_state, *, base_speed: float, v_ego: float, a_ego: float, follow_personality, acc_selected: bool,
|
||||
engaged: bool, cruise_initialized: bool, stock_accel_max: float, previous_should_stop: bool, radar_fresh: bool = True,
|
||||
previous_mpc_source=None, planner_speed: float | None = None, planner_accel: float = 0.0) -> None:
|
||||
self.profile = self._profile(self.profile)
|
||||
sanitized_v_ego = max(v_ego, 0.0) if math.isfinite(v_ego) and v_ego >= -VEGO_NOISE_TOLERANCE else v_ego
|
||||
profile_max_accel = self.get_profile_accel_max(self.profile, sanitized_v_ego)
|
||||
stock_accel_max = float(stock_accel_max)
|
||||
positive_accel_max = (max(0.0, min(profile_max_accel, stock_accel_max, ACCEL_MAX))
|
||||
if math.isfinite(profile_max_accel) and math.isfinite(stock_accel_max) else math.nan)
|
||||
planner_speed = sanitized_v_ego if planner_speed is None else planner_speed
|
||||
valid_context = is_valid_context(base_speed, sanitized_v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, self.delay,
|
||||
engaged, cruise_initialized)
|
||||
enabled_context = valid_context and self.is_enabled and bool(acc_selected)
|
||||
if enabled_context and radar_fresh:
|
||||
lead_plan = calculate_lead_plan(radar_state, sanitized_v_ego, a_ego, self.delay, self.profile, follow_personality)
|
||||
self._held_lead_plan = lead_plan
|
||||
elif enabled_context and self._held_lead_plan is not None:
|
||||
lead_plan = self._held_lead_plan
|
||||
else:
|
||||
lead_plan = LeadPlan(lead_status=has_radar_lead(radar_state))
|
||||
self._held_lead_plan = None
|
||||
|
||||
if enabled_context:
|
||||
self._update_freshness(radar_fresh)
|
||||
active = enabled_context and (radar_fresh or self.target_state.target_speed is not None)
|
||||
if active and radar_fresh:
|
||||
target_speed = self._update_target(
|
||||
lead_plan, base_speed, sanitized_v_ego, self.profile, profile_max_accel, previous_should_stop,
|
||||
previous_mpc_source, planner_speed, planner_accel,
|
||||
)
|
||||
elif active:
|
||||
target_speed = self.target_state.target_speed
|
||||
else:
|
||||
self.target_state = TargetState()
|
||||
target_speed = base_speed
|
||||
|
||||
if not radar_fresh and not active:
|
||||
self._held_lead_plan = None
|
||||
lead_plan = LeadPlan(lead_status=has_radar_lead(radar_state))
|
||||
|
||||
state = self.target_state
|
||||
stop_hold_active = active and state.state == AccelControllerState.stopHold
|
||||
matched_limit_active = active and state.matched_lead and state.matched_accel_limit is not None and not state.e2e_braking_handoff
|
||||
lead_accel_request = active and lead_plan.selected_lead >= 0 and lead_plan.closing_speed <= 0.0 and planner_accel >= 0.0
|
||||
profile_limit_active = active and not stop_hold_active and (state.launching or not lead_plan.lead_status or lead_accel_request)
|
||||
if matched_limit_active:
|
||||
effective_accel_max = min(positive_accel_max, state.matched_accel_limit)
|
||||
elif profile_limit_active:
|
||||
effective_accel_max = positive_accel_max
|
||||
else:
|
||||
effective_accel_max = math.inf
|
||||
mpc_accel_max = build_accel_ceiling(effective_accel_max, planner_accel) if matched_limit_active or profile_limit_active else None
|
||||
guarded_lead_loss = not lead_plan.lead_status and state.selected_lead >= 0 and state.lead_loss_frames < self.lead_loss_hold_frames
|
||||
lead_context = lead_plan.lead_status or math.isfinite(state.filtered_cap) or guarded_lead_loss
|
||||
reserve_eligible = (active and lead_context and not stop_hold_active and state.lead_switch_guard_frames == 0 and not state.launching
|
||||
and not state.e2e_braking_handoff)
|
||||
if not lead_context:
|
||||
state.speed_reserve_armed = False
|
||||
elif (reserve_eligible and not state.speed_reserve_armed and math.isfinite(state.filtered_cap)
|
||||
and state.filtered_cap <= target_speed + TARGET_SPEED_ARM_MARGIN):
|
||||
state.speed_reserve_armed = True
|
||||
|
||||
output_target = 0.0 if stop_hold_active else target_speed
|
||||
if reserve_eligible and state.speed_reserve_armed:
|
||||
output_target = max(0.0, output_target - TARGET_SPEED_RESERVE)
|
||||
|
||||
self.is_active = active
|
||||
self.launching = active and state.launching
|
||||
self.departure_launching = self.launching and state.departure_launch
|
||||
self.output_v_target = output_target
|
||||
self.mpc_accel_max = mpc_accel_max
|
||||
self.state = state.state
|
||||
self.selected_lead = lead_plan.selected_lead
|
||||
self.selected_lead_track_id = lead_plan.selected_lead_track_id
|
||||
self.required_decel = lead_plan.required_decel
|
||||
|
||||
def get_jerk_cost_multiplier(self, actuating: bool, prev_accel_constraint: bool, target_reduction: float, previous_mpc_failed: bool) -> float:
|
||||
lead_restriction = (actuating and prev_accel_constraint and self.state == AccelControllerState.restrict and self.selected_lead >= 0
|
||||
and not self.launching and target_reduction > 1e-6)
|
||||
same_lead = self.selected_lead == self._required_decel_lead and self.selected_lead_track_id == self._required_decel_lead_track_id
|
||||
lead_changed = lead_restriction and self._required_decel_lead >= 0 and not same_lead
|
||||
if lead_changed:
|
||||
self._lead_trend_warmup = True
|
||||
elif not lead_restriction:
|
||||
self._lead_trend_warmup = False
|
||||
if not lead_restriction or not same_lead or not math.isfinite(self.required_decel):
|
||||
self._required_decel_samples.clear()
|
||||
if lead_restriction and math.isfinite(self.required_decel):
|
||||
self._required_decel_samples.append(self.required_decel)
|
||||
if len(self._required_decel_samples) > MPC_DECEL_TREND_FRAMES:
|
||||
self._required_decel_samples.pop(0)
|
||||
self._required_decel_lead = self.selected_lead if lead_restriction else -1
|
||||
self._required_decel_lead_track_id = self.selected_lead_track_id if lead_restriction else -1
|
||||
|
||||
history = self._required_decel_samples
|
||||
history_ready = len(history) == MPC_DECEL_TREND_FRAMES
|
||||
tightening_lead = (history_ready
|
||||
and (history[-1] - history[0]) / (self.dt * (len(history) - 1)) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE
|
||||
and sum(after > before for before, after in zip(history[:-1], history[1:], strict=True)) >= 2)
|
||||
modest_decel = (lead_restriction and target_reduction < MPC_DECEL_JERK_MAX_TARGET_REDUCTION
|
||||
and 0.0 < self.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL)
|
||||
smoothing_eligible = modest_decel and (not self._lead_trend_warmup or history_ready) and not tightening_lead
|
||||
if history_ready:
|
||||
self._lead_trend_warmup = False
|
||||
if previous_mpc_failed or (lead_restriction and not self._jerk_smoothing_blocked and (not modest_decel or tightening_lead)):
|
||||
self._jerk_smoothing_blocked = True
|
||||
elif not lead_restriction:
|
||||
self._jerk_smoothing_blocked = False
|
||||
return MPC_DECEL_JERK_COST_MULTIPLIER if smoothing_eligible and not self._jerk_smoothing_blocked else 1.0
|
||||
|
||||
def update_should_stop(self, should_stop: bool) -> bool:
|
||||
if not self.is_active:
|
||||
return should_stop
|
||||
if self.departure_launching:
|
||||
return False
|
||||
return should_stop or self.state == AccelControllerState.stopHold
|
||||
@@ -0,0 +1,73 @@
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
from cereal import custom
|
||||
|
||||
|
||||
AccelProfile = custom.LongitudinalPlanSP.AccelController.Profile
|
||||
ACCEL_PROFILES = tuple(AccelProfile.schema.enumerants.values())
|
||||
|
||||
COMFORT_DECEL = {
|
||||
AccelProfile.eco: 0.25,
|
||||
AccelProfile.normal: 0.30,
|
||||
AccelProfile.sport: 0.35,
|
||||
}
|
||||
|
||||
ACCEL_PROFILE_MAX_BP = [0.0, 3.0, 10.0, 25.0, 40.0]
|
||||
ACCEL_PROFILE_MAX_V = {
|
||||
AccelProfile.eco: [1.65, 1.30, 0.72, 0.32, 0.16],
|
||||
AccelProfile.normal: [1.80, 1.50, 0.97, 0.48, 0.30],
|
||||
AccelProfile.sport: [2.00, 1.90, 1.15, 0.68, 0.42],
|
||||
}
|
||||
|
||||
CAP_FILTER_FRAMES = 5
|
||||
LEAD_LOSS_HOLD_TIME = 0.50
|
||||
SPEED_RESTRICT_DEADBAND = 0.15
|
||||
SPEED_RELIEF_DEADBAND = 0.35
|
||||
TARGET_SPEED_ARM_MARGIN = 1.0
|
||||
TARGET_SPEED_RESERVE = 0.10
|
||||
LAUNCH_TARGET_HEADROOM = 3.0
|
||||
LAUNCH_TARGET_SLEW = 8.75
|
||||
LAUNCH_END_SPEED = 3.0
|
||||
ACCEL_LIMIT_HORIZON_JERK = 1.0
|
||||
LEAD_MATCH_GAP_GAIN = 0.04
|
||||
LEAD_MATCH_SPEED_HEADROOM = 1.25
|
||||
LEAD_MATCH_ACCEL_SLEW = 0.25
|
||||
MATCHED_SPEED_DECEL_RATE = 0.50
|
||||
PLANNER_BRAKING_ACCEL_THRESHOLD = -0.11
|
||||
LEAD_BRAKING_ACCEL_THRESHOLD = -0.11
|
||||
MPC_DECEL_JERK_COST_MULTIPLIER = 1.05
|
||||
MPC_DECEL_JERK_MAX_REQUIRED_DECEL = 0.80
|
||||
MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE = 0.35
|
||||
MPC_DECEL_JERK_MAX_TARGET_REDUCTION = 9.0
|
||||
MPC_DECEL_TREND_FRAMES = 4
|
||||
|
||||
STOP_HOLD_EGO_SPEED = 0.30
|
||||
STOPPED_LEAD_SPEED = 0.30
|
||||
STOP_HOLD_EXIT_SPEED = 0.80
|
||||
STOP_HOLD_EXIT_FRAMES = 4
|
||||
STOP_HOLD_CREEP_SPEED = 0.15
|
||||
STOP_HOLD_CREEP_DISTANCE = 0.30
|
||||
DEPARTURE_MOTION_NOISE_FLOOR = 0.03
|
||||
DEPARTURE_MOTION_STEP_MIN = 0.005
|
||||
STOP_HOLD_MAX_LEAD_DISTANCE = 30.0
|
||||
STOP_GAP_RESERVE = 0.75
|
||||
STOP_GAP_RESERVE_LEAD_SPEED = 2.0
|
||||
STOP_GAP_RESERVE_DECEL_BP = (0.30, 0.80)
|
||||
|
||||
RADAR_STALE_TIMEOUT = 0.50
|
||||
MAX_LEAD_ACCEL_TAU = 10.0
|
||||
MIN_LEAD_SPEED = -1.0
|
||||
VEGO_NOISE_TOLERANCE = 0.10
|
||||
PARAM_READ_INTERVAL = 0.25
|
||||
|
||||
|
||||
def sanitize_profile(profile: int) -> int:
|
||||
return profile if profile in ACCEL_PROFILES else AccelProfile.normal
|
||||
|
||||
|
||||
def profile_accel_max(profile: int, v_ego: float) -> float:
|
||||
if not math.isfinite(v_ego):
|
||||
return math.nan
|
||||
return float(np.interp(max(v_ego, 0.0), ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[sanitize_profile(profile)]))
|
||||
@@ -0,0 +1,22 @@
|
||||
import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ACCEL_LIMIT_HORIZON_JERK, VEGO_NOISE_TOLERANCE
|
||||
|
||||
|
||||
def is_valid_context(base_speed: float, v_ego: float, a_ego: float, planner_speed: float, planner_accel: float, stock_accel_max: float,
|
||||
delay: float, engaged: bool, cruise_initialized: bool) -> bool:
|
||||
values = (base_speed, v_ego, a_ego, planner_speed, planner_accel, stock_accel_max, delay)
|
||||
return (engaged and cruise_initialized and base_speed >= 0.0 and v_ego >= -VEGO_NOISE_TOLERANCE
|
||||
and planner_speed >= 0.0 and stock_accel_max >= 0.0 and delay >= 0.0 and all(math.isfinite(value) for value in values))
|
||||
|
||||
|
||||
def build_accel_ceiling(limit: float, planner_accel: float) -> tuple[float, ...] | None:
|
||||
if limit >= ACCEL_MAX - 1e-9:
|
||||
return None
|
||||
a0 = float(np.clip(planner_accel, ACCEL_MIN, ACCEL_MAX))
|
||||
ceiling = np.clip(np.maximum(limit, a0 - ACCEL_LIMIT_HORIZON_JERK * T_IDXS), 0.0, ACCEL_MAX)
|
||||
return tuple(float(value) for value in ceiling)
|
||||
@@ -0,0 +1,147 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, 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.
|
||||
"""
|
||||
|
||||
import math
|
||||
from typing import NamedTuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
from cereal import log
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
|
||||
LongitudinalMpc, LongitudinalPlanSource, STOP_DISTANCE, T_IDXS, get_T_FOLLOW, get_stopped_equivalence_factor,
|
||||
)
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
|
||||
COMFORT_DECEL, MAX_LEAD_ACCEL_TAU, MIN_LEAD_SPEED, STOP_GAP_RESERVE, STOP_GAP_RESERVE_DECEL_BP,
|
||||
STOP_GAP_RESERVE_LEAD_SPEED, STOPPED_LEAD_SPEED, sanitize_profile,
|
||||
)
|
||||
|
||||
|
||||
class LeadPlan(NamedTuple):
|
||||
cap: float = math.inf
|
||||
selected_lead: int = -1
|
||||
selected_lead_track_id: int = -1
|
||||
selected_lead_speed: float = math.inf
|
||||
selected_lead_accel: float = 0.0
|
||||
departure_lead_index: int = -1
|
||||
departure_lead_speed: float = math.inf
|
||||
departure_cap: float = math.inf
|
||||
departure_lead_speeds: tuple[float, float] = (math.inf, math.inf)
|
||||
departure_lead_distances: tuple[float, float] = (-math.inf, -math.inf)
|
||||
departure_lead_track_ids: tuple[int, int] = (-1, -1)
|
||||
departure_lead_separations: tuple[float, float] = (-math.inf, -math.inf)
|
||||
usable_gap: float = math.inf
|
||||
closing_speed: float = 0.0
|
||||
required_decel: float = 0.0
|
||||
has_nearly_stopped_lead: bool = False
|
||||
lead_status: bool = False
|
||||
|
||||
|
||||
def is_lead_source(source) -> bool:
|
||||
return source in (LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1)
|
||||
|
||||
|
||||
def has_radar_lead(radar_state) -> bool:
|
||||
return bool(radar_state.leadOne.status or radar_state.leadTwo.status)
|
||||
|
||||
|
||||
def _project_ego(v_ego: float, a_ego: float, delay: float) -> tuple[float, float]:
|
||||
if a_ego < 0.0:
|
||||
stop_time = -v_ego / a_ego if v_ego > 0.0 else 0.0
|
||||
if stop_time <= delay:
|
||||
distance = -v_ego**2 / (2.0 * a_ego) if v_ego > 0.0 else 0.0
|
||||
return distance, 0.0
|
||||
return max(v_ego * delay + 0.5 * a_ego * delay**2, 0.0), max(v_ego + a_ego * delay, 0.0)
|
||||
|
||||
|
||||
def _lead_values(lead) -> tuple[float, float, float, float] | None:
|
||||
if not lead.status:
|
||||
return None
|
||||
d_rel, v_lead = float(lead.dRel), float(lead.vLeadK)
|
||||
if not math.isfinite(d_rel) or d_rel < 0.0 or not math.isfinite(v_lead) or v_lead < MIN_LEAD_SPEED:
|
||||
return None
|
||||
|
||||
a_lead = float(lead.aLeadK)
|
||||
if not math.isfinite(a_lead):
|
||||
a_lead = 0.0
|
||||
a_lead_tau = float(lead.aLeadTau)
|
||||
if not math.isfinite(a_lead_tau) or not 0.0 < a_lead_tau <= MAX_LEAD_ACCEL_TAU:
|
||||
a_lead_tau = _LEAD_ACCEL_TAU
|
||||
return d_rel, max(v_lead, 0.0), float(np.clip(a_lead, -10.0, 5.0)), a_lead_tau
|
||||
|
||||
|
||||
def calculate_lead_plan(radar_state, v_ego: float, a_ego: float, delay: float, profile: int,
|
||||
follow_personality=log.LongitudinalPersonality.standard) -> LeadPlan:
|
||||
if not all(math.isfinite(value) for value in (v_ego, a_ego, delay)) or v_ego < 0.0 or delay < 0.0:
|
||||
return LeadPlan()
|
||||
|
||||
leads = (radar_state.leadOne, radar_state.leadTwo)
|
||||
lead_status = any(lead.status for lead in leads)
|
||||
t_follow = get_T_FOLLOW(follow_personality)
|
||||
if not math.isfinite(t_follow) or t_follow < 0.0:
|
||||
return LeadPlan(lead_status=lead_status)
|
||||
|
||||
profile = sanitize_profile(profile)
|
||||
x_ego, v_ego_delay = _project_ego(v_ego, a_ego, delay)
|
||||
comfort_decel = COMFORT_DECEL[profile]
|
||||
candidates: list[LeadPlan] = []
|
||||
departure_candidates: list[tuple[float, int]] = []
|
||||
departure_speeds = [math.inf, math.inf]
|
||||
departure_distances = [-math.inf, -math.inf]
|
||||
departure_track_ids = [-1, -1]
|
||||
departure_separations = [-math.inf, -math.inf]
|
||||
departure_caps = [math.inf, math.inf]
|
||||
|
||||
for lead_index, lead in enumerate(leads):
|
||||
values = _lead_values(lead)
|
||||
if values is None:
|
||||
continue
|
||||
|
||||
d_rel, v_lead, a_lead, a_lead_tau = values
|
||||
lead_xv = LongitudinalMpc.extrapolate_lead(d_rel, v_lead, a_lead, a_lead_tau)
|
||||
x_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
|
||||
v_lead_delay = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
|
||||
safety_gap = max(x_lead - x_ego - STOP_DISTANCE - t_follow * v_lead_delay, 0.0)
|
||||
closing_speed = max(v_ego_delay - v_lead_delay, 0.0)
|
||||
required_decel = 0.0 if closing_speed == 0.0 else math.inf if safety_gap == 0.0 else closing_speed**2 / (2.0 * safety_gap)
|
||||
reserve = float(np.interp(v_lead_delay, (0.0, STOP_GAP_RESERVE_LEAD_SPEED), (STOP_GAP_RESERVE, 0.0)))
|
||||
reserve_scale = float(np.interp(required_decel, STOP_GAP_RESERVE_DECEL_BP, (1.0, 0.0)))
|
||||
usable_gap = max(safety_gap - reserve * reserve_scale, 0.0)
|
||||
cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * usable_gap)
|
||||
departure_cap = v_lead_delay + math.sqrt(2.0 * comfort_decel * safety_gap)
|
||||
separation = x_lead - x_ego
|
||||
departure_distance = x_lead + float(get_stopped_equivalence_factor(v_lead_delay))
|
||||
|
||||
finite_values = (x_lead, v_lead_delay, safety_gap, usable_gap, closing_speed, cap, departure_cap, departure_distance)
|
||||
if (not all(math.isfinite(value) and value >= 0.0 for value in finite_values) or math.isnan(required_decel)
|
||||
or required_decel < 0.0 or not math.isfinite(separation)):
|
||||
continue
|
||||
|
||||
track_id = max(int(lead.radarTrackId), -1) if math.isfinite(lead.radarTrackId) else -1
|
||||
candidates.append(LeadPlan(
|
||||
cap=cap, selected_lead=lead_index, selected_lead_track_id=track_id, selected_lead_speed=v_lead_delay, selected_lead_accel=a_lead,
|
||||
usable_gap=usable_gap, closing_speed=closing_speed, required_decel=required_decel, lead_status=lead_status,
|
||||
))
|
||||
departure_candidates.append((departure_distance, lead_index))
|
||||
departure_speeds[lead_index] = v_lead_delay
|
||||
departure_distances[lead_index] = d_rel
|
||||
departure_track_ids[lead_index] = track_id
|
||||
departure_separations[lead_index] = separation
|
||||
departure_caps[lead_index] = departure_cap
|
||||
|
||||
if not candidates:
|
||||
return LeadPlan(lead_status=lead_status)
|
||||
|
||||
selected = min(candidates, key=lambda candidate: candidate.cap)
|
||||
departure_lead_index = min(departure_candidates, key=lambda candidate: candidate[0])[1]
|
||||
departure_lead_speed = departure_speeds[departure_lead_index]
|
||||
return selected._replace(
|
||||
departure_lead_index=departure_lead_index, departure_lead_speed=departure_lead_speed,
|
||||
departure_cap=departure_caps[departure_lead_index], departure_lead_speeds=tuple(departure_speeds),
|
||||
departure_lead_distances=tuple(departure_distances), departure_lead_track_ids=tuple(departure_track_ids),
|
||||
departure_lead_separations=tuple(departure_separations), has_nearly_stopped_lead=departure_lead_speed < STOPPED_LEAD_SPEED,
|
||||
)
|
||||
@@ -0,0 +1,139 @@
|
||||
import math
|
||||
from statistics import median
|
||||
|
||||
import numpy as np
|
||||
|
||||
from cereal import custom
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
|
||||
CAP_FILTER_FRAMES, DEPARTURE_MOTION_NOISE_FLOOR, DEPARTURE_MOTION_STEP_MIN, STOP_HOLD_CREEP_DISTANCE, STOP_HOLD_CREEP_SPEED, STOP_HOLD_EXIT_FRAMES,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan
|
||||
|
||||
|
||||
AccelControllerState = custom.LongitudinalPlanSP.AccelController.State
|
||||
|
||||
|
||||
class DepartureTracker:
|
||||
def __init__(self) -> None:
|
||||
self.samples: list[list[float]] = [[], []]
|
||||
self.motion_samples: list[float] = []
|
||||
self.references: list[float | None] = [None, None]
|
||||
self.track_ids = [-1, -1]
|
||||
|
||||
def separation(self, lead_index: int) -> float:
|
||||
samples = self.samples[lead_index]
|
||||
return float(median(samples)) if samples else -math.inf
|
||||
|
||||
def update(self, lead_plan: LeadPlan, dt: float) -> None:
|
||||
for lead_index, distance in enumerate(lead_plan.departure_lead_distances):
|
||||
if not math.isfinite(distance):
|
||||
continue
|
||||
samples = self.samples[lead_index]
|
||||
track_id = lead_plan.departure_lead_track_ids[lead_index]
|
||||
identity_changed = bool(samples) and track_id != self.track_ids[lead_index] and (track_id >= 0 or self.track_ids[lead_index] >= 0)
|
||||
max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * lead_plan.departure_lead_speeds[lead_index] * dt)
|
||||
geometry_jump = bool(samples) and abs(distance - samples[-1]) > max_distance_step
|
||||
if identity_changed or geometry_jump:
|
||||
samples.clear()
|
||||
self.references[lead_index] = distance
|
||||
samples.append(distance)
|
||||
if len(samples) > CAP_FILTER_FRAMES:
|
||||
samples.pop(0)
|
||||
self.track_ids[lead_index] = track_id
|
||||
lead_index = lead_plan.departure_lead_index
|
||||
if lead_index >= 0:
|
||||
distance = lead_plan.departure_lead_distances[lead_index]
|
||||
samples = self.motion_samples
|
||||
max_distance_step = max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * lead_plan.departure_lead_speed * dt)
|
||||
if samples and abs(distance - samples[-1]) > max_distance_step:
|
||||
samples.clear()
|
||||
samples.append(distance)
|
||||
if len(samples) > CAP_FILTER_FRAMES:
|
||||
samples.pop(0)
|
||||
|
||||
def seed(self, lead_plan: LeadPlan) -> None:
|
||||
self.samples = [[], []]
|
||||
self.motion_samples = []
|
||||
self.references = [None, None]
|
||||
self.track_ids = list(lead_plan.departure_lead_track_ids)
|
||||
for lead_index, distance in enumerate(lead_plan.departure_lead_distances):
|
||||
if math.isfinite(distance):
|
||||
self.samples[lead_index].append(distance)
|
||||
self.references[lead_index] = distance
|
||||
if lead_plan.departure_lead_index >= 0:
|
||||
self.motion_samples.append(lead_plan.departure_lead_distances[lead_plan.departure_lead_index])
|
||||
|
||||
def progress(self, lead_plan: LeadPlan, minimum_distance: float) -> bool:
|
||||
lead_index = lead_plan.departure_lead_index
|
||||
if lead_index < 0 or lead_plan.departure_lead_speed <= STOP_HOLD_CREEP_SPEED:
|
||||
return False
|
||||
reference = self.references[lead_index]
|
||||
distance = self.separation(lead_index)
|
||||
return reference is not None and distance - reference >= minimum_distance
|
||||
|
||||
def recent_motion(self) -> bool:
|
||||
samples = self.motion_samples[-STOP_HOLD_EXIT_FRAMES:]
|
||||
if len(samples) < STOP_HOLD_EXIT_FRAMES:
|
||||
return False
|
||||
deltas = np.diff(samples)
|
||||
return bool(samples[-1] - samples[0] >= DEPARTURE_MOTION_NOISE_FLOOR and np.count_nonzero(deltas > DEPARTURE_MOTION_STEP_MIN) >= 2)
|
||||
|
||||
def backfill_references(self) -> None:
|
||||
for lead_index in range(len(self.references)):
|
||||
separation = self.separation(lead_index)
|
||||
if math.isfinite(separation) and self.references[lead_index] is None:
|
||||
self.references[lead_index] = separation
|
||||
|
||||
def keep_latest_motion_sample(self) -> None:
|
||||
if self.motion_samples:
|
||||
self.motion_samples = self.motion_samples[-1:]
|
||||
|
||||
|
||||
class TargetState:
|
||||
def __init__(self) -> None:
|
||||
self.cap_samples = [math.inf] * CAP_FILTER_FRAMES
|
||||
self.lead_speed_samples = [math.inf] * CAP_FILTER_FRAMES
|
||||
self.lead_accel_samples = [0.0] * CAP_FILTER_FRAMES
|
||||
self.departure = DepartureTracker()
|
||||
self.target_speed: float | None = None
|
||||
self.state = AccelControllerState.inactive
|
||||
self.departure_frames = self.active_frames = self.lead_loss_frames = 0
|
||||
self.lead_switch_guard_frames = self.stale_frames = 0
|
||||
self.selected_lead = self.selected_lead_track_id = -1
|
||||
self.launching = self.departure_launch = self.matched_lead = False
|
||||
self.lead_braking = self.e2e_braking_handoff = self.speed_reserve_armed = False
|
||||
self.matched_accel_limit: float | None = None
|
||||
|
||||
@property
|
||||
def filtered_cap(self) -> float:
|
||||
return sorted(self.cap_samples)[CAP_FILTER_FRAMES // 2]
|
||||
|
||||
@property
|
||||
def filtered_lead_speed(self) -> float:
|
||||
return sorted(self.lead_speed_samples)[CAP_FILTER_FRAMES // 2]
|
||||
|
||||
@property
|
||||
def filtered_lead_accel(self) -> float:
|
||||
return sorted(self.lead_accel_samples)[CAP_FILTER_FRAMES // 2]
|
||||
|
||||
def update_samples(self, lead_plan: LeadPlan, dt: float) -> bool:
|
||||
had_filtered_lead = math.isfinite(self.filtered_cap)
|
||||
has_lead = lead_plan.selected_lead >= 0
|
||||
self.cap_samples.append(lead_plan.cap if has_lead else math.inf)
|
||||
self.lead_speed_samples.append(lead_plan.selected_lead_speed if has_lead else math.inf)
|
||||
self.lead_accel_samples.append(lead_plan.selected_lead_accel if has_lead else 0.0)
|
||||
self.cap_samples.pop(0)
|
||||
self.lead_speed_samples.pop(0)
|
||||
self.lead_accel_samples.pop(0)
|
||||
self.lead_loss_frames = 0 if has_lead else self.lead_loss_frames + 1
|
||||
self.departure.update(lead_plan, dt)
|
||||
return not had_filtered_lead and math.isfinite(self.filtered_cap)
|
||||
|
||||
def enter_stop_hold(self, lead_plan: LeadPlan) -> None:
|
||||
self.departure.seed(lead_plan)
|
||||
self.target_speed = 0.0
|
||||
self.state = AccelControllerState.stopHold
|
||||
self.departure_frames = 0
|
||||
self.launching = self.departure_launch = False
|
||||
self.matched_lead = self.speed_reserve_armed = False
|
||||
self.matched_accel_limit = None
|
||||
@@ -0,0 +1,882 @@
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from cereal import log
|
||||
from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import (
|
||||
STOP_DISTANCE, T_IDXS, LongitudinalMpc, LongitudinalPlanSource, get_T_FOLLOW,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
|
||||
ACCEL_LIMIT_HORIZON_JERK, ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V, ACCEL_PROFILES, CAP_FILTER_FRAMES, LAUNCH_END_SPEED,
|
||||
COMFORT_DECEL, LAUNCH_TARGET_HEADROOM, LAUNCH_TARGET_SLEW, LEAD_MATCH_ACCEL_SLEW, MATCHED_SPEED_DECEL_RATE,
|
||||
MPC_DECEL_JERK_COST_MULTIPLIER, TARGET_SPEED_RESERVE, RADAR_STALE_TIMEOUT, STOP_GAP_RESERVE, STOP_HOLD_EXIT_FRAMES, AccelProfile,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.helpers import build_accel_ceiling
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import _project_ego, calculate_lead_plan
|
||||
|
||||
|
||||
def make_lead(*, status=False, d_rel=0.0, v_lead_k=0.0, a_lead_k=0.0, a_lead_tau=1.5, radar_track_id=-1):
|
||||
return SimpleNamespace(status=status, dRel=d_rel, vLeadK=v_lead_k, aLeadK=a_lead_k, aLeadTau=a_lead_tau,
|
||||
radarTrackId=radar_track_id)
|
||||
|
||||
|
||||
def make_radar(lead_one=None, lead_two=None):
|
||||
return SimpleNamespace(leadOne=lead_one or make_lead(), leadTwo=lead_two or make_lead())
|
||||
|
||||
|
||||
def make_controller(delay=0.10):
|
||||
return AccelController(SimpleNamespace(longitudinalActuatorDelay=delay, openpilotLongitudinalControl=True))
|
||||
|
||||
|
||||
def get_lead_plan(controller, radar_state, v_ego: float, a_ego: float, profile: int):
|
||||
return calculate_lead_plan(radar_state, v_ego, a_ego, controller.delay, profile)
|
||||
|
||||
|
||||
def update(controller, radar_state=None, **overrides):
|
||||
args = {
|
||||
"base_speed": 25.0,
|
||||
"v_ego": 10.0,
|
||||
"a_ego": 0.0,
|
||||
"profile": AccelProfile.normal,
|
||||
"follow_personality": log.LongitudinalPersonality.standard,
|
||||
"enabled": True,
|
||||
"acc_selected": True,
|
||||
"engaged": True,
|
||||
"cruise_initialized": True,
|
||||
"stock_accel_max": ACCEL_MAX,
|
||||
"previous_should_stop": False,
|
||||
}
|
||||
args.update(overrides)
|
||||
controller.profile = args.pop("profile")
|
||||
controller.enabled = args.pop("enabled")
|
||||
controller.update(radar_state or make_radar(), **args)
|
||||
return SimpleNamespace(
|
||||
target_speed=controller.output_v_target, active=controller.is_active, launching=controller.launching,
|
||||
departure_launching=controller.departure_launching, mpc_accel_max=controller.mpc_accel_max, state=controller.state,
|
||||
selected_lead=controller.selected_lead, required_decel=controller.required_decel,
|
||||
)
|
||||
|
||||
|
||||
def effective_accel_max(result):
|
||||
return math.inf if result.mpc_accel_max is None else min(result.mpc_accel_max)
|
||||
|
||||
|
||||
def restrictive_radar():
|
||||
return make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-0.5))
|
||||
|
||||
|
||||
def enter_stop_hold(controller, *, base_speed=8.0, v_ego=0.1):
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
|
||||
return update(controller, stopped, base_speed=base_speed, v_ego=v_ego, previous_should_stop=True)
|
||||
|
||||
|
||||
class TestProfiles:
|
||||
def test_lookup_table_is_explicit_and_tunable(self):
|
||||
assert ACCEL_PROFILE_MAX_BP == [0.0, 3.0, 10.0, 25.0, 40.0]
|
||||
assert ACCEL_PROFILE_MAX_V == {
|
||||
AccelProfile.eco: [1.65, 1.30, 0.72, 0.32, 0.16],
|
||||
AccelProfile.normal: [1.80, 1.50, 0.97, 0.48, 0.30],
|
||||
AccelProfile.sport: [2.00, 1.90, 1.15, 0.68, 0.42],
|
||||
}
|
||||
|
||||
@pytest.mark.parametrize("profile", ACCEL_PROFILES)
|
||||
def test_lookup_interpolates_and_stays_inside_global_limit(self, profile):
|
||||
for speed, expected in zip(ACCEL_PROFILE_MAX_BP, ACCEL_PROFILE_MAX_V[profile], strict=True):
|
||||
assert AccelController.get_profile_accel_max(profile, speed) == expected
|
||||
|
||||
limits = [AccelController.get_profile_accel_max(profile, speed) for speed in np.linspace(-1.0, 50.0, 201)]
|
||||
assert all(0.0 <= limit <= ACCEL_MAX for limit in limits)
|
||||
assert np.all(np.diff(limits) <= 0.0)
|
||||
|
||||
@pytest.mark.parametrize("speed", ACCEL_PROFILE_MAX_BP)
|
||||
def test_profile_order_is_distinct(self, speed):
|
||||
eco, normal, sport = [AccelController.get_profile_accel_max(profile, speed) for profile in ACCEL_PROFILES]
|
||||
assert eco < normal < sport
|
||||
|
||||
def test_invalid_profile_defaults_to_normal(self):
|
||||
assert AccelController._profile(999) == AccelProfile.normal
|
||||
|
||||
def test_stock_limit_intersects_profile_before_mpc(self):
|
||||
controller = make_controller()
|
||||
results = [update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=0.30)
|
||||
for _ in range(controller.lead_loss_hold_frames)]
|
||||
result = results[-1]
|
||||
assert AccelController.get_profile_accel_max(AccelProfile.sport, 10.0) == pytest.approx(1.15)
|
||||
assert effective_accel_max(result) == pytest.approx(0.30)
|
||||
assert all(sample.mpc_accel_max is not None for sample in results)
|
||||
assert all(max(sample.mpc_accel_max) <= 0.30 + 1e-9 for sample in results)
|
||||
|
||||
def test_runtime_profile_switch_applies_the_lookup_value_directly(self):
|
||||
controller = make_controller()
|
||||
sport = [update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=1.20)
|
||||
for _ in range(controller.lead_loss_hold_frames)][-1]
|
||||
eco = update(controller, v_ego=10.0, profile=AccelProfile.eco, stock_accel_max=1.20)
|
||||
|
||||
assert effective_accel_max(sport) == pytest.approx(1.15)
|
||||
assert effective_accel_max(eco) == pytest.approx(0.72)
|
||||
|
||||
def test_matched_lead_waits_until_ego_catches_the_lead(self):
|
||||
radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
|
||||
slow_controller, caught_controller = make_controller(), make_controller()
|
||||
for controller in (slow_controller, caught_controller):
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, radar, v_ego=10.0, planner_accel=-0.2)
|
||||
|
||||
update(slow_controller, radar, v_ego=3.0, planner_accel=-0.2)
|
||||
update(caught_controller, radar, v_ego=8.0, planner_accel=-0.2)
|
||||
|
||||
assert not slow_controller.target_state.matched_lead
|
||||
assert caught_controller.target_state.matched_lead
|
||||
|
||||
def test_stock_limit_reduction_applies_immediately(self):
|
||||
controller = make_controller()
|
||||
for _ in range(controller.lead_loss_hold_frames):
|
||||
update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=1.20)
|
||||
|
||||
reduced = update(controller, v_ego=10.0, profile=AccelProfile.sport, stock_accel_max=0.30)
|
||||
assert effective_accel_max(reduced) == pytest.approx(0.30)
|
||||
assert reduced.mpc_accel_max is not None
|
||||
assert max(reduced.mpc_accel_max) <= 0.30 + 1e-9
|
||||
|
||||
def test_one_frame_stock_zero_does_not_poison_profile_recovery(self):
|
||||
clean_controller, glitch_controller = make_controller(), make_controller()
|
||||
for _ in range(clean_controller.lead_loss_hold_frames + 10):
|
||||
clean = update(clean_controller, v_ego=10.0, stock_accel_max=1.5)
|
||||
recovered = update(glitch_controller, v_ego=10.0, stock_accel_max=1.5)
|
||||
|
||||
limited = update(glitch_controller, v_ego=10.0, stock_accel_max=0.0)
|
||||
clean = update(clean_controller, v_ego=10.0, stock_accel_max=1.5)
|
||||
recovered = update(glitch_controller, v_ego=10.0, stock_accel_max=1.5)
|
||||
|
||||
assert effective_accel_max(limited) == 0.0
|
||||
assert effective_accel_max(recovered) == pytest.approx(effective_accel_max(clean))
|
||||
|
||||
@pytest.mark.parametrize("radar_fresh", (True, False), ids=("dropout", "stale"))
|
||||
def test_matched_lead_ceiling_obeys_current_stock_limit(self, radar_fresh):
|
||||
controller = make_controller()
|
||||
radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, radar, v_ego=10.0, planner_accel=-0.2)
|
||||
for _ in range(20):
|
||||
update(controller, radar, v_ego=8.0, planner_accel=-0.2)
|
||||
assert controller.target_state.matched_lead
|
||||
|
||||
limited = update(controller, stock_accel_max=0.0, radar_fresh=radar_fresh)
|
||||
assert effective_accel_max(limited) == 0.0
|
||||
assert limited.mpc_accel_max is not None
|
||||
assert max(limited.mpc_accel_max) == 0.0
|
||||
|
||||
def test_exact_global_max_uses_stock_ceiling(self):
|
||||
result = update(make_controller(), base_speed=8.0, v_ego=0.0, profile=AccelProfile.sport)
|
||||
assert AccelController.get_profile_accel_max(AccelProfile.sport, 0.0) == ACCEL_MAX
|
||||
assert result.mpc_accel_max is None
|
||||
|
||||
|
||||
class TestMpcCeiling:
|
||||
@pytest.mark.parametrize("planner_accel", (-1.0, 0.0, 1.2, ACCEL_MAX))
|
||||
def test_ceiling_is_finite_feasible_and_jerk_bounded(self, planner_accel):
|
||||
limit = 0.50
|
||||
ceiling = np.asarray(build_accel_ceiling(limit, planner_accel))
|
||||
a0 = float(np.clip(planner_accel, ACCEL_MIN, ACCEL_MAX))
|
||||
|
||||
assert ceiling.shape == T_IDXS.shape
|
||||
assert np.all(np.isfinite(ceiling))
|
||||
assert np.all((0.0 <= ceiling) & (ceiling <= ACCEL_MAX))
|
||||
assert ceiling[0] + 1e-9 >= a0
|
||||
assert np.all(ceiling + 1e-9 >= limit)
|
||||
assert np.all(np.diff(ceiling) <= 1e-9)
|
||||
assert np.all(-np.diff(ceiling) <= ACCEL_LIMIT_HORIZON_JERK * np.diff(T_IDXS) + 1e-9)
|
||||
|
||||
def test_zero_limit_remains_feasible_for_positive_x0(self):
|
||||
ceiling = np.asarray(build_accel_ceiling(0.0, 0.8))
|
||||
assert ceiling[0] == pytest.approx(0.8)
|
||||
assert ceiling[-1] == pytest.approx(0.0)
|
||||
assert np.all(ceiling >= 0.0)
|
||||
|
||||
def test_inactive_controller_has_no_custom_ceiling(self):
|
||||
controller = make_controller()
|
||||
result = update(controller, enabled=False)
|
||||
assert not result.active
|
||||
assert result.mpc_accel_max is None
|
||||
assert math.isinf(effective_accel_max(result))
|
||||
assert controller.target_state.target_speed is None
|
||||
|
||||
def test_profile_ceiling_does_not_interfere_while_planner_is_braking(self):
|
||||
controller = make_controller()
|
||||
radar = restrictive_radar()
|
||||
warmup = [update(controller, radar, planner_accel=-0.2) for _ in range(controller.lead_loss_hold_frames)]
|
||||
|
||||
assert all(sample.mpc_accel_max is None for sample in warmup)
|
||||
assert controller.target_state.lead_braking
|
||||
|
||||
bypassed = update(controller, radar, planner_accel=-0.2, acc_selected=False)
|
||||
assert not bypassed.active and bypassed.mpc_accel_max is None
|
||||
assert not controller.target_state.lead_braking
|
||||
|
||||
def test_profile_ceiling_stays_continuous_while_a_lead_begins_pulling_away(self):
|
||||
controller = make_controller()
|
||||
for _ in range(CAP_FILTER_FRAMES + 5):
|
||||
update(controller, restrictive_radar(), v_ego=10.0, planner_accel=-0.2)
|
||||
|
||||
pulling_away = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=12.0))
|
||||
result = update(controller, pulling_away, v_ego=10.0, planner_accel=0.2)
|
||||
|
||||
assert result.state == AccelControllerState.restrict
|
||||
assert effective_accel_max(result) == pytest.approx(AccelController.get_profile_accel_max(AccelProfile.normal, 10.0))
|
||||
assert result.mpc_accel_max is not None
|
||||
|
||||
def test_matched_lead_terminal_taper_changes_smoothly(self):
|
||||
controller = make_controller()
|
||||
radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
|
||||
for _ in range(CAP_FILTER_FRAMES + 5):
|
||||
update(controller, radar, v_ego=10.0, planner_accel=-0.2)
|
||||
|
||||
braking = update(controller, radar, v_ego=8.0, planner_accel=-0.2)
|
||||
braking_limit = controller.target_state.matched_accel_limit
|
||||
accelerating = update(controller, radar, v_ego=8.0, planner_accel=0.2)
|
||||
|
||||
assert controller.target_state.matched_lead
|
||||
assert braking.mpc_accel_max is not None and accelerating.mpc_accel_max is not None
|
||||
assert braking_limit is not None
|
||||
assert abs(controller.target_state.matched_accel_limit - braking_limit) <= LEAD_MATCH_ACCEL_SLEW * DT_MDL + 1e-9
|
||||
profile_accel_max = AccelController.get_profile_accel_max(AccelProfile.normal, 8.0)
|
||||
assert effective_accel_max(braking) <= profile_accel_max
|
||||
assert effective_accel_max(accelerating) <= profile_accel_max
|
||||
|
||||
def test_matched_lead_ignores_two_frame_speed_jump(self):
|
||||
clean_controller, noisy_controller = make_controller(), make_controller()
|
||||
radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
|
||||
for controller in (clean_controller, noisy_controller):
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, radar, v_ego=10.0, planner_accel=-0.2)
|
||||
for _ in range(20):
|
||||
update(controller, radar, v_ego=8.0, planner_accel=-0.2)
|
||||
|
||||
speed_jump = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=16.0))
|
||||
for _ in range(2):
|
||||
clean = update(clean_controller, radar, v_ego=8.0)
|
||||
noisy = update(noisy_controller, speed_jump, v_ego=8.0)
|
||||
assert effective_accel_max(noisy) == pytest.approx(effective_accel_max(clean))
|
||||
assert noisy.target_speed == pytest.approx(clean.target_speed)
|
||||
|
||||
def test_matched_lead_ignores_two_frame_acceleration_jump(self):
|
||||
clean_controller, noisy_controller = make_controller(), make_controller()
|
||||
steady = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
|
||||
for controller in (clean_controller, noisy_controller):
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, steady, v_ego=10.0, planner_accel=-0.2)
|
||||
for _ in range(20):
|
||||
update(controller, steady, v_ego=8.0, planner_accel=-0.2)
|
||||
|
||||
braking_jump = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-1.0))
|
||||
for _ in range(2):
|
||||
clean = update(clean_controller, steady, v_ego=8.0)
|
||||
noisy = update(noisy_controller, braking_jump, v_ego=8.0)
|
||||
assert effective_accel_max(noisy) == pytest.approx(effective_accel_max(clean))
|
||||
assert noisy.target_speed == pytest.approx(clean.target_speed)
|
||||
|
||||
|
||||
class TestLead:
|
||||
def test_cap_matches_stopping_energy_formula(self):
|
||||
controller = make_controller()
|
||||
lead = make_lead(status=True, d_rel=50.0, v_lead_k=8.0)
|
||||
result = get_lead_plan(controller, make_radar(lead), 10.0, 0.0, AccelProfile.normal)
|
||||
delay = controller.delay
|
||||
lead_xv = LongitudinalMpc.extrapolate_lead(lead.dRel, lead.vLeadK, lead.aLeadK, lead.aLeadTau)
|
||||
x_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 0]))
|
||||
v_lead = float(np.interp(delay, T_IDXS, lead_xv[:, 1]))
|
||||
x_ego, _ = _project_ego(10.0, 0.0, delay)
|
||||
safety_gap = max(x_lead - x_ego - STOP_DISTANCE - get_T_FOLLOW(log.LongitudinalPersonality.standard) * v_lead, 0.0)
|
||||
expected = v_lead + math.sqrt(2.0 * COMFORT_DECEL[AccelProfile.normal] * safety_gap)
|
||||
|
||||
assert result.cap == pytest.approx(expected)
|
||||
assert result.cap != pytest.approx(math.sqrt(v_lead**2 + 2.0 * COMFORT_DECEL[AccelProfile.normal] * safety_gap))
|
||||
|
||||
def test_profile_order_controls_approach_timing(self):
|
||||
radar = make_radar(make_lead(status=True, d_rel=50.0, v_lead_k=8.0))
|
||||
caps = [get_lead_plan(make_controller(), radar, 10.0, 0.0, profile).cap for profile in ACCEL_PROFILES]
|
||||
assert caps[0] < caps[1] < caps[2]
|
||||
|
||||
def test_stopped_lead_reserve_only_reduces_comfort_gap(self):
|
||||
lead = get_lead_plan(make_controller(),
|
||||
make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=0.0)), 5.0, 0.0, AccelProfile.normal,
|
||||
)
|
||||
comfort_decel = COMFORT_DECEL[AccelProfile.normal]
|
||||
safety_gap = (lead.departure_cap - lead.departure_lead_speed) ** 2 / (2.0 * comfort_decel)
|
||||
assert lead.required_decel < 0.30
|
||||
assert safety_gap - lead.usable_gap == pytest.approx(STOP_GAP_RESERVE)
|
||||
assert lead.departure_cap > lead.cap
|
||||
|
||||
def test_more_restrictive_lead_is_selected(self):
|
||||
radar = make_radar(make_lead(status=True, d_rel=70.0, v_lead_k=12.0), make_lead(status=True, d_rel=25.0, v_lead_k=8.0))
|
||||
assert get_lead_plan(make_controller(), radar, 10.0, 0.0, AccelProfile.normal).selected_lead == 1
|
||||
|
||||
@pytest.mark.parametrize("field,value", [
|
||||
("aLeadK", math.nan), ("aLeadK", math.inf), ("aLeadTau", math.nan), ("aLeadTau", -1.0), ("radarTrackId", math.nan),
|
||||
])
|
||||
def test_nonessential_invalid_lead_fields_are_sanitized(self, field, value):
|
||||
lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0)
|
||||
setattr(lead, field, value)
|
||||
result = get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal)
|
||||
assert result.selected_lead == 0
|
||||
assert math.isfinite(result.cap)
|
||||
|
||||
@pytest.mark.parametrize("field,value", [("dRel", math.nan), ("dRel", -1.0), ("vLeadK", math.nan), ("vLeadK", -2.0)])
|
||||
def test_invalid_geometry_is_not_used(self, field, value):
|
||||
lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0)
|
||||
setattr(lead, field, value)
|
||||
result = get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal)
|
||||
assert result.selected_lead == -1
|
||||
assert result.lead_status
|
||||
assert math.isinf(result.cap)
|
||||
|
||||
def test_raw_radar_is_never_mutated(self):
|
||||
lead = make_lead(status=True, d_rel=30.0, v_lead_k=8.0, a_lead_k=-15.0, a_lead_tau=math.nan)
|
||||
before = vars(lead).copy()
|
||||
get_lead_plan(make_controller(), make_radar(lead), 10.0, 0.0, AccelProfile.normal)
|
||||
assert vars(lead) == before
|
||||
|
||||
|
||||
class TestTargetLifecycle:
|
||||
def test_five_frame_median_needs_three_restrictive_samples(self):
|
||||
controller = make_controller()
|
||||
filtered_caps = []
|
||||
for _ in range(CAP_FILTER_FRAMES):
|
||||
update(controller, restrictive_radar())
|
||||
filtered_caps.append(controller.target_state.filtered_cap)
|
||||
assert math.isinf(filtered_caps[1])
|
||||
assert math.isfinite(filtered_caps[2])
|
||||
|
||||
def test_restriction_uses_comfort_rate_with_one_bounded_reserve_step(self):
|
||||
controller = make_controller()
|
||||
results = [update(controller, restrictive_radar()) for _ in range(CAP_FILTER_FRAMES + 10)]
|
||||
targets = np.asarray([result.target_speed for result in results])
|
||||
max_step = COMFORT_DECEL[AccelProfile.normal] * DT_MDL
|
||||
|
||||
target_steps = -np.diff(targets)
|
||||
assert np.count_nonzero(target_steps > max_step + 1e-9) == 1
|
||||
assert np.max(target_steps) <= TARGET_SPEED_RESERVE + max_step + 1e-9
|
||||
assert results[-1].state == AccelControllerState.restrict
|
||||
assert results[-1].target_speed < results[0].target_speed
|
||||
|
||||
@pytest.mark.parametrize("clear_frames", (1, 2, CAP_FILTER_FRAMES + 1))
|
||||
def test_lead_acquired_after_clear_road_cannot_step_speed_to_planner(self, clear_frames):
|
||||
controller = make_controller()
|
||||
for _ in range(clear_frames):
|
||||
update(controller, base_speed=25.0, v_ego=20.0, planner_speed=25.0)
|
||||
|
||||
results = [update(controller, restrictive_radar(), base_speed=25.0, v_ego=20.0, planner_speed=20.0)
|
||||
for _ in range(CAP_FILTER_FRAMES)]
|
||||
targets = np.asarray([25.0, *(result.target_speed for result in results)])
|
||||
max_step = COMFORT_DECEL[AccelProfile.normal] * DT_MDL
|
||||
|
||||
target_steps = -np.diff(targets)
|
||||
assert np.count_nonzero(target_steps > max_step + 1e-9) == 1
|
||||
assert np.max(target_steps) <= TARGET_SPEED_RESERVE + max_step + 1e-9
|
||||
|
||||
def test_lead_slot_is_forgotten_before_reacquisition(self):
|
||||
controller = make_controller()
|
||||
lead_one = restrictive_radar()
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, lead_one, base_speed=25.0, v_ego=20.0, planner_speed=20.0, planner_accel=-0.2)
|
||||
|
||||
for _ in range(controller.lead_loss_hold_frames):
|
||||
before = update(controller, base_speed=25.0, v_ego=20.0, planner_speed=20.0, planner_accel=-0.2)
|
||||
assert controller.target_state.selected_lead == -1
|
||||
|
||||
lead_two = make_radar(lead_two=make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-0.5))
|
||||
results = [update(controller, lead_two, base_speed=25.0, v_ego=20.0, planner_speed=5.0, planner_accel=-0.2)
|
||||
for _ in range(CAP_FILTER_FRAMES)]
|
||||
targets = np.asarray([before.target_speed, *(result.target_speed for result in results)])
|
||||
max_step = COMFORT_DECEL[AccelProfile.normal] * DT_MDL
|
||||
|
||||
target_steps = -np.diff(targets)
|
||||
assert np.count_nonzero(target_steps > max_step + 1e-9) == 1
|
||||
assert np.max(target_steps) <= TARGET_SPEED_RESERVE + max_step + 1e-9
|
||||
|
||||
@pytest.mark.parametrize("replacement_track_id", (200, -1), ids=("radar-track", "vision-track"))
|
||||
def test_false_relief_track_replacement_freezes_bounded_speed_release(self, replacement_track_id):
|
||||
controller = make_controller()
|
||||
original = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=100))
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, original, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2)
|
||||
for _ in range(20):
|
||||
before = update(controller, original, base_speed=25.0, v_ego=8.0, planner_speed=8.0, planner_accel=-0.2)
|
||||
assert controller.target_state.matched_lead
|
||||
|
||||
replacement = make_radar(make_lead(status=True, d_rel=40.0, v_lead_k=12.0, radar_track_id=replacement_track_id))
|
||||
switched = update(controller, replacement, base_speed=25.0, v_ego=8.0, planner_speed=5.0, planner_accel=-0.2)
|
||||
|
||||
target_drop = before.target_speed - switched.target_speed
|
||||
assert -TARGET_SPEED_RESERVE - 1e-9 <= target_drop <= MATCHED_SPEED_DECEL_RATE * DT_MDL + 1e-9
|
||||
assert effective_accel_max(switched) <= AccelController.get_profile_accel_max(AccelProfile.normal, 8.0) + 1e-9
|
||||
assert switched.target_speed < 25.0
|
||||
assert controller.target_state.lead_switch_guard_frames == controller.lead_loss_hold_frames
|
||||
|
||||
def test_track_id_churn_without_false_relief_does_not_arm_guard(self):
|
||||
controller = make_controller()
|
||||
original = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=100))
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, original, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2)
|
||||
|
||||
replacement = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, radar_track_id=200))
|
||||
update(controller, replacement, base_speed=25.0, v_ego=10.0, planner_speed=10.0, planner_accel=-0.2)
|
||||
|
||||
assert controller.target_state.lead_switch_guard_frames == 0
|
||||
|
||||
def test_short_dropout_holds_then_releases_without_a_second_accel_cap(self):
|
||||
controller = make_controller()
|
||||
for _ in range(CAP_FILTER_FRAMES + 20):
|
||||
restricted = update(controller, restrictive_radar())
|
||||
|
||||
held = [update(controller) for _ in range(controller.lead_loss_hold_frames - 1)]
|
||||
assert all(result.target_speed <= restricted.target_speed + 1e-9 for result in held)
|
||||
|
||||
released = update(controller)
|
||||
assert released.target_speed == 25.0
|
||||
|
||||
def test_previous_lead_source_synchronizes_down_to_planner(self):
|
||||
controller = make_controller()
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
restricted = update(controller, restrictive_radar())
|
||||
planner_speed = restricted.target_speed - 2.0
|
||||
synchronized = update(controller, previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=planner_speed)
|
||||
assert restricted.target_speed - synchronized.target_speed == pytest.approx(MATCHED_SPEED_DECEL_RATE * DT_MDL)
|
||||
assert synchronized.state == AccelControllerState.hold
|
||||
|
||||
def test_matched_lead_dropout_synchronizes_down_to_planner(self):
|
||||
controller = make_controller()
|
||||
radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, radar, v_ego=10.0, planner_accel=-0.2)
|
||||
for _ in range(20):
|
||||
matched = update(controller, radar, v_ego=8.0, planner_accel=-0.2)
|
||||
assert controller.target_state.matched_lead
|
||||
|
||||
planner_speed = matched.target_speed - 2.0
|
||||
synchronized = update(controller, previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=planner_speed)
|
||||
assert matched.target_speed - synchronized.target_speed == pytest.approx(MATCHED_SPEED_DECEL_RATE * DT_MDL)
|
||||
assert synchronized.state == AccelControllerState.hold
|
||||
|
||||
def test_reused_radar_holds_matched_lead_until_a_fresh_dropout(self):
|
||||
controller = make_controller()
|
||||
radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0))
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, radar, v_ego=10.0, planner_accel=-0.2)
|
||||
for _ in range(20):
|
||||
matched = update(controller, radar, v_ego=8.0, planner_accel=-0.2)
|
||||
assert controller.target_state.matched_lead
|
||||
|
||||
planner_speed = matched.target_speed - 2.0
|
||||
held = update(controller, radar, previous_mpc_source=LongitudinalPlanSource.lead0,
|
||||
planner_speed=planner_speed, radar_fresh=False)
|
||||
synchronized = update(controller, previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=planner_speed)
|
||||
|
||||
assert held.target_speed == pytest.approx(matched.target_speed)
|
||||
assert held.state == matched.state
|
||||
assert held.target_speed - synchronized.target_speed == pytest.approx(MATCHED_SPEED_DECEL_RATE * DT_MDL)
|
||||
assert synchronized.state == AccelControllerState.hold
|
||||
|
||||
def test_clear_road_launch_has_immediate_headroom_and_bounded_target_slew(self):
|
||||
controller = make_controller()
|
||||
initial = update(controller, base_speed=12.0, v_ego=0.0, profile=AccelProfile.normal)
|
||||
rolling = update(controller, base_speed=12.0, v_ego=0.31, profile=AccelProfile.normal)
|
||||
|
||||
assert initial.active and initial.launching
|
||||
assert LAUNCH_TARGET_HEADROOM <= initial.target_speed <= LAUNCH_TARGET_HEADROOM + LAUNCH_TARGET_SLEW * DT_MDL
|
||||
assert rolling.launching
|
||||
assert rolling.target_speed >= 0.31 + LAUNCH_TARGET_HEADROOM
|
||||
assert rolling.target_speed - max(initial.target_speed, 0.31 + LAUNCH_TARGET_HEADROOM) <= LAUNCH_TARGET_SLEW * DT_MDL + 1e-9
|
||||
|
||||
finished = update(controller, base_speed=12.0, v_ego=LAUNCH_END_SPEED, profile=AccelProfile.normal)
|
||||
assert not finished.launching
|
||||
|
||||
def test_far_stopped_lead_does_not_create_stop_hold(self):
|
||||
controller = make_controller()
|
||||
far_stopped = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=0.0))
|
||||
results = [update(controller, far_stopped, base_speed=12.0, v_ego=0.0) for _ in range(4)]
|
||||
assert all(result.state != AccelControllerState.stopHold for result in results)
|
||||
|
||||
def test_renewed_stop_above_stop_hold_speed_does_not_apply_extra_launch_ramp_step(self):
|
||||
controller = make_controller()
|
||||
ramp = [update(controller, base_speed=12.0, v_ego=v_ego, profile=AccelProfile.normal) for v_ego in (0.0, 0.5)]
|
||||
assert all(result.launching for result in ramp)
|
||||
|
||||
stopped_lead = make_radar(make_lead(status=True, d_rel=3.0, v_lead_k=0.0))
|
||||
renewed = update(controller, stopped_lead, base_speed=12.0, v_ego=0.5, profile=AccelProfile.normal)
|
||||
assert not renewed.launching
|
||||
assert renewed.target_speed <= ramp[-1].target_speed + 1e-9
|
||||
|
||||
def test_far_stopped_lead_does_not_use_sticky_braking_history_as_stop_evidence(self):
|
||||
controller = make_controller()
|
||||
far_stopped = make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=0.0))
|
||||
for _ in range(controller.lead_loss_hold_frames):
|
||||
update(controller, far_stopped, base_speed=12.0, v_ego=10.0, planner_accel=-0.2)
|
||||
assert controller.target_state.lead_braking
|
||||
|
||||
result = update(controller, far_stopped, base_speed=12.0, v_ego=0.2, planner_accel=-0.2)
|
||||
assert result.state != AccelControllerState.stopHold
|
||||
assert result.target_speed > 0.0
|
||||
|
||||
def test_near_stopped_lead_uses_braking_history_to_hold_completed_stop(self):
|
||||
controller = make_controller()
|
||||
stopped = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=0.0))
|
||||
for _ in range(controller.lead_loss_hold_frames):
|
||||
update(controller, stopped, base_speed=12.0, v_ego=10.0, planner_accel=-0.2)
|
||||
assert controller.target_state.lead_braking
|
||||
|
||||
result = update(controller, stopped, base_speed=12.0, v_ego=0.2, planner_accel=-0.2)
|
||||
assert result.state == AccelControllerState.stopHold
|
||||
assert controller.target_state.target_speed == 0.0
|
||||
assert result.target_speed == 0.0
|
||||
assert math.isinf(effective_accel_max(result))
|
||||
assert result.mpc_accel_max is None
|
||||
|
||||
stock_limited = update(controller, stopped, base_speed=12.0, v_ego=0.2, stock_accel_max=0.0)
|
||||
assert math.isinf(effective_accel_max(stock_limited))
|
||||
assert stock_limited.mpc_accel_max is None
|
||||
|
||||
def test_stop_hold_needs_four_confirmed_departure_frames(self):
|
||||
controller = make_controller()
|
||||
held = enter_stop_hold(controller)
|
||||
assert controller.target_state.target_speed == 0.0
|
||||
results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)),
|
||||
base_speed=8.0, v_ego=0.1) for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
|
||||
launch_index = next(index for index, result in enumerate(results) if result.launching)
|
||||
|
||||
assert held.state == AccelControllerState.stopHold
|
||||
assert held.target_speed == 0.0 and math.isinf(effective_accel_max(held))
|
||||
assert held.mpc_accel_max is None
|
||||
assert all(result.state == AccelControllerState.stopHold and not result.launching for result in results[:launch_index])
|
||||
assert launch_index == STOP_HOLD_EXIT_FRAMES - 1
|
||||
assert results[launch_index].target_speed >= 0.1 + LAUNCH_TARGET_HEADROOM
|
||||
assert results[launch_index].departure_launching
|
||||
assert effective_accel_max(results[launch_index]) == pytest.approx(
|
||||
AccelController.get_profile_accel_max(AccelProfile.normal, 0.1),
|
||||
)
|
||||
|
||||
def test_stopped_governing_lead_rejects_route_51d_radar_speed_pulse_without_delaying_departure(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller, v_ego=0.0)
|
||||
speed_pulse = (0.1361, 0.1731, 0.2146, 0.2253, 0.2137, 0.1877)
|
||||
distances = (6.0, 6.0, 6.0, 5.96, 6.04, 6.04)
|
||||
|
||||
for distance, speed in zip(distances, speed_pulse, strict=True):
|
||||
radar = make_radar(make_lead(status=True, d_rel=distance, v_lead_k=speed, radar_track_id=4887),
|
||||
make_lead(status=True, d_rel=6.08, v_lead_k=0.0, radar_track_id=4905))
|
||||
held = update(controller, radar, base_speed=8.0, v_ego=0.0)
|
||||
assert held.state == AccelControllerState.stopHold
|
||||
assert held.target_speed == 0.0 and not held.launching
|
||||
|
||||
results = [
|
||||
update(controller, make_radar(make_lead(status=True, d_rel=6.04 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=4887),
|
||||
make_lead(status=True, d_rel=6.12 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=4905)),
|
||||
base_speed=8.0, v_ego=0.0)
|
||||
for frame in range(STOP_HOLD_EXIT_FRAMES)
|
||||
]
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results[:-1])
|
||||
assert results[-1].launching and results[-1].departure_launching
|
||||
|
||||
def test_route_520_slow_lead_pulse_cannot_release_stop_hold_but_real_departure_can(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller, v_ego=0.0)
|
||||
speeds = (0.01, 0.03, 0.07, 0.10, 0.14, 0.20, 0.26, 0.32, 0.34, 0.33, 0.31, 0.28, 0.24, 0.20, 0.15, 0.09, 0.05, 0.01)
|
||||
offsets = (0.00, 0.00, 0.00, 0.01, 0.01, 0.02, 0.03, 0.04, 0.06, 0.07, 0.09, 0.11, 0.12, 0.13, 0.14, 0.15, 0.15, 0.16)
|
||||
|
||||
for offset, speed in zip(offsets, speeds, strict=True):
|
||||
pulse = make_radar(make_lead(status=True, d_rel=6.0 + offset, v_lead_k=speed, radar_track_id=2133))
|
||||
held = update(controller, pulse, base_speed=8.0, v_ego=0.0)
|
||||
assert held.state == AccelControllerState.stopHold
|
||||
assert held.target_speed == 0.0 and not held.launching
|
||||
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.2, v_lead_k=0.0, radar_track_id=2133))
|
||||
assert update(controller, stopped, base_speed=8.0, v_ego=0.0).state == AccelControllerState.stopHold
|
||||
results = [update(controller, make_radar(make_lead(status=True, d_rel=6.2 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=2133)),
|
||||
base_speed=8.0, v_ego=0.0) for frame in range(STOP_HOLD_EXIT_FRAMES)]
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results[:-1])
|
||||
assert results[-1].launching and results[-1].departure_launching
|
||||
|
||||
def test_fast_speed_signal_that_slows_without_separating_never_releases_stop_hold(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller, v_ego=0.0)
|
||||
departing = make_radar(make_lead(status=True, d_rel=5.9, v_lead_k=2.0))
|
||||
results = [update(controller, departing, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)]
|
||||
slowed = update(controller, make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.2)), base_speed=8.0, v_ego=0.0)
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold and not result.launching for result in results)
|
||||
assert slowed.state == AccelControllerState.stopHold
|
||||
assert slowed.target_speed == 0.0 and not slowed.launching
|
||||
|
||||
def test_stop_hold_reseeds_departure_distance_when_radar_track_is_replaced(self):
|
||||
controller = make_controller()
|
||||
original = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
|
||||
update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
|
||||
replacement = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=0.2, radar_track_id=200))
|
||||
results = [update(controller, replacement, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results)
|
||||
assert all(result.target_speed == 0.0 and not result.launching for result in results)
|
||||
|
||||
def test_stop_hold_rejects_persistent_same_track_distance_step(self):
|
||||
controller = make_controller()
|
||||
original = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
|
||||
update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
|
||||
stepped = make_radar(make_lead(status=True, d_rel=6.4, v_lead_k=0.2, radar_track_id=100))
|
||||
results = [update(controller, stepped, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results)
|
||||
assert all(result.target_speed == 0.0 and not result.launching for result in results)
|
||||
|
||||
def test_stop_hold_reseeds_non_selected_departure_lead_when_its_track_is_replaced(self):
|
||||
controller = make_controller()
|
||||
original = make_radar(make_lead(status=True, d_rel=3.0, v_lead_k=0.2, radar_track_id=100),
|
||||
make_lead(status=True, d_rel=6.0, v_lead_k=0.1, radar_track_id=200))
|
||||
update(controller, original, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
|
||||
replacement = make_radar(make_lead(status=True, d_rel=3.4, v_lead_k=0.2, radar_track_id=101),
|
||||
make_lead(status=True, d_rel=6.0, v_lead_k=0.1, radar_track_id=200))
|
||||
lead = get_lead_plan(controller, replacement, 0.0, 0.0, AccelProfile.normal)
|
||||
results = [update(controller, replacement, base_speed=8.0, v_ego=0.0) for _ in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
|
||||
|
||||
assert lead.selected_lead == 1 and lead.departure_lead_index == 0
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results)
|
||||
assert all(result.target_speed == 0.0 and not result.launching for result in results)
|
||||
|
||||
def test_genuine_departure_survives_lead_slot_and_track_flicker(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller, v_ego=0.0)
|
||||
results = []
|
||||
for frame in range(STOP_HOLD_EXIT_FRAMES):
|
||||
moving = make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100)
|
||||
secondary = make_lead(status=True, d_rel=7.0, v_lead_k=2.0, radar_track_id=200)
|
||||
results.append(update(controller, make_radar(moving, secondary) if frame % 2 == 0 else make_radar(secondary, moving),
|
||||
base_speed=8.0, v_ego=0.0))
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results[:-1])
|
||||
assert results[-1].launching and results[-1].departure_launching
|
||||
|
||||
def test_fast_speed_glitch_without_distance_progress_stays_in_stop_hold(self):
|
||||
controller = make_controller()
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
|
||||
update(controller, stopped, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
|
||||
glitch = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.9, radar_track_id=100))
|
||||
results = [update(controller, glitch, base_speed=8.0, v_ego=0.0) for _ in range(STOP_HOLD_EXIT_FRAMES)]
|
||||
results.append(update(controller, stopped, base_speed=8.0, v_ego=0.0))
|
||||
|
||||
assert all(result.state == AccelControllerState.stopHold for result in results)
|
||||
assert all(result.target_speed == 0.0 and not result.launching for result in results)
|
||||
|
||||
def test_moving_departure_does_not_reenter_stop_hold_when_speed_crosses_exit_threshold(self):
|
||||
controller = make_controller()
|
||||
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100))
|
||||
update(controller, stopped, base_speed=8.0, v_ego=0.0, previous_should_stop=True)
|
||||
distance = 6.0
|
||||
results = []
|
||||
for speed in (0.81, 0.82, 0.83, 0.84, 0.79, 0.76, 0.74, 0.72):
|
||||
distance += speed * DT_MDL
|
||||
radar = make_radar(make_lead(status=True, d_rel=distance, v_lead_k=speed, radar_track_id=100))
|
||||
results.append(update(controller, radar, base_speed=8.0, v_ego=0.0))
|
||||
|
||||
launch_index = next(index for index, result in enumerate(results) if result.launching)
|
||||
assert all(result.state != AccelControllerState.stopHold for result in results[launch_index:])
|
||||
assert all(result.target_speed > 0.0 and result.departure_launching for result in results[launch_index:])
|
||||
|
||||
def test_reused_radar_does_not_pulse_stop_hold_or_departure_target(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller)
|
||||
|
||||
for frame in range(STOP_HOLD_EXIT_FRAMES):
|
||||
departing = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0))
|
||||
fresh = update(controller, departing, base_speed=8.0, v_ego=0.1)
|
||||
held = update(controller, departing, base_speed=8.0, v_ego=0.1, radar_fresh=False,
|
||||
previous_mpc_source=LongitudinalPlanSource.lead0, planner_speed=0.01)
|
||||
assert held.target_speed == pytest.approx(fresh.target_speed)
|
||||
assert held.state == fresh.state
|
||||
assert held.selected_lead == fresh.selected_lead == 0
|
||||
assert effective_accel_max(held) == pytest.approx(effective_accel_max(fresh))
|
||||
if frame < STOP_HOLD_EXIT_FRAMES - 1:
|
||||
assert fresh.state == AccelControllerState.stopHold
|
||||
assert math.isinf(effective_accel_max(fresh))
|
||||
assert fresh.mpc_accel_max is None
|
||||
|
||||
assert fresh.launching and held.launching
|
||||
assert fresh.departure_launching and held.departure_launching
|
||||
assert fresh.target_speed == held.target_speed == 8.0
|
||||
assert effective_accel_max(fresh) == pytest.approx(AccelController.get_profile_accel_max(AccelProfile.normal, 0.1))
|
||||
|
||||
def test_single_frame_departure_stays_at_zero_target_without_an_accel_ceiling(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller)
|
||||
departing = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=2.0))
|
||||
stopped = make_radar(make_lead(status=True, d_rel=8.0, v_lead_k=0.0))
|
||||
|
||||
warm = update(controller, departing, base_speed=8.0, v_ego=0.0)
|
||||
held = update(controller, stopped, base_speed=8.0, v_ego=0.0)
|
||||
|
||||
assert warm.state == held.state == AccelControllerState.stopHold
|
||||
assert not warm.launching and not held.launching
|
||||
assert math.isinf(effective_accel_max(warm)) and warm.mpc_accel_max is None
|
||||
assert math.isinf(effective_accel_max(held)) and held.mpc_accel_max is None
|
||||
assert held.target_speed == 0.0
|
||||
|
||||
def test_previous_stop_without_a_lead_does_not_latch_stop_hold(self):
|
||||
result = update(
|
||||
make_controller(), base_speed=8.0, v_ego=0.0, previous_should_stop=True,
|
||||
previous_mpc_source=LongitudinalPlanSource.cruise,
|
||||
)
|
||||
|
||||
assert result.state != AccelControllerState.stopHold
|
||||
assert result.target_speed >= LAUNCH_TARGET_HEADROOM
|
||||
|
||||
def test_previous_lead_stop_survives_a_fresh_full_field_dropout(self):
|
||||
result = update(
|
||||
make_controller(), base_speed=8.0, v_ego=0.0, previous_should_stop=True,
|
||||
previous_mpc_source=LongitudinalPlanSource.lead0,
|
||||
)
|
||||
|
||||
assert result.state == AccelControllerState.stopHold
|
||||
assert result.target_speed == 0.0
|
||||
assert math.isinf(effective_accel_max(result))
|
||||
assert result.mpc_accel_max is None
|
||||
|
||||
def test_stop_hold_without_usable_lead_stays_pinned_to_zero(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller)
|
||||
missing = update(controller, base_speed=8.0, v_ego=0.1)
|
||||
|
||||
assert missing.state == AccelControllerState.stopHold
|
||||
assert missing.target_speed == 0.0
|
||||
assert math.isinf(effective_accel_max(missing))
|
||||
assert missing.mpc_accel_max is None
|
||||
|
||||
def test_confirmed_creep_departure_does_not_reenter_stop_hold(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller, v_ego=0.0)
|
||||
results = []
|
||||
for frame in range(60):
|
||||
creeping = make_radar(make_lead(status=True, d_rel=6.0 + frame * 0.01, v_lead_k=0.2))
|
||||
results.append(update(controller, creeping, base_speed=8.0, v_ego=0.0))
|
||||
|
||||
launch_index = next(index for index, result in enumerate(results) if result.launching)
|
||||
assert launch_index * DT_MDL <= 2.0
|
||||
assert all(result.state != AccelControllerState.stopHold for result in results[launch_index:])
|
||||
assert all(result.target_speed > 0.0 for result in results[launch_index:])
|
||||
|
||||
def test_departure_dropout_holds_without_resurrecting_stop_hold(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller)
|
||||
results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)),
|
||||
base_speed=8.0, v_ego=0.1) for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES)]
|
||||
launched = next(result for result in results if result.launching)
|
||||
before_dropout = results[-1]
|
||||
dropout = [update(controller, base_speed=8.0, v_ego=0.1) for _ in range(controller.lead_loss_hold_frames + 1)]
|
||||
|
||||
assert launched.launching
|
||||
assert all(result.state != AccelControllerState.stopHold for result in dropout)
|
||||
assert all(result.target_speed <= before_dropout.target_speed + 1e-9 for result in dropout[:controller.lead_loss_hold_frames - 1])
|
||||
assert dropout[controller.lead_loss_hold_frames - 1].target_speed > before_dropout.target_speed
|
||||
assert dropout[-1].launching
|
||||
|
||||
def test_invalid_departure_geometry_returns_to_stop_hold(self):
|
||||
controller = make_controller()
|
||||
enter_stop_hold(controller)
|
||||
for frame in range(CAP_FILTER_FRAMES + STOP_HOLD_EXIT_FRAMES):
|
||||
departing = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0))
|
||||
launched = update(controller, departing, base_speed=8.0, v_ego=0.1)
|
||||
invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=2.0))
|
||||
guarded = update(controller, invalid, base_speed=8.0, v_ego=0.1)
|
||||
assert launched.launching
|
||||
assert guarded.state == AccelControllerState.stopHold
|
||||
assert guarded.target_speed == 0.0
|
||||
|
||||
def test_stale_timeout_fully_resets_live_state(self):
|
||||
controller = make_controller()
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
restricted = update(controller, restrictive_radar())
|
||||
stale_frames = math.ceil(RADAR_STALE_TIMEOUT / DT_MDL)
|
||||
held = [update(controller, radar_fresh=False) for _ in range(stale_frames - 1)]
|
||||
timed_out = update(controller, radar_fresh=False)
|
||||
|
||||
assert all(result.active and result.target_speed == pytest.approx(restricted.target_speed) for result in held)
|
||||
assert not timed_out.active
|
||||
assert timed_out.target_speed == 25.0
|
||||
assert timed_out.mpc_accel_max is None
|
||||
assert timed_out.selected_lead == -1 and controller._held_lead_plan is None
|
||||
assert controller.target_state.target_speed is None
|
||||
|
||||
@pytest.mark.parametrize("override", [{"enabled": False}, {"acc_selected": False}, {"engaged": False}, {"cruise_initialized": False}, {"a_ego": math.inf}])
|
||||
def test_bypass_or_invalid_context_resets_live_state(self, override):
|
||||
controller = make_controller()
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, restrictive_radar())
|
||||
result = update(controller, restrictive_radar(), **override)
|
||||
|
||||
assert not result.active
|
||||
assert result.target_speed == 25.0
|
||||
assert result.mpc_accel_max is None
|
||||
assert controller.target_state.target_speed is None
|
||||
|
||||
def test_acc_bypass_does_not_retain_state_for_live_actuation(self):
|
||||
controller = make_controller()
|
||||
for _ in range(CAP_FILTER_FRAMES + 20):
|
||||
bypassed = update(controller, restrictive_radar(), acc_selected=False)
|
||||
assert not bypassed.active
|
||||
assert controller.target_state.target_speed is None
|
||||
assert controller._held_lead_plan is None
|
||||
live = update(controller)
|
||||
|
||||
assert live.active and live.target_speed == 25.0
|
||||
assert math.isinf(controller.target_state.filtered_cap)
|
||||
|
||||
def test_explicit_reset_clears_target_state(self):
|
||||
controller = make_controller()
|
||||
for _ in range(CAP_FILTER_FRAMES + 10):
|
||||
update(controller, restrictive_radar())
|
||||
controller._jerk_smoothing_blocked = True
|
||||
controller._required_decel_samples = [0.2]
|
||||
controller._required_decel_lead = controller._required_decel_lead_track_id = 1
|
||||
controller._lead_trend_warmup = True
|
||||
controller.reset()
|
||||
|
||||
assert controller._held_lead_plan is None
|
||||
assert not controller._jerk_smoothing_blocked
|
||||
assert controller._required_decel_samples == []
|
||||
assert controller._required_decel_lead == controller._required_decel_lead_track_id == -1
|
||||
assert not controller._lead_trend_warmup
|
||||
target_state = controller.target_state
|
||||
assert target_state.target_speed is None and target_state.matched_accel_limit is None
|
||||
assert target_state.state == AccelControllerState.inactive
|
||||
assert target_state.departure_frames == target_state.active_frames == target_state.lead_loss_frames == target_state.stale_frames == 0
|
||||
assert target_state.lead_switch_guard_frames == 0
|
||||
assert target_state.selected_lead == target_state.selected_lead_track_id == -1
|
||||
assert target_state.cap_samples == [math.inf] * CAP_FILTER_FRAMES
|
||||
assert target_state.lead_speed_samples == [math.inf] * CAP_FILTER_FRAMES
|
||||
assert target_state.lead_accel_samples == [0.0] * CAP_FILTER_FRAMES
|
||||
assert target_state.departure.samples == [[], []] and target_state.departure.motion_samples == []
|
||||
assert target_state.departure.references == [None, None] and target_state.departure.track_ids == [-1, -1]
|
||||
assert not target_state.launching and not target_state.departure_launch and not target_state.matched_lead
|
||||
assert not target_state.lead_braking and not target_state.e2e_braking_handoff and not target_state.speed_reserve_armed
|
||||
assert math.isinf(target_state.filtered_cap) and math.isinf(target_state.filtered_lead_speed) and target_state.filtered_lead_accel == 0.0
|
||||
|
||||
@pytest.mark.parametrize("replacement_track_id", (200, -1), ids=("radar-track", "vision-track"))
|
||||
def test_track_id_change_requires_new_history_before_jerk_smoothing(self, replacement_track_id):
|
||||
controller = make_controller()
|
||||
controller.state = AccelControllerState.restrict
|
||||
controller.launching = False
|
||||
controller.selected_lead = 0
|
||||
controller.selected_lead_track_id = 100
|
||||
controller.required_decel = 0.2
|
||||
original = [controller.get_jerk_cost_multiplier(True, True, 1.0, False) for _ in range(4)]
|
||||
|
||||
controller.selected_lead_track_id = replacement_track_id
|
||||
replacement = [controller.get_jerk_cost_multiplier(True, True, 1.0, False) for _ in range(4)]
|
||||
|
||||
assert original == [MPC_DECEL_JERK_COST_MULTIPLIER] * 4
|
||||
assert replacement == [1.0, 1.0, 1.0, MPC_DECEL_JERK_COST_MULTIPLIER]
|
||||
assert controller._required_decel_samples == [0.2] * 4
|
||||
+515
@@ -0,0 +1,515 @@
|
||||
import inspect
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from cereal import custom, log, messaging
|
||||
from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import N, LongitudinalMpc
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource as MpcLongitudinalPlanSource
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import (
|
||||
MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, MPC_DECEL_JERK_MAX_TARGET_REDUCTION, AccelProfile,
|
||||
)
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpcSP
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP, LongitudinalPlanSource
|
||||
|
||||
|
||||
def radar_state():
|
||||
return messaging.new_message("radarState").radarState
|
||||
|
||||
|
||||
class PlannerSM(dict):
|
||||
def __init__(self, radar_log_mono_time: int):
|
||||
super().__init__(
|
||||
radarState=radar_state(),
|
||||
carState=SimpleNamespace(vEgo=10.0, aEgo=0.0, vCruise=20.0),
|
||||
selfdriveState=SimpleNamespace(personality=0),
|
||||
controlsState=SimpleNamespace(forceDecel=False),
|
||||
)
|
||||
self.valid = {"radarState": True}
|
||||
self.alive = {"radarState": True}
|
||||
self.logMonoTime = {"radarState": radar_log_mono_time}
|
||||
|
||||
|
||||
class ControllerStub:
|
||||
def __init__(self, *, target_speed=15.0, active=True, mpc_accel_max=None, state=AccelControllerState.free, selected_lead=-1,
|
||||
selected_lead_track_id=-1, launching=False, departure_launching=False, required_decel=0.0):
|
||||
self.available = self.enabled = True
|
||||
self.profile = AccelProfile.normal
|
||||
self.output_v_target = target_speed
|
||||
self.is_active = active
|
||||
self.mpc_accel_max = mpc_accel_max
|
||||
self.state = state
|
||||
self.selected_lead = selected_lead
|
||||
self.selected_lead_track_id = selected_lead_track_id
|
||||
self.launching = launching
|
||||
self.departure_launching = departure_launching
|
||||
self.required_decel = required_decel
|
||||
self.dt = DT_MDL
|
||||
self._jerk_smoothing_blocked = False
|
||||
self._required_decel_samples = []
|
||||
self._required_decel_lead = -1
|
||||
self._required_decel_lead_track_id = -1
|
||||
self._lead_trend_warmup = False
|
||||
self.update_kwargs = None
|
||||
self.reset_calls = 0
|
||||
|
||||
def update(self, _radar_state, **kwargs):
|
||||
self.update_kwargs = kwargs
|
||||
|
||||
@property
|
||||
def is_enabled(self):
|
||||
return self.available and self.enabled
|
||||
|
||||
def update_params(self):
|
||||
pass
|
||||
|
||||
def reset(self):
|
||||
self.reset_calls += 1
|
||||
|
||||
def get_jerk_cost_multiplier(self, *args):
|
||||
return AccelController.get_jerk_cost_multiplier(self, *args)
|
||||
|
||||
def update_should_stop(self, should_stop):
|
||||
return AccelController.update_should_stop(self, should_stop)
|
||||
|
||||
|
||||
def planner_for_mpc_test(*, target_speed=15.0, active=True, is_e2e=False, mpc_accel_max=None,
|
||||
state=AccelControllerState.free, selected_lead=-1, launching=False,
|
||||
departure_launching=False, required_decel=0.0,
|
||||
mpc_source=MpcLongitudinalPlanSource.lead0):
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
is_e2e_calls = []
|
||||
planner.is_e2e = lambda _sm: is_e2e_calls.append(True) or is_e2e
|
||||
planner.output_v_target = 20.0
|
||||
planner.output_should_stop = False
|
||||
planner.allow_throttle = True
|
||||
planner.a_desired = 0.0
|
||||
planner.v_desired_filter = SimpleNamespace(x=10.0)
|
||||
planner._radar_fresh_this_cycle = True
|
||||
planner.mpc = SimpleNamespace(source=mpc_source, last_solution_status=0)
|
||||
planner.accel_controller = ControllerStub(
|
||||
target_speed=target_speed, active=active, state=state, selected_lead=selected_lead, launching=launching,
|
||||
departure_launching=departure_launching, required_decel=required_decel, mpc_accel_max=mpc_accel_max,
|
||||
)
|
||||
return planner, is_e2e_calls
|
||||
|
||||
|
||||
def run_controller_mpc(planner, *, mpc_v_cruise=20.0, force_decel=False):
|
||||
calls = []
|
||||
planner._run_mpc = lambda _sm, *args, **kwargs: calls.append((({}, *args), kwargs))
|
||||
sm = {
|
||||
"radarState": radar_state(),
|
||||
"controlsState": SimpleNamespace(forceDecel=force_decel),
|
||||
"carState": SimpleNamespace(vCruise=20.0, vEgo=10.0, aEgo=0.0),
|
||||
"selfdriveState": SimpleNamespace(personality=0),
|
||||
}
|
||||
is_e2e = planner.update_mpc(sm, mpc_v_cruise, True, ACCEL_MAX, False)
|
||||
return is_e2e, calls
|
||||
|
||||
|
||||
def test_accel_controller_schema_contract():
|
||||
expected = {"eco": 0, "normal": 1, "sport": 2}
|
||||
state = {"inactive": 0, "free": 1, "restrict": 2, "hold": 3, "release": 4, "stopHold": 5}
|
||||
accel_controller = custom.LongitudinalPlanSP.schema.fields["accelController"]
|
||||
fields = custom.LongitudinalPlanSP.AccelController.schema.fields
|
||||
|
||||
assert accel_controller.proto.ordinal.explicit == 8
|
||||
assert {name: field.proto.ordinal.explicit for name, field in fields.items()} == {
|
||||
"enabled": 0, "active": 1, "shadowOnlyDEPRECATED": 2, "profile": 3, "state": 4,
|
||||
}
|
||||
assert fields["shadowOnlyDEPRECATED"].proto.slot.type.which() == "bool"
|
||||
assert custom.LongitudinalPlanSP.AccelerationPersonality.schema.enumerants == expected
|
||||
assert custom.LongitudinalPlanSP.AccelController.Profile.schema.enumerants == expected
|
||||
assert custom.LongitudinalPlanSP.AccelController.State.schema.enumerants == state
|
||||
|
||||
|
||||
def test_accel_controller_schema_round_trip_and_toyota_compatibility():
|
||||
message = custom.LongitudinalPlanSP.new_message()
|
||||
message.accelController.enabled = True
|
||||
message.accelController.active = True
|
||||
message.accelController.profile = custom.LongitudinalPlanSP.AccelController.Profile.sport
|
||||
message.accelController.state = custom.LongitudinalPlanSP.AccelController.State.release
|
||||
|
||||
with custom.LongitudinalPlanSP.from_bytes(message.to_bytes()) as reader:
|
||||
assert reader.accelController.enabled and reader.accelController.active
|
||||
assert reader.accelController.profile == custom.LongitudinalPlanSP.AccelController.Profile.sport
|
||||
assert reader.accelController.state == custom.LongitudinalPlanSP.AccelController.State.release
|
||||
|
||||
from opendbc.car.toyota.carstate import AccelPersonality, CarState
|
||||
|
||||
assert AccelPersonality.schema.enumerants == {"eco": 0, "normal": 1, "sport": 2}
|
||||
assert CarState.__module__ == "opendbc.car.toyota.carstate"
|
||||
|
||||
|
||||
def test_longitudinal_planner_sp_owns_accel_controller_integration():
|
||||
assert "update_mpc" in LongitudinalPlannerSP.__dict__
|
||||
assert "update_should_stop" in LongitudinalPlannerSP.__dict__
|
||||
|
||||
|
||||
def test_mpc_inherits_accel_controller_extension_without_changing_stock_signature_or_bounds():
|
||||
assert LongitudinalMpc.__bases__ == (LongitudinalMpcSP,)
|
||||
assert tuple(inspect.signature(LongitudinalMpc.update).parameters) == ("self", "radarstate", "v_cruise", "personality")
|
||||
mpc = LongitudinalMpc()
|
||||
radar = radar_state()
|
||||
mpc.run = lambda: None
|
||||
|
||||
mpc.set_cur_state(10.0, 0.8)
|
||||
mpc.update(radar, 30.0)
|
||||
np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
|
||||
np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
|
||||
|
||||
requested_ceiling = tuple(np.full(N + 1, 0.4))
|
||||
mpc.set_accel_controller_params(requested_ceiling, 1.0)
|
||||
mpc.update(radar, 30.0)
|
||||
np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
|
||||
assert mpc.params[0, 1] == pytest.approx(0.8)
|
||||
np.testing.assert_array_equal(mpc.params[1:, 1], requested_ceiling[1:])
|
||||
|
||||
for malformed_ceiling in ("bad", [0.4] * N, np.full(N + 1, math.nan), [10**10000] * (N + 1)):
|
||||
mpc.set_accel_controller_params(malformed_ceiling, 1.0)
|
||||
mpc.update(radar, 30.0)
|
||||
np.testing.assert_array_equal(mpc.params[:, 0], ACCEL_MIN)
|
||||
np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
|
||||
|
||||
mpc.set_accel_controller_params(None, 1.0)
|
||||
mpc.update(radar, 30.0)
|
||||
np.testing.assert_array_equal(mpc.params[:, 1], ACCEL_MAX)
|
||||
|
||||
|
||||
def test_mpc_jerk_cost_multiplier_is_backward_compatible_and_does_not_change_other_costs():
|
||||
mpc = LongitudinalMpc.__new__(LongitudinalMpc)
|
||||
LongitudinalMpcSP.__init__(mpc)
|
||||
captured = []
|
||||
mpc.set_cost_weights = lambda costs, constraints: captured.append((np.asarray(costs), np.asarray(constraints)))
|
||||
|
||||
mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
|
||||
default_costs, default_constraints = captured[-1]
|
||||
mpc.set_accel_controller_params(None, 1.0)
|
||||
mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
|
||||
explicit_costs, explicit_constraints = captured[-1]
|
||||
mpc.set_accel_controller_params(None, 1.2)
|
||||
mpc.set_weights(True, personality=log.LongitudinalPersonality.standard)
|
||||
smoothed_costs, smoothed_constraints = captured[-1]
|
||||
|
||||
np.testing.assert_array_equal(explicit_costs, default_costs)
|
||||
np.testing.assert_array_equal(explicit_constraints, default_constraints)
|
||||
np.testing.assert_array_equal(smoothed_costs[:-1], default_costs[:-1])
|
||||
assert smoothed_costs[-1] == pytest.approx(default_costs[-1] * 1.2)
|
||||
np.testing.assert_array_equal(smoothed_constraints, default_constraints)
|
||||
|
||||
mpc.set_weights(False, personality=log.LongitudinalPersonality.standard)
|
||||
assert captured[-1][0][-2] == 0.0
|
||||
assert captured[-1][0][-1] == pytest.approx(default_costs[-1] * 1.2)
|
||||
|
||||
|
||||
def test_inherited_planner_uses_real_state_raw_radar_and_one_mpc_solve():
|
||||
radar = radar_state()
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner.a_desired = -0.2
|
||||
planner.v_desired_filter = SimpleNamespace(x=12.0)
|
||||
calls = []
|
||||
|
||||
def update_mpc(radar_arg, target, *, personality):
|
||||
calls.append(("update", radar_arg, target, personality))
|
||||
|
||||
planner.mpc = SimpleNamespace(
|
||||
set_accel_controller_params=lambda accel_max, multiplier: calls.append(("configure", accel_max, multiplier)),
|
||||
set_weights=lambda constraint, personality: calls.append(("weights", constraint, personality)),
|
||||
set_cur_state=lambda speed, accel: calls.append(("state", speed, accel)),
|
||||
update=update_mpc,
|
||||
)
|
||||
ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
|
||||
sm = {"radarState": radar, "selfdriveState": SimpleNamespace(personality=2)}
|
||||
planner._run_mpc(sm, 17.5, True, ceiling, jerk_cost_multiplier=1.2)
|
||||
|
||||
assert calls == [
|
||||
("configure", ceiling, 1.2),
|
||||
("weights", True, 2),
|
||||
("state", 12.0, -0.2),
|
||||
("update", radar, 17.5, 2),
|
||||
]
|
||||
assert calls[-1][1] is radar
|
||||
|
||||
|
||||
def test_active_acc_uses_target_and_ceiling_in_exactly_one_solve():
|
||||
ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
|
||||
planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling)
|
||||
is_e2e, calls = run_controller_mpc(planner)
|
||||
|
||||
assert not is_e2e
|
||||
assert len(mode_calls) == 1
|
||||
assert calls == [(({}, 15.0, True, ceiling), {"jerk_cost_multiplier": 1.0})]
|
||||
|
||||
|
||||
def test_valid_lead_stop_hold_preplans_from_raw_target_without_an_accel_ceiling():
|
||||
planner, _ = planner_for_mpc_test(
|
||||
target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=0,
|
||||
)
|
||||
_, calls = run_controller_mpc(planner)
|
||||
|
||||
assert calls == [(({}, 20.0, True, None), {"jerk_cost_multiplier": 1.0})]
|
||||
|
||||
|
||||
def test_missing_lead_stop_hold_keeps_zero_mpc_target_without_an_accel_ceiling():
|
||||
planner, _ = planner_for_mpc_test(
|
||||
target_speed=0.0, mpc_accel_max=None, state=AccelControllerState.stopHold, selected_lead=-1,
|
||||
)
|
||||
_, calls = run_controller_mpc(planner)
|
||||
|
||||
assert calls == [(({}, 0.0, True, None), {"jerk_cost_multiplier": 1.0})]
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
("active", "departure_launching", "expected"),
|
||||
[
|
||||
(True, True, False),
|
||||
(True, False, True),
|
||||
(False, True, True),
|
||||
],
|
||||
)
|
||||
def test_only_confirmed_live_acc_departure_clears_should_stop(active, departure_launching, expected):
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner.accel_controller = ControllerStub(active=active, departure_launching=departure_launching, state=AccelControllerState.stopHold)
|
||||
assert planner.update_should_stop(True) is expected
|
||||
assert planner.update_should_stop(False) is (active and not departure_launching)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("active", "is_e2e"), [(False, False), (True, True)])
|
||||
def test_disabled_or_e2e_is_an_exact_mpc_bypass(active, is_e2e):
|
||||
ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
|
||||
planner, mode_calls = planner_for_mpc_test(active=active, is_e2e=is_e2e, mpc_accel_max=ceiling)
|
||||
returned_e2e, calls = run_controller_mpc(planner)
|
||||
|
||||
assert returned_e2e is is_e2e
|
||||
assert len(mode_calls) == 1
|
||||
assert calls == [(({}, 20.0, True, None), {"jerk_cost_multiplier": 1.0})]
|
||||
|
||||
|
||||
def test_force_decel_target_remains_authoritative_and_disables_ceiling():
|
||||
ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
|
||||
planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling)
|
||||
_, calls = run_controller_mpc(planner, mpc_v_cruise=0.0, force_decel=True)
|
||||
|
||||
assert len(mode_calls) == 1
|
||||
assert calls == [(({}, 0.0, True, None), {"jerk_cost_multiplier": 1.0})]
|
||||
|
||||
|
||||
def test_previous_mpc_failure_gets_one_stock_recovery_cycle():
|
||||
ceiling = tuple(np.linspace(0.8, 0.4, N + 1))
|
||||
planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling)
|
||||
controller = planner.accel_controller
|
||||
planner.mpc.last_solution_status = 4
|
||||
|
||||
_, failed_recovery_calls = run_controller_mpc(planner)
|
||||
assert controller.reset_calls == 1
|
||||
assert len(mode_calls) == 1
|
||||
assert failed_recovery_calls == [(({}, 20.0, True, None), {"jerk_cost_multiplier": 1.0})]
|
||||
|
||||
planner.mpc.last_solution_status = 0
|
||||
_, recovered_calls = run_controller_mpc(planner)
|
||||
assert controller.reset_calls == 1
|
||||
assert len(mode_calls) == 2
|
||||
assert recovered_calls == [(({}, 15.0, True, ceiling), {"jerk_cost_multiplier": 1.0})]
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"mpc_source",
|
||||
(MpcLongitudinalPlanSource.cruise, MpcLongitudinalPlanSource.lead0, MpcLongitudinalPlanSource.lead1),
|
||||
)
|
||||
def test_routine_governor_restriction_forwards_the_jerk_cost_multiplier(mpc_source):
|
||||
planner, _ = planner_for_mpc_test(
|
||||
state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30,
|
||||
mpc_source=mpc_source,
|
||||
)
|
||||
_, calls = run_controller_mpc(planner)
|
||||
|
||||
assert calls == [(({}, 15.0, True, None), {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER})]
|
||||
|
||||
|
||||
def test_ineligible_required_decel_blocks_smoothing_only_until_the_restriction_episode_ends():
|
||||
planner, _ = planner_for_mpc_test(
|
||||
state=AccelControllerState.restrict, selected_lead=0, required_decel=0.30,
|
||||
)
|
||||
_, initial_calls = run_controller_mpc(planner)
|
||||
controller = planner.accel_controller
|
||||
assert initial_calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER}
|
||||
|
||||
controller.required_decel = MPC_DECEL_JERK_MAX_REQUIRED_DECEL
|
||||
_, ineligible_calls = run_controller_mpc(planner)
|
||||
assert ineligible_calls[0][1] == {"jerk_cost_multiplier": 1.0}
|
||||
|
||||
controller.required_decel = 0.30
|
||||
_, flicker_calls = run_controller_mpc(planner)
|
||||
assert flicker_calls[0][1] == {"jerk_cost_multiplier": 1.0}
|
||||
|
||||
controller.state = AccelControllerState.free
|
||||
controller.output_v_target = 20.0
|
||||
run_controller_mpc(planner)
|
||||
controller.state = AccelControllerState.restrict
|
||||
controller.output_v_target = 15.0
|
||||
_, rearmed_calls = run_controller_mpc(planner)
|
||||
assert rearmed_calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER}
|
||||
|
||||
|
||||
def test_consistently_tightening_lead_releases_smoothing_until_the_restriction_ends():
|
||||
planner, _ = planner_for_mpc_test(
|
||||
state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
|
||||
)
|
||||
_, calls = run_controller_mpc(planner)
|
||||
controller = planner.accel_controller
|
||||
multipliers = [calls[0][1]["jerk_cost_multiplier"]]
|
||||
for required_decel in (0.20, 0.23, 0.25):
|
||||
controller.required_decel = required_decel
|
||||
_, calls = run_controller_mpc(planner)
|
||||
multipliers.append(calls[0][1]["jerk_cost_multiplier"])
|
||||
|
||||
assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 3 + [1.0]
|
||||
|
||||
controller.required_decel = 0.20
|
||||
_, calls = run_controller_mpc(planner)
|
||||
assert calls[0][1] == {"jerk_cost_multiplier": 1.0}
|
||||
|
||||
controller.state = AccelControllerState.free
|
||||
controller.output_v_target = 20.0
|
||||
run_controller_mpc(planner)
|
||||
controller.state = AccelControllerState.restrict
|
||||
controller.output_v_target = 15.0
|
||||
controller.required_decel = 0.18
|
||||
_, calls = run_controller_mpc(planner)
|
||||
assert calls[0][1] == {"jerk_cost_multiplier": MPC_DECEL_JERK_COST_MULTIPLIER}
|
||||
|
||||
|
||||
def test_one_frame_required_decel_noise_does_not_disable_routine_smoothing():
|
||||
planner, _ = planner_for_mpc_test(
|
||||
state=AccelControllerState.restrict, selected_lead=0, required_decel=0.18,
|
||||
)
|
||||
_, calls = run_controller_mpc(planner)
|
||||
controller = planner.accel_controller
|
||||
multipliers = [calls[0][1]["jerk_cost_multiplier"]]
|
||||
for required_decel in (0.24, 0.19, 0.22):
|
||||
controller.required_decel = required_decel
|
||||
_, calls = run_controller_mpc(planner)
|
||||
multipliers.append(calls[0][1]["jerk_cost_multiplier"])
|
||||
|
||||
assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 4
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
("state", "selected_lead", "launching", "required_decel", "target_speed", "mpc_source"),
|
||||
[
|
||||
(AccelControllerState.free, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
|
||||
(AccelControllerState.hold, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
|
||||
(AccelControllerState.stopHold, 0, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
|
||||
(AccelControllerState.restrict, -1, False, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
|
||||
(AccelControllerState.restrict, 0, True, 0.30, 15.0, MpcLongitudinalPlanSource.cruise),
|
||||
(AccelControllerState.restrict, 0, False, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, 15.0, MpcLongitudinalPlanSource.cruise),
|
||||
(AccelControllerState.restrict, 0, False, math.inf, 15.0, MpcLongitudinalPlanSource.cruise),
|
||||
(AccelControllerState.restrict, 0, False, math.nan, 15.0, MpcLongitudinalPlanSource.cruise),
|
||||
(AccelControllerState.restrict, 0, False, 0.0, 15.0, MpcLongitudinalPlanSource.cruise),
|
||||
(AccelControllerState.restrict, 0, False, -0.01, 15.0, MpcLongitudinalPlanSource.cruise),
|
||||
(AccelControllerState.restrict, 0, False, 0.30, 20.0 - MPC_DECEL_JERK_MAX_TARGET_REDUCTION, MpcLongitudinalPlanSource.cruise),
|
||||
(AccelControllerState.restrict, 0, False, 0.30, 20.0, MpcLongitudinalPlanSource.cruise),
|
||||
(AccelControllerState.restrict, 0, False, 0.30, 25.0, MpcLongitudinalPlanSource.cruise),
|
||||
],
|
||||
)
|
||||
def test_non_routine_or_stock_lead_states_keep_stock_jerk_cost(
|
||||
state, selected_lead, launching, required_decel, target_speed, mpc_source,
|
||||
):
|
||||
planner, _ = planner_for_mpc_test(
|
||||
state=state, selected_lead=selected_lead, launching=launching,
|
||||
required_decel=required_decel, target_speed=target_speed, mpc_source=mpc_source,
|
||||
)
|
||||
_, calls = run_controller_mpc(planner)
|
||||
|
||||
assert calls[0][1] == {"jerk_cost_multiplier": 1.0}
|
||||
|
||||
|
||||
def test_controller_receives_previous_mpc_state_and_cached_radar_freshness():
|
||||
planner, _ = planner_for_mpc_test(mpc_source=log.LongitudinalPlan.LongitudinalPlanSource.lead0)
|
||||
planner._radar_fresh_this_cycle = True
|
||||
planner.a_desired = -0.4
|
||||
planner.v_desired_filter = SimpleNamespace(x=9.5)
|
||||
run_controller_mpc(planner)
|
||||
received = planner.accel_controller.update_kwargs
|
||||
|
||||
assert received["previous_mpc_source"] == log.LongitudinalPlan.LongitudinalPlanSource.lead0
|
||||
assert received["planner_speed"] == 9.5
|
||||
assert received["planner_accel"] == -0.4
|
||||
assert received["radar_fresh"] is True
|
||||
|
||||
|
||||
def test_controller_is_disabled_when_openpilot_longitudinal_control_is_unavailable():
|
||||
controller = AccelController(SimpleNamespace(longitudinalActuatorDelay=0.1, openpilotLongitudinalControl=False))
|
||||
controller.enabled = True
|
||||
assert not controller.is_enabled
|
||||
|
||||
|
||||
def test_radar_freshness_is_computed_once_and_shared_with_dec_and_controller():
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner._radar_log_mono_time = None
|
||||
planner._radar_fresh_this_cycle = True
|
||||
planner.events_sp = SimpleNamespace(clear=lambda: None)
|
||||
dec_freshness = []
|
||||
planner.dec = SimpleNamespace(update=lambda _sm, *, radar_fresh, planner_accel: dec_freshness.append(radar_fresh))
|
||||
planner.e2e_alerts_helper = SimpleNamespace(update=lambda *_args: None)
|
||||
planner.output_a_target = 0.0
|
||||
planner.output_v_target = 20.0
|
||||
planner.output_should_stop = False
|
||||
planner.allow_throttle = True
|
||||
planner.a_desired = 0.0
|
||||
planner.v_desired_filter = SimpleNamespace(x=10.0)
|
||||
planner.mpc = SimpleNamespace(source=log.LongitudinalPlan.LongitudinalPlanSource.cruise, last_solution_status=0)
|
||||
planner.is_e2e = lambda _sm: False
|
||||
planner._run_mpc = lambda *_args, **_kwargs: None
|
||||
planner.accel_controller = ControllerStub(target_speed=20.0, active=False)
|
||||
|
||||
sm = PlannerSM(100)
|
||||
for expected in (True, False):
|
||||
planner.update(sm)
|
||||
planner.update_mpc(sm, 20.0, True, ACCEL_MAX, False)
|
||||
assert dec_freshness[-1] is expected and planner.accel_controller.update_kwargs["radar_fresh"] is expected
|
||||
|
||||
sm.logMonoTime["radarState"] = 101
|
||||
planner.update(sm)
|
||||
planner.update_mpc(sm, 20.0, True, ACCEL_MAX, False)
|
||||
assert dec_freshness[-1] is True and planner.accel_controller.update_kwargs["radar_fresh"] is True
|
||||
|
||||
|
||||
def test_accel_controller_status_publishes_minimal_fields():
|
||||
planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP)
|
||||
planner.source = LongitudinalPlanSource.cruise
|
||||
planner.output_v_target = 20.0
|
||||
planner.output_a_target = 0.0
|
||||
planner.events_sp = SimpleNamespace(to_msg=list)
|
||||
planner.dec = SimpleNamespace(mode=lambda: "acc", enabled=lambda: False, active=lambda: False)
|
||||
planner.accel_controller = ControllerStub(active=False, state=AccelControllerState.restrict)
|
||||
planner.scc = SimpleNamespace(
|
||||
vision=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, current_lat_acc=0.0, max_pred_lat_acc=0.0, is_enabled=False, is_active=False),
|
||||
map=SimpleNamespace(state=0, output_v_target=20.0, output_a_target=0.0, is_enabled=False, is_active=False),
|
||||
)
|
||||
planner.resolver = SimpleNamespace(
|
||||
speed_limit=0.0, speed_limit_last=0.0, speed_limit_final=0.0, speed_limit_final_last=0.0,
|
||||
speed_limit_valid=False, speed_limit_last_valid=False, speed_limit_offset=0.0, distance=0.0,
|
||||
source=custom.LongitudinalPlanSP.SpeedLimit.Source.none,
|
||||
)
|
||||
planner.sla = SimpleNamespace(
|
||||
state=custom.LongitudinalPlanSP.SpeedLimit.AssistState.disabled, is_enabled=False, is_active=False,
|
||||
output_v_target=20.0, output_a_target=0.0,
|
||||
)
|
||||
planner.e2e_alerts_helper = SimpleNamespace(green_light_alert=False, lead_depart_alert=False)
|
||||
sent = {}
|
||||
planner.publish_longitudinal_plan_sp(
|
||||
SimpleNamespace(all_checks=lambda service_list: True),
|
||||
SimpleNamespace(send=lambda service, message: sent.update({service: message})),
|
||||
)
|
||||
|
||||
telemetry = sent["longitudinalPlanSP"].longitudinalPlanSP.accelController
|
||||
assert telemetry.enabled and not telemetry.active
|
||||
assert telemetry.profile == int(AccelProfile.normal)
|
||||
assert telemetry.state == int(AccelControllerState.restrict)
|
||||
assert set(custom.LongitudinalPlanSP.AccelController.schema.fields) == {"enabled", "active", "shadowOnlyDEPRECATED", "profile", "state"}
|
||||
+83
@@ -0,0 +1,83 @@
|
||||
import pytest
|
||||
|
||||
from opendbc.car import DT_CTRL, gen_empty_fingerprint, structs
|
||||
from opendbc.car.car_helpers import interfaces
|
||||
from opendbc.car.ford.values import CAR as FORD
|
||||
from opendbc.car.gm.values import CAR as GM
|
||||
from opendbc.car.honda.values import CAR as HONDA
|
||||
from opendbc.car.hyundai.values import CAR as HYUNDAI
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl, LongCtrlState, long_control_state_trans
|
||||
|
||||
|
||||
VEHICLES = [
|
||||
pytest.param(TOYOTA.TOYOTA_RAV4_TSS2, (True, False, 0.0, -2.0, 0.25, 0.25, 0.3), id="toyota-rav4-tss2"),
|
||||
pytest.param(HONDA.HONDA_ACCORD, (True, False, 0.0, -2.0, 0.5, 0.5, 0.8), id="honda-accord"),
|
||||
pytest.param(GM.CHEVROLET_BOLT_EUV, (True, False, 0.0, -2.0, 0.25, 0.25, 2.0), id="gm-bolt-euv"),
|
||||
pytest.param(HYUNDAI.HYUNDAI_SONATA, (True, True, 1.0, -2.0, 0.1, 0.5, 0.8), id="hyundai-sonata"),
|
||||
pytest.param(FORD.FORD_ESCAPE_MK4, (True, False, 0.0, -2.0, 0.5, 0.5, 0.8), id="ford-escape"),
|
||||
]
|
||||
|
||||
|
||||
def get_car_params(candidate):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
interface = interfaces[candidate]
|
||||
CP = interface.get_params(candidate, fingerprint, [], True, False, False)
|
||||
CP_SP = interface.get_params_sp(CP, candidate, fingerprint, [], True, False, False)
|
||||
return CP, CP_SP
|
||||
|
||||
|
||||
@pytest.mark.parametrize(("candidate", "expected"), VEHICLES)
|
||||
def test_real_vehicle_longcontrol_stop_and_start(candidate, expected):
|
||||
CP, CP_SP = get_car_params(candidate)
|
||||
expected_long, expected_starting, *expected_tuning = expected
|
||||
|
||||
assert CP.openpilotLongitudinalControl is expected_long
|
||||
assert CP.startingState is expected_starting
|
||||
assert (CP.startAccel, CP.stopAccel, CP.vEgoStarting, CP.vEgoStopping, CP.stoppingDecelRate) == pytest.approx(expected_tuning)
|
||||
|
||||
stop_speeds = [CP.vEgoStopping - 0.01] * 2
|
||||
drive_speeds = [CP.vEgoStopping + 0.01] * 2
|
||||
_, should_stop = get_accel_from_plan(stop_speeds, [0.0, 0.0], [0.0, 1.0], vEgoStopping=CP.vEgoStopping)
|
||||
_, should_drive = get_accel_from_plan(drive_speeds, [0.0, 0.0], [0.0, 1.0], vEgoStopping=CP.vEgoStopping)
|
||||
assert should_stop
|
||||
assert not should_drive
|
||||
|
||||
departure_state = long_control_state_trans(
|
||||
CP,
|
||||
CP_SP,
|
||||
True,
|
||||
LongCtrlState.stopping,
|
||||
CP.vEgoStarting - 0.01,
|
||||
should_drive,
|
||||
brake_pressed=False,
|
||||
cruise_standstill=False,
|
||||
)
|
||||
assert departure_state == (LongCtrlState.starting if CP.startingState else LongCtrlState.pid)
|
||||
assert (
|
||||
long_control_state_trans(
|
||||
CP,
|
||||
CP_SP,
|
||||
True,
|
||||
departure_state,
|
||||
CP.vEgoStarting + 0.01,
|
||||
should_drive,
|
||||
brake_pressed=False,
|
||||
cruise_standstill=False,
|
||||
)
|
||||
== LongCtrlState.pid
|
||||
)
|
||||
|
||||
CS = structs.CarState()
|
||||
CS.vEgo = 0.0
|
||||
CS.aEgo = 0.0
|
||||
control = LongControl(CP, CP_SP)
|
||||
|
||||
stopping_accel = control.update(True, CS, 0.0, should_stop, (-3.0, 2.0))
|
||||
assert control.long_control_state == LongCtrlState.stopping
|
||||
assert stopping_accel == pytest.approx(-CP.stoppingDecelRate * DT_CTRL)
|
||||
|
||||
departure_accel = control.update(True, CS, 0.0, should_drive, (-3.0, 2.0))
|
||||
assert control.long_control_state == departure_state
|
||||
assert departure_accel == pytest.approx(CP.startAccel)
|
||||
@@ -0,0 +1,38 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, 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.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
|
||||
|
||||
|
||||
class LongitudinalMpcSP:
|
||||
def __init__(self) -> None:
|
||||
self._accel_max_trajectory: tuple[float, ...] | None = None
|
||||
self._jerk_cost_multiplier = 1.0
|
||||
self.last_solution_status = 0
|
||||
|
||||
def set_accel_controller_params(self, accel_max: tuple[float, ...] | None, jerk_cost_multiplier: float) -> None:
|
||||
self._accel_max_trajectory = accel_max
|
||||
self._jerk_cost_multiplier = jerk_cost_multiplier
|
||||
|
||||
def scale_jerk_cost(self, jerk_cost: float) -> float:
|
||||
return jerk_cost * self._jerk_cost_multiplier
|
||||
|
||||
def apply_accel_limits(self) -> None:
|
||||
if self._accel_max_trajectory is None:
|
||||
return
|
||||
|
||||
accel_max = np.asarray(self._accel_max_trajectory)
|
||||
if accel_max.shape != self.params[:, 1].shape or accel_max.dtype.kind not in "iuf" or not np.all(np.isfinite(accel_max)):
|
||||
return
|
||||
|
||||
self.params[:, 1] = np.clip(accel_max, 0.0, ACCEL_MAX)
|
||||
self.params[0, 1] = max(self.params[0, 1], float(np.clip(self.x0[2], ACCEL_MIN, ACCEL_MAX)))
|
||||
|
||||
def save_solution_status(self) -> None:
|
||||
self.last_solution_status = self.solution_status
|
||||
@@ -5,10 +5,12 @@ 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.
|
||||
"""
|
||||
|
||||
from cereal import messaging, custom
|
||||
from cereal import custom, messaging
|
||||
from opendbc.car import structs
|
||||
from openpilot.common.constants import CV
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelController, AccelControllerState
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.e2e_alerts_helper import E2EAlertsHelper
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.smart_cruise_control import SmartCruiseControl
|
||||
@@ -22,9 +24,10 @@ LongitudinalPlanSource = custom.LongitudinalPlanSP.LongitudinalPlanSource
|
||||
|
||||
|
||||
class LongitudinalPlannerSP:
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc):
|
||||
def __init__(self, CP: structs.CarParams, CP_SP: structs.CarParamsSP, mpc, dt: float = DT_MDL):
|
||||
self.mpc = mpc
|
||||
self.accel_controller = AccelController(CP, dt=dt)
|
||||
self.events_sp = EventsSP()
|
||||
self.resolver = SpeedLimitResolver()
|
||||
self.dec = DynamicExperimentalController(CP, mpc)
|
||||
self.scc = SmartCruiseControl()
|
||||
self.resolver = SpeedLimitResolver()
|
||||
@@ -32,6 +35,8 @@ class LongitudinalPlannerSP:
|
||||
self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None
|
||||
self.source = LongitudinalPlanSource.cruise
|
||||
self.e2e_alerts_helper = E2EAlertsHelper()
|
||||
self._radar_log_mono_time = None
|
||||
self._radar_fresh_this_cycle = True
|
||||
|
||||
self.output_v_target = 0.
|
||||
self.output_a_target = 0.
|
||||
@@ -43,6 +48,41 @@ class LongitudinalPlannerSP:
|
||||
|
||||
return experimental_mode and self.dec.mode() == "blended"
|
||||
|
||||
def _run_mpc(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool, accel_max=None, *, jerk_cost_multiplier: float = 1.0) -> None:
|
||||
self.mpc.set_accel_controller_params(accel_max, jerk_cost_multiplier)
|
||||
self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)
|
||||
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
|
||||
self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality)
|
||||
|
||||
def update_mpc(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool, stock_accel_max: float, reset_state: bool) -> bool:
|
||||
is_e2e = self.is_e2e(sm)
|
||||
force_decel = sm['controlsState'].forceDecel
|
||||
previous_mpc_failed = self.mpc.last_solution_status != 0
|
||||
if previous_mpc_failed:
|
||||
self.accel_controller.reset()
|
||||
|
||||
self.accel_controller.update(
|
||||
sm['radarState'], base_speed=self.output_v_target, v_ego=sm['carState'].vEgo, a_ego=sm['carState'].aEgo,
|
||||
follow_personality=sm['selfdriveState'].personality, acc_selected=not is_e2e and not previous_mpc_failed,
|
||||
engaged=not reset_state and not force_decel, cruise_initialized=sm['carState'].vCruise != V_CRUISE_UNSET,
|
||||
stock_accel_max=stock_accel_max if self.allow_throttle else 0.0, previous_should_stop=self.output_should_stop,
|
||||
radar_fresh=self._radar_fresh_this_cycle, previous_mpc_source=self.mpc.source, planner_speed=self.v_desired_filter.x,
|
||||
planner_accel=self.a_desired,
|
||||
)
|
||||
controller = self.accel_controller
|
||||
actuating = controller.is_active and not is_e2e and not force_decel and not previous_mpc_failed
|
||||
valid_lead_stop_hold = actuating and controller.state == AccelControllerState.stopHold and controller.selected_lead >= 0
|
||||
controller_v_cruise = v_cruise if valid_lead_stop_hold else min(v_cruise, controller.output_v_target) if actuating else v_cruise
|
||||
accel_max = controller.mpc_accel_max if actuating else None
|
||||
jerk_cost_multiplier = controller.get_jerk_cost_multiplier(
|
||||
actuating, prev_accel_constraint, v_cruise - controller_v_cruise, previous_mpc_failed,
|
||||
)
|
||||
self._run_mpc(sm, controller_v_cruise, prev_accel_constraint, accel_max, jerk_cost_multiplier=jerk_cost_multiplier)
|
||||
return is_e2e
|
||||
|
||||
def update_should_stop(self, should_stop: bool) -> bool:
|
||||
return self.accel_controller.update_should_stop(should_stop)
|
||||
|
||||
def update_targets(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> tuple[float, float]:
|
||||
CS = sm['carState']
|
||||
v_cruise_cluster_kph = min(CS.vCruiseCluster, V_CRUISE_MAX)
|
||||
@@ -73,7 +113,17 @@ class LongitudinalPlannerSP:
|
||||
self.output_v_target, self.output_a_target = targets[self.source]
|
||||
return self.output_v_target, self.output_a_target
|
||||
|
||||
def _update_radar_freshness(self, sm: messaging.SubMaster) -> bool:
|
||||
radar_log_mono_time = sm.logMonoTime['radarState']
|
||||
radar_healthy = sm.valid['radarState'] and sm.alive['radarState']
|
||||
radar_advanced = self._radar_log_mono_time is None or radar_log_mono_time > self._radar_log_mono_time
|
||||
if radar_advanced:
|
||||
self._radar_log_mono_time = radar_log_mono_time
|
||||
return radar_healthy and radar_advanced
|
||||
|
||||
def update(self, sm: messaging.SubMaster) -> None:
|
||||
self._radar_fresh_this_cycle = self._update_radar_freshness(sm)
|
||||
self.accel_controller.update_params()
|
||||
self.events_sp.clear()
|
||||
self.dec.update(sm)
|
||||
self.e2e_alerts_helper.update(sm, self.events_sp)
|
||||
@@ -95,6 +145,12 @@ class LongitudinalPlannerSP:
|
||||
dec.enabled = self.dec.enabled()
|
||||
dec.active = self.dec.active()
|
||||
|
||||
accelController = longitudinalPlanSP.accelController
|
||||
accelController.enabled = self.accel_controller.is_enabled
|
||||
accelController.active = self.accel_controller.is_active
|
||||
accelController.profile = self.accel_controller.profile
|
||||
accelController.state = self.accel_controller.state
|
||||
|
||||
# Smart Cruise Control
|
||||
smartCruiseControl = longitudinalPlanSP.smartCruiseControl
|
||||
# Vision Control
|
||||
|
||||
+82
@@ -0,0 +1,82 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, 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.
|
||||
"""
|
||||
import gc
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP as Plant
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanSource
|
||||
from openpilot.sunnypilot.selfdrive.controls.lib.smart_cruise_control.vision_controller import _A_LAT_REG_MAX
|
||||
|
||||
|
||||
def _run_constant_curve(*, scc_enabled: bool, cruise: float, duration: float = 70.) -> dict[str, np.ndarray]:
|
||||
gc.collect()
|
||||
curvature = 0.005
|
||||
plant = Plant(lead_relevancy=False, speed=30., actuator_delay=0.15, actuator_lag=0.20)
|
||||
planner = plant.planner
|
||||
planner.accel_controller.enabled = False
|
||||
planner.accel_controller.update_params = lambda: None
|
||||
planner.dec._enabled = False
|
||||
planner.dec._read_params = lambda: None
|
||||
planner.scc.map.enabled = False
|
||||
planner.scc.map.update_params = lambda: None
|
||||
planner.scc.vision.enabled = scc_enabled
|
||||
planner.scc.vision._update_params = lambda: None
|
||||
|
||||
if scc_enabled:
|
||||
original_update_calculations = planner.scc.vision._update_calculations
|
||||
|
||||
def inject_constant_curvature(sm):
|
||||
velocities = np.asarray(sm['modelV2'].velocity.x, dtype=float)
|
||||
sm['modelV2'].orientationRate.z = (curvature * velocities).tolist()
|
||||
sm['controlsState'].curvature = curvature
|
||||
original_update_calculations(sm)
|
||||
|
||||
planner.scc.vision._update_calculations = inject_constant_curvature
|
||||
|
||||
original_update = planner.update
|
||||
|
||||
def enable_longitudinal(sm):
|
||||
sm['carControl'].enabled = True
|
||||
sm['carControl'].longActive = True
|
||||
original_update(sm)
|
||||
|
||||
planner.update = enable_longitudinal
|
||||
rows = []
|
||||
while plant.current_time < duration:
|
||||
output = plant.step(v_cruise=cruise)
|
||||
rows.append((
|
||||
plant.current_time, output['speed'], planner.mpc.last_solution_status, output['should_stop'],
|
||||
planner.scc.vision.is_active, planner.source == LongitudinalPlanSource.sccVision,
|
||||
planner.scc.vision.output_v_target,
|
||||
))
|
||||
|
||||
data = np.asarray(rows, dtype=float)
|
||||
gc.collect()
|
||||
return {
|
||||
'time': data[:, 0], 'speed': data[:, 1], 'solver_status': data[:, 2], 'should_stop': data[:, 3],
|
||||
'active': data[:, 4], 'scc_source': data[:, 5], 'target': data[:, 6],
|
||||
}
|
||||
|
||||
|
||||
def test_constant_curve_recovers_like_stock_speed_cap():
|
||||
target = (_A_LAT_REG_MAX / 0.005) ** 0.5
|
||||
scc = _run_constant_curve(scc_enabled=True, cruise=30.)
|
||||
stock = _run_constant_curve(scc_enabled=False, cruise=target)
|
||||
scc_final = scc['speed'][scc['time'] >= 60.]
|
||||
stock_final = stock['speed'][stock['time'] >= 60.]
|
||||
|
||||
assert not scc['solver_status'].any()
|
||||
assert not stock['solver_status'].any()
|
||||
assert not scc['should_stop'].any()
|
||||
assert np.all(scc['active'][scc['time'] >= 60.])
|
||||
assert np.all(scc['scc_source'][scc['time'] >= 60.])
|
||||
assert np.allclose(scc['target'][scc['time'] >= 60.], target)
|
||||
assert scc_final.min() >= target - 1.
|
||||
assert abs(scc_final.mean() - stock_final.mean()) < 0.5
|
||||
assert abs(scc_final.min() - stock_final.min()) < 1.
|
||||
assert abs(scc_final.max() - stock_final.max()) < 1.
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,402 @@
|
||||
"""
|
||||
Copyright (c) 2021-, Haibin Wen, 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.
|
||||
"""
|
||||
|
||||
from collections import deque
|
||||
from collections.abc import Callable
|
||||
from dataclasses import dataclass
|
||||
import math
|
||||
import time
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from cereal import log
|
||||
import cereal.messaging as messaging
|
||||
from openpilot.common.realtime import DT_MDL, Ratekeeper
|
||||
from openpilot.selfdrive.modeld.constants import ModelConstants
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
|
||||
from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
|
||||
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant, PlannerSM
|
||||
|
||||
|
||||
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]]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ActuatorModel:
|
||||
planner_delay: float
|
||||
transport_delay: float
|
||||
actuator_lag: float
|
||||
command_rate_limit: float
|
||||
stopping_acceleration: float
|
||||
standstill_breakaway_acceleration: float
|
||||
standstill_breakaway_time: float
|
||||
|
||||
def __post_init__(self):
|
||||
nonnegative_fields = {
|
||||
"planner_delay": self.planner_delay,
|
||||
"transport_delay": self.transport_delay,
|
||||
"actuator_lag": self.actuator_lag,
|
||||
"standstill_breakaway_acceleration": self.standstill_breakaway_acceleration,
|
||||
"standstill_breakaway_time": self.standstill_breakaway_time,
|
||||
}
|
||||
if any(not math.isfinite(value) or value < 0.0 for value in nonnegative_fields.values()):
|
||||
raise ValueError(f"ActuatorModel fields must be finite and non-negative: {nonnegative_fields}")
|
||||
if not math.isfinite(self.command_rate_limit) or self.command_rate_limit <= 0.0:
|
||||
raise ValueError("command_rate_limit must be finite and positive")
|
||||
if not math.isfinite(self.stopping_acceleration) or self.stopping_acceleration > 0.0:
|
||||
raise ValueError("stopping_acceleration must be finite and non-positive")
|
||||
|
||||
|
||||
# Conservative Prius TSS2 actuator model.
|
||||
PRIUS_TSS2_ROUTE_MODEL = ActuatorModel(
|
||||
planner_delay=0.05,
|
||||
transport_delay=0.0,
|
||||
actuator_lag=0.20,
|
||||
command_rate_limit=4.0,
|
||||
stopping_acceleration=-2.0,
|
||||
standstill_breakaway_acceleration=1.0,
|
||||
standstill_breakaway_time=0.05,
|
||||
)
|
||||
|
||||
|
||||
class PlantSP(Plant):
|
||||
"""Closed-loop plant with configurable observations and actuator response."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
lead_relevancy=False,
|
||||
speed=0.0,
|
||||
distance_lead=2.0,
|
||||
enabled=True,
|
||||
only_lead2=False,
|
||||
only_radar=False,
|
||||
e2e=False,
|
||||
personality=0,
|
||||
force_decel=False,
|
||||
lead_observation_fn: LeadObservationFn | None = None,
|
||||
model_action_fn: ModelActionFn | None = None,
|
||||
ego_observation_fn: EgoObservationFn | None = None,
|
||||
actuator_delay: float | None = None,
|
||||
actuator_lag: float = 0.0,
|
||||
actuator_model: ActuatorModel | None = None,
|
||||
):
|
||||
if actuator_delay is not None and (not math.isfinite(actuator_delay) or actuator_delay < 0.0):
|
||||
raise ValueError("actuator_delay must be finite and non-negative")
|
||||
if not math.isfinite(actuator_lag) or actuator_lag < 0.0:
|
||||
raise ValueError("actuator_lag must be finite and non-negative")
|
||||
|
||||
self.rate = 1.0 / DT_MDL
|
||||
|
||||
if not Plant.messaging_initialized:
|
||||
Plant.radar = messaging.pub_sock('radarState')
|
||||
Plant.controls_state = messaging.pub_sock('controlsState')
|
||||
Plant.selfdrive_state = messaging.pub_sock('selfdriveState')
|
||||
Plant.car_state = messaging.pub_sock('carState')
|
||||
Plant.plan = messaging.sub_sock('longitudinalPlan')
|
||||
Plant.messaging_initialized = True
|
||||
|
||||
self.v_lead_prev = 0.0
|
||||
|
||||
self.distance = 0.0
|
||||
self.speed = speed
|
||||
self.should_stop = False
|
||||
self.acceleration = 0.0
|
||||
self.a_target = 0.0
|
||||
self.actuator_command = 0.0
|
||||
self.applied_actuator_command = 0.0
|
||||
self.breakaway_confirmed = False
|
||||
self._breakaway_timer = 0.0
|
||||
|
||||
# lead car
|
||||
self.lead_relevancy = lead_relevancy
|
||||
self.distance_lead = distance_lead
|
||||
self.enabled = enabled
|
||||
self.only_lead2 = only_lead2
|
||||
self.only_radar = only_radar
|
||||
self.e2e = e2e
|
||||
self.personality = personality
|
||||
self.force_decel = force_decel
|
||||
self.lead_observation_fn = lead_observation_fn
|
||||
self.model_action_fn = model_action_fn
|
||||
self.ego_observation_fn = ego_observation_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
|
||||
self.actuator_lag = actuator_model.actuator_lag if actuator_model is not None else actuator_lag
|
||||
self.publish_realized_a_ego = any((lead_observation_fn is not None, model_action_fn is not None, ego_observation_fn is not None,
|
||||
actuator_delay is not None, actuator_lag > 0.0, actuator_model is not None))
|
||||
|
||||
self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
|
||||
self.ts = 1.0 / self.rate
|
||||
time.sleep(0.1)
|
||||
self.sm = messaging.SubMaster(['longitudinalPlan'])
|
||||
|
||||
from opendbc.car.honda.values import CAR
|
||||
from opendbc.car.honda.interface import CarInterface
|
||||
|
||||
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
|
||||
if self.actuator_delay is not None:
|
||||
CP.longitudinalActuatorDelay = self.actuator_delay
|
||||
CP_SP = CarInterface.get_non_essential_params_sp(CP, CAR.HONDA_CIVIC)
|
||||
self.planner = LongitudinalPlanner(CP, CP_SP, init_v=self.speed)
|
||||
|
||||
if self.actuator_model is not None and self.speed >= 0.01:
|
||||
self.breakaway_confirmed = True
|
||||
delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.ts)
|
||||
self._actuator_delay_queue = deque([self.acceleration] * delay_steps)
|
||||
|
||||
@staticmethod
|
||||
def _lead_message(observation: LeadObservation):
|
||||
lead = log.RadarState.LeadData.new_message()
|
||||
for field, value in observation.items():
|
||||
setattr(lead, field, value)
|
||||
return lead
|
||||
|
||||
def _observe_lead(self, lead_name: str, truth: LeadObservation, present_by_default: bool) -> LeadObservation | None:
|
||||
if self.lead_observation_fn is None:
|
||||
return dict(truth) if present_by_default else None
|
||||
|
||||
observed = self.lead_observation_fn(self.current_time, lead_name, dict(truth))
|
||||
if observed is None:
|
||||
return None
|
||||
|
||||
complete_observation = dict(truth)
|
||||
complete_observation.update(observed)
|
||||
return complete_observation
|
||||
|
||||
def _update_actuator(self, command: float) -> tuple[float, float]:
|
||||
if self._actuator_delay_queue:
|
||||
self._actuator_delay_queue.append(command)
|
||||
delayed_command = self._actuator_delay_queue.popleft()
|
||||
else:
|
||||
delayed_command = command
|
||||
|
||||
if self.actuator_model is not None:
|
||||
max_command_delta = self.actuator_model.command_rate_limit * self.ts
|
||||
self.applied_actuator_command = float(np.clip(delayed_command,
|
||||
self.applied_actuator_command - max_command_delta,
|
||||
self.applied_actuator_command + max_command_delta))
|
||||
|
||||
if self.speed < 0.01:
|
||||
if self.applied_actuator_command <= 0.0:
|
||||
self.breakaway_confirmed = False
|
||||
self._breakaway_timer = 0.0
|
||||
elif not self.breakaway_confirmed:
|
||||
breakaway_ready = self.applied_actuator_command + 1e-9 >= self.actuator_model.standstill_breakaway_acceleration
|
||||
if breakaway_ready:
|
||||
self._breakaway_timer += self.ts
|
||||
else:
|
||||
self._breakaway_timer = 0.0
|
||||
|
||||
self.breakaway_confirmed = breakaway_ready and self._breakaway_timer + 1e-9 >= self.actuator_model.standstill_breakaway_time
|
||||
if not self.breakaway_confirmed:
|
||||
self.acceleration = 0.0
|
||||
return delayed_command, self.acceleration
|
||||
else:
|
||||
self.breakaway_confirmed = True
|
||||
|
||||
response_command = self.applied_actuator_command
|
||||
else:
|
||||
self.applied_actuator_command = delayed_command
|
||||
response_command = delayed_command
|
||||
|
||||
if self.actuator_lag > 0.0:
|
||||
alpha = 1.0 - math.exp(-self.ts / self.actuator_lag)
|
||||
self.acceleration += alpha * (response_command - self.acceleration)
|
||||
else:
|
||||
self.acceleration = response_command
|
||||
return delayed_command, self.acceleration
|
||||
|
||||
def step(self, v_lead=0.0, prob_lead=1.0, v_cruise=50.0, pitch=0.0, prob_throttle=1.0):
|
||||
# ******** publish a fake model going straight and fake calibration ********
|
||||
# note that this is worst case for MPC, since model will delay long mpc by one time step
|
||||
radar = messaging.new_message('radarState')
|
||||
control = messaging.new_message('controlsState')
|
||||
ss = messaging.new_message('selfdriveState')
|
||||
car_state = messaging.new_message('carState')
|
||||
lp = messaging.new_message('liveParameters')
|
||||
car_control = messaging.new_message('carControl')
|
||||
model = messaging.new_message('modelV2')
|
||||
car_state_sp = messaging.new_message('carStateSP')
|
||||
live_map_data_sp = messaging.new_message('liveMapDataSP')
|
||||
gps_data = messaging.new_message('gpsLocation')
|
||||
a_lead = (v_lead - self.v_lead_prev) / self.ts
|
||||
self.v_lead_prev = v_lead
|
||||
|
||||
if self.lead_relevancy:
|
||||
d_rel = np.maximum(0.0, self.distance_lead - self.distance)
|
||||
v_rel = v_lead - self.speed
|
||||
if self.only_radar:
|
||||
status = True
|
||||
elif prob_lead > 0.5:
|
||||
status = True
|
||||
else:
|
||||
status = False
|
||||
else:
|
||||
d_rel = 200.0
|
||||
v_rel = 0.0
|
||||
prob_lead = 0.0
|
||||
status = False
|
||||
|
||||
truth_lead: LeadObservation = {
|
||||
"dRel": float(d_rel),
|
||||
"yRel": 0.0,
|
||||
"vRel": float(v_rel),
|
||||
"aRel": float(a_lead - self.acceleration),
|
||||
"vLead": float(v_lead),
|
||||
"dPath": 0.0,
|
||||
"vLat": 0.0,
|
||||
"vLeadK": float(v_lead),
|
||||
"aLeadK": float(a_lead),
|
||||
"fcw": False,
|
||||
"status": bool(status),
|
||||
# TODO use real radard logic for this
|
||||
"aLeadTau": float(_LEAD_ACCEL_TAU),
|
||||
"modelProb": float(prob_lead),
|
||||
"radar": bool(self.only_radar),
|
||||
"radarTrackId": -1,
|
||||
}
|
||||
lead_one_observation = self._observe_lead("leadOne", truth_lead, not self.only_lead2)
|
||||
lead_two_observation = self._observe_lead("leadTwo", truth_lead, True)
|
||||
if lead_one_observation is not None:
|
||||
radar.radarState.leadOne = self._lead_message(lead_one_observation)
|
||||
if lead_two_observation is not None:
|
||||
radar.radarState.leadTwo = self._lead_message(lead_two_observation)
|
||||
|
||||
# Simulate model predicting slightly faster speed
|
||||
# this is to ensure lead policy is effective when model
|
||||
# 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)]
|
||||
model.modelV2.position = position
|
||||
if self.model_action_fn is None:
|
||||
model_acceleration, model_should_stop = self.acceleration + 0.1, False
|
||||
else:
|
||||
model_acceleration, model_should_stop = self.model_action_fn(self.current_time, self.speed, self.acceleration)
|
||||
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
|
||||
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)]
|
||||
|
||||
control.controlsState.longControlState = LongCtrlState.pid if self.enabled else LongCtrlState.off
|
||||
ss.selfdriveState.experimentalMode = self.e2e
|
||||
ss.selfdriveState.personality = self.personality
|
||||
control.controlsState.forceDecel = self.force_decel
|
||||
true_v_ego = self.speed
|
||||
true_a_ego = self.acceleration
|
||||
published_v_ego = true_v_ego
|
||||
published_a_ego = true_a_ego if self.publish_realized_a_ego else 0.0
|
||||
if self.ego_observation_fn is not None:
|
||||
published_v_ego, published_a_ego = self.ego_observation_fn(self.current_time, true_v_ego, true_a_ego)
|
||||
car_state.carState.vEgo = float(published_v_ego)
|
||||
car_state.carState.aEgo = float(published_a_ego)
|
||||
car_state.carState.standstill = bool(self.speed < 0.01)
|
||||
car_state.carState.vCruise = float(v_cruise * 3.6)
|
||||
car_control.carControl.orientationNED = [0.0, float(pitch), 0.0]
|
||||
|
||||
# ******** get controlsState messages for plotting ***
|
||||
sm = PlannerSM(self.rk.frame, {
|
||||
'radarState': radar.radarState,
|
||||
'carState': car_state.carState,
|
||||
'carControl': car_control.carControl,
|
||||
'controlsState': control.controlsState,
|
||||
'selfdriveState': ss.selfdriveState,
|
||||
'liveParameters': lp.liveParameters,
|
||||
'modelV2': model.modelV2,
|
||||
'carStateSP': car_state_sp.carStateSP,
|
||||
'liveMapDataSP': live_map_data_sp.liveMapDataSP,
|
||||
'gpsLocation': gps_data.gpsLocation,
|
||||
})
|
||||
self.planner.update(sm)
|
||||
self.a_target = self.planner.output_a_target
|
||||
self.actuator_command = self.a_target
|
||||
if self.planner.output_should_stop:
|
||||
stopping_acceleration = -0.5 if self.actuator_model is None else self.actuator_model.stopping_acceleration
|
||||
self.actuator_command = min(stopping_acceleration, self.actuator_command)
|
||||
delayed_actuator_command, _ = self._update_actuator(self.actuator_command)
|
||||
self.speed = self.speed + self.acceleration * self.ts
|
||||
self.should_stop = self.planner.output_should_stop
|
||||
fcw = self.planner.fcw
|
||||
self.distance_lead = self.distance_lead + v_lead * self.ts
|
||||
|
||||
# ******** run the car ********
|
||||
# print(self.distance, speed)
|
||||
if self.speed <= 0:
|
||||
self.speed = 0
|
||||
self.acceleration = 0
|
||||
self.distance = self.distance + self.speed * self.ts
|
||||
|
||||
# *** radar model ***
|
||||
if self.lead_relevancy:
|
||||
d_rel = np.maximum(0.0, self.distance_lead - self.distance)
|
||||
v_rel = v_lead - self.speed
|
||||
else:
|
||||
d_rel = 200.0
|
||||
v_rel = 0.0
|
||||
|
||||
# print at 5hz
|
||||
# if (self.rk.frame % (self.rate // 5)) == 0:
|
||||
# print("%2.2f sec %6.2f m %6.2f m/s %6.2f m/s2 lead_rel: %6.2f m %6.2f m/s"
|
||||
# % (self.current_time, self.distance, self.speed, self.acceleration, d_rel, v_rel))
|
||||
|
||||
# ******** update prevs ********
|
||||
self.rk.monitor_time()
|
||||
|
||||
accel_controller = self.planner.accel_controller
|
||||
lead_plan = accel_controller._held_lead_plan
|
||||
target_state = accel_controller.target_state
|
||||
return {
|
||||
"distance": self.distance,
|
||||
"speed": self.speed,
|
||||
"acceleration": self.acceleration,
|
||||
"realized_acceleration": self.acceleration,
|
||||
"a_target": self.a_target,
|
||||
"planner_acceleration": self.a_target,
|
||||
"actuator_command": self.actuator_command,
|
||||
"stop_clamped_actuator_command": self.actuator_command,
|
||||
"delayed_actuator_command": delayed_actuator_command,
|
||||
"applied_actuator_command": self.applied_actuator_command,
|
||||
"vehicle_actuator_command": self.applied_actuator_command,
|
||||
"true_v_ego": true_v_ego,
|
||||
"true_a_ego": true_a_ego,
|
||||
"published_a_ego": published_a_ego,
|
||||
"published_v_ego": published_v_ego,
|
||||
"observed_a_ego": published_a_ego,
|
||||
"observed_v_ego": published_v_ego,
|
||||
"planner_delay": self.actuator_delay,
|
||||
"transport_delay": self.transport_delay,
|
||||
"breakaway_confirmed": self.breakaway_confirmed,
|
||||
"breakaway_time": self._breakaway_timer,
|
||||
"should_stop": self.should_stop,
|
||||
"distance_lead": self.distance_lead,
|
||||
"fcw": fcw,
|
||||
"mpc_source": self.planner.mpc.source,
|
||||
"dec_mode": self.planner.dec.mode(),
|
||||
"controller_target": accel_controller.output_v_target,
|
||||
"base_target": self.planner.output_v_target,
|
||||
"raw_energy_cap": lead_plan.cap if lead_plan is not None else math.inf,
|
||||
"live_filtered_cap": target_state.filtered_cap,
|
||||
"accel_controller_selected_lead": accel_controller.selected_lead,
|
||||
"model_action": {
|
||||
"desiredAcceleration": float(model_acceleration),
|
||||
"shouldStop": bool(model_should_stop),
|
||||
},
|
||||
"truth_lead": dict(truth_lead),
|
||||
"lead_one_observation": None if lead_one_observation is None else dict(lead_one_observation),
|
||||
"lead_two_observation": None if lead_two_observation is None else dict(lead_two_observation),
|
||||
}
|
||||
@@ -0,0 +1,154 @@
|
||||
from collections.abc import Callable
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from openpilot.common.realtime import DT_MDL
|
||||
from openpilot.selfdrive.test.longitudinal_maneuvers.plant import Plant
|
||||
from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PlantSP
|
||||
|
||||
STOCK_STEP_KEYS = ("distance", "speed", "acceleration", "should_stop", "distance_lead", "fcw")
|
||||
|
||||
|
||||
def departing_lead(current_time: float) -> float:
|
||||
return 0.0 if current_time < 1.0 else min(2.0, 2.0 * (current_time - 1.0))
|
||||
|
||||
|
||||
PARITY_SCENARIOS = {
|
||||
"approach_stopped_lead": dict(lead_relevancy=True, speed=15.0, distance_lead=60.0, v_cruise=20.0, v_lead=0.0, steps=80),
|
||||
"stop_then_depart": dict(lead_relevancy=True, speed=0.0, distance_lead=6.0, v_cruise=8.0, v_lead=departing_lead, steps=120),
|
||||
}
|
||||
|
||||
|
||||
def _drive(cls, *, v_cruise: float, v_lead: float | Callable[[float], float], steps: int, **kwargs):
|
||||
plant = cls(**kwargs)
|
||||
plant.v_lead_prev = float(v_lead(0.0)) if callable(v_lead) else float(v_lead)
|
||||
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
|
||||
results = []
|
||||
for _ in range(steps):
|
||||
lead_speed = float(v_lead(plant.current_time)) if callable(v_lead) else v_lead
|
||||
result = plant.step(v_lead=lead_speed, v_cruise=v_cruise)
|
||||
results.append((result, plant.planner.mpc.source, plant.planner.output_a_target))
|
||||
return results, solver_failures
|
||||
|
||||
|
||||
@pytest.mark.parametrize("scenario", PARITY_SCENARIOS, ids=list(PARITY_SCENARIOS))
|
||||
def test_plant_sp_matches_stock_plant_on_shared_kwargs(scenario):
|
||||
kwargs = dict(PARITY_SCENARIOS[scenario])
|
||||
v_cruise, v_lead, steps = kwargs.pop("v_cruise"), kwargs.pop("v_lead"), kwargs.pop("steps")
|
||||
|
||||
stock_results, stock_failures = _drive(Plant, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
|
||||
sp_results, sp_failures = _drive(PlantSP, v_cruise=v_cruise, v_lead=v_lead, steps=steps, **kwargs)
|
||||
|
||||
assert stock_failures == 0, f"stock Plant solver failed {stock_failures} times in {scenario!r}"
|
||||
assert sp_failures == 0, f"PlantSP solver failed {sp_failures} times in {scenario!r}"
|
||||
|
||||
for frame, ((stock_result, stock_source, stock_a_target), (sp_result, sp_source, sp_a_target)) in enumerate(
|
||||
zip(stock_results, sp_results, strict=True),
|
||||
):
|
||||
for key in STOCK_STEP_KEYS:
|
||||
if isinstance(stock_result[key], float):
|
||||
assert sp_result[key] == pytest.approx(stock_result[key]), f"{scenario} frame {frame} key {key}"
|
||||
else:
|
||||
assert sp_result[key] == stock_result[key], f"{scenario} frame {frame} key {key}"
|
||||
assert sp_source == stock_source, f"{scenario} frame {frame} mpc.source"
|
||||
assert sp_a_target == pytest.approx(stock_a_target), f"{scenario} frame {frame} output_a_target"
|
||||
|
||||
if scenario == "stop_then_depart":
|
||||
departure_frame = round(1.0 / DT_MDL)
|
||||
for results in (stock_results, sp_results):
|
||||
assert all(result["speed"] < 0.01 for result, _, _ in results[:departure_frame])
|
||||
assert results[departure_frame - 1][0]["should_stop"]
|
||||
assert any(not result["should_stop"] for result, _, _ in results[departure_frame:])
|
||||
assert any(result["speed"] > 0.05 for result, _, _ in results[departure_frame:])
|
||||
stock_release = next(frame for frame, (result, _, _) in enumerate(stock_results) if frame >= departure_frame and not result["should_stop"])
|
||||
sp_release = next(frame for frame, (result, _, _) in enumerate(sp_results) if frame >= departure_frame and not result["should_stop"])
|
||||
stock_motion = next(frame for frame, (result, _, _) in enumerate(stock_results) if frame >= departure_frame and result["speed"] > 0.05)
|
||||
sp_motion = next(frame for frame, (result, _, _) in enumerate(sp_results) if frame >= departure_frame and result["speed"] > 0.05)
|
||||
assert sp_release == stock_release
|
||||
assert sp_motion == stock_motion
|
||||
|
||||
|
||||
def test_full_lead_observation_is_independent_from_truth():
|
||||
callback_inputs = []
|
||||
|
||||
def observe_lead(current_time, lead_name, truth):
|
||||
callback_inputs.append((current_time, lead_name, truth))
|
||||
if lead_name == "leadOne":
|
||||
return {
|
||||
"dRel": 12.5,
|
||||
"vRel": -4.0,
|
||||
"vLead": 6.0,
|
||||
"vLeadK": 5.5,
|
||||
"aLeadK": -1.25,
|
||||
"aLeadTau": 0.7,
|
||||
"status": True,
|
||||
"modelProb": 0.9,
|
||||
"radarTrackId": 42,
|
||||
}
|
||||
return None
|
||||
|
||||
plant = PlantSP(lead_relevancy=True, speed=10.0, distance_lead=50.0, lead_observation_fn=observe_lead)
|
||||
result = plant.step(v_lead=8.0)
|
||||
|
||||
assert [entry[1] for entry in callback_inputs] == ["leadOne", "leadTwo"]
|
||||
assert callback_inputs[0][2]["dRel"] == pytest.approx(50.0)
|
||||
assert result["truth_lead"]["dRel"] == pytest.approx(50.0)
|
||||
assert result["lead_one_observation"]["dRel"] == pytest.approx(12.5)
|
||||
assert result["lead_one_observation"]["radarTrackId"] == 42
|
||||
assert result["lead_two_observation"] is None
|
||||
assert result["distance_lead"] == pytest.approx(50.0 + 8.0 * DT_MDL)
|
||||
|
||||
|
||||
def test_model_action_realized_acceleration_and_source_logging():
|
||||
def model_action(current_time, v_ego, a_ego):
|
||||
return -1.25, True
|
||||
|
||||
plant = PlantSP(speed=10.0, e2e=True, force_decel=True, model_action_fn=model_action, actuator_lag=0.5)
|
||||
first = plant.step()
|
||||
second = plant.step()
|
||||
|
||||
assert first["model_action"] == {"desiredAcceleration": -1.25, "shouldStop": True}
|
||||
assert first["published_a_ego"] == pytest.approx(0.0)
|
||||
assert second["published_a_ego"] == pytest.approx(first["realized_acceleration"])
|
||||
assert first["acceleration"] == first["realized_acceleration"]
|
||||
assert abs(first["realized_acceleration"]) < abs(first["actuator_command"])
|
||||
assert first["mpc_source"] is not None
|
||||
assert first["dec_mode"] in ("acc", "blended")
|
||||
assert "controller_target" in first
|
||||
assert "base_target" in first
|
||||
assert "raw_energy_cap" in first
|
||||
assert "live_filtered_cap" in first
|
||||
assert "shadow_filtered_cap" not in first
|
||||
assert first["lead_one_observation"] is not None
|
||||
assert first["truth_lead"] == first["lead_one_observation"]
|
||||
|
||||
|
||||
def test_configurable_transport_delay_and_first_order_lag():
|
||||
plant = PlantSP(speed=10.0, actuator_delay=2 * DT_MDL, actuator_lag=0.2)
|
||||
|
||||
assert plant.planner.CP.longitudinalActuatorDelay == pytest.approx(2 * DT_MDL)
|
||||
delayed_commands = [plant._update_actuator(-1.0) for _ in range(3)]
|
||||
assert [command for command, _ in delayed_commands[:2]] == [0.0, 0.0]
|
||||
|
||||
expected_acceleration = -(1.0 - math.exp(-DT_MDL / 0.2))
|
||||
assert delayed_commands[2][0] == -1.0
|
||||
assert delayed_commands[2][1] == pytest.approx(expected_acceleration)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
("delay", "lag"),
|
||||
[(-0.1, 0.0), (float("nan"), 0.0), (float("inf"), 0.0), (None, -0.1), (None, float("nan")), (None, float("inf"))],
|
||||
)
|
||||
def test_invalid_actuator_dynamics(delay, lag):
|
||||
with pytest.raises(ValueError):
|
||||
PlantSP(actuator_delay=delay, actuator_lag=lag)
|
||||
@@ -1,4 +1,26 @@
|
||||
{
|
||||
"AccelPersonality": {
|
||||
"title": "Acceleration Profile",
|
||||
"description": "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly.",
|
||||
"options": [
|
||||
{
|
||||
"value": 0,
|
||||
"label": "Eco"
|
||||
},
|
||||
{
|
||||
"value": 1,
|
||||
"label": "Normal"
|
||||
},
|
||||
{
|
||||
"value": 2,
|
||||
"label": "Sport"
|
||||
}
|
||||
]
|
||||
},
|
||||
"AccelPersonalityEnabled": {
|
||||
"title": "Enable Accel Controller",
|
||||
"description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority."
|
||||
},
|
||||
"AccessToken": {
|
||||
"title": "AccessTokenIsNice",
|
||||
"description": ""
|
||||
|
||||
@@ -519,12 +519,6 @@
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "RoadEdgeLaneChangeEnabled",
|
||||
"widget": "toggle",
|
||||
"title": "Block Lane Change: Road Edge Detection",
|
||||
"description": "Blocks lane change when the model sees a road edge on the side you signal."
|
||||
},
|
||||
{
|
||||
"key": "AutoLaneChangeBsmDelay",
|
||||
"widget": "toggle",
|
||||
@@ -629,8 +623,8 @@
|
||||
{
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"widget": "toggle",
|
||||
"title": "Enable Acceleration Profiles",
|
||||
"description": "Enables acceleration profile selection for longitudinal control.",
|
||||
"title": "Enable Accel Controller",
|
||||
"description": "Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking and stopping authority.",
|
||||
"visibility": [
|
||||
{
|
||||
"type": "capability",
|
||||
@@ -650,10 +644,10 @@
|
||||
"key": "AccelPersonality",
|
||||
"widget": "multiple_button",
|
||||
"title": "Acceleration Profile",
|
||||
"description": "Controls how quickly sunnypilot accelerates while preserving braking and stop behavior.",
|
||||
"description": "Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts and recovers more quickly.",
|
||||
"options": [
|
||||
{
|
||||
"value": 2,
|
||||
"value": 0,
|
||||
"label": "Eco"
|
||||
},
|
||||
{
|
||||
@@ -661,7 +655,7 @@
|
||||
"label": "Normal"
|
||||
},
|
||||
{
|
||||
"value": 0,
|
||||
"value": 2,
|
||||
"label": "Sport"
|
||||
}
|
||||
],
|
||||
@@ -2059,6 +2053,22 @@
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "PlanplusControl",
|
||||
"widget": "option",
|
||||
"title": "Plan Plus Controls",
|
||||
"description": "Adjust planplus model recentering strength. The higher this number the more aggressively the model will recover to lane center; too high and it will ping-pong.",
|
||||
"min": 0.0,
|
||||
"max": 2.0,
|
||||
"step": 0.1,
|
||||
"enablement": [
|
||||
{
|
||||
"type": "param",
|
||||
"key": "ShowAdvancedControls",
|
||||
"equals": true
|
||||
}
|
||||
]
|
||||
}
|
||||
]
|
||||
},
|
||||
@@ -2226,6 +2236,50 @@
|
||||
"title": "Toyota / Lexus Settings",
|
||||
"description": "",
|
||||
"items": [
|
||||
{
|
||||
"key": "ToyotaAutoHold",
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Toyota: Auto Brake Hold FOR TSS2 HYBRID CARS",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "not_engaged"
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "ToyotaEnhancedBsm",
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Toyota: Prius TSS2 BSM and some tssp",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "not_engaged"
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "ToyotaTSS2Long",
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Toyota: custom longitudinal for TSS2",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "not_engaged"
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "ToyotaDriveMode",
|
||||
"widget": "toggle",
|
||||
"needs_onroad_cycle": true,
|
||||
"title": "Enable drive mode btn link",
|
||||
"enablement": [
|
||||
{
|
||||
"type": "not_engaged"
|
||||
}
|
||||
]
|
||||
},
|
||||
{
|
||||
"key": "ToyotaEnforceStockLongitudinal",
|
||||
"widget": "toggle",
|
||||
|
||||
@@ -43,19 +43,32 @@ sections:
|
||||
label: Relaxed
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelPersonalityEnabled
|
||||
widget: toggle
|
||||
title: Enable Accel Controller
|
||||
description: Begin slowing early and smoothly behind lead vehicles. Stock longitudinal control retains braking
|
||||
and stopping authority.
|
||||
visibility:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- key: AccelPersonality
|
||||
widget: multiple_button
|
||||
title: Acceleration Profile
|
||||
description: Controls how quickly sunnypilot accelerates while preserving braking and stop behavior.
|
||||
description: Eco slows earliest and recovers gently, Normal balances comfort and response, and Sport reacts
|
||||
and recovers more quickly.
|
||||
options:
|
||||
- value: 2
|
||||
- value: 0
|
||||
label: Eco
|
||||
- value: 1
|
||||
label: Normal
|
||||
- value: 0
|
||||
- value: 2
|
||||
label: Sport
|
||||
enablement:
|
||||
- $ref: '#/macros/longitudinal'
|
||||
- type: param
|
||||
key: AccelPersonalityEnabled
|
||||
equals: true
|
||||
- key: IntelligentCruiseButtonManagement
|
||||
widget: toggle
|
||||
title: Intelligent Cruise Button Management (ICBM) (Alpha)
|
||||
|
||||
@@ -272,6 +272,22 @@ class TestKnownPanels:
|
||||
nnlc_enable_keys = {r.get("key") for r in nnlc.get("enablement", []) if r.get("type") == "param"}
|
||||
assert "EnforceTorqueControl" in nnlc_enable_keys
|
||||
|
||||
def test_accel_controller_profile_mapping_and_enablement(self, schema):
|
||||
cruise = next(p for p in schema["panels"] if p["id"] == "cruise")
|
||||
items = {item["key"]: item for item in _iter_panel_items(cruise)}
|
||||
|
||||
assert items["AccelPersonalityEnabled"]["widget"] == "toggle"
|
||||
assert items["AccelPersonality"]["options"] == [
|
||||
{"value": 0, "label": "Eco"},
|
||||
{"value": 1, "label": "Normal"},
|
||||
{"value": 2, "label": "Sport"},
|
||||
]
|
||||
assert {
|
||||
"type": "param",
|
||||
"key": "AccelPersonalityEnabled",
|
||||
"equals": True,
|
||||
} in items["AccelPersonality"]["enablement"]
|
||||
|
||||
|
||||
class TestKnownVehicleSettings:
|
||||
def test_hyundai_has_longitudinal_tuning(self, schema):
|
||||
|
||||
Reference in New Issue
Block a user