From 86eac9f044910e00fd99d976b7ac359cf8ed1e88 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sat, 15 Aug 2026 00:58:05 -0700 Subject: [PATCH] feat(long): smooth lead following with acceleration profiles --- openpilot/cereal/custom.capnp | 30 + openpilot/common/params_keys.h | 4 + openpilot/common/tests/test_params.py | 4 + .../lib/longitudinal_mpc_lib/long_mpc.py | 11 +- .../controls/lib/longitudinal_planner.py | 13 +- .../test/longitudinal_maneuvers/plant.py | 12 +- .../selfdrive/ui/layouts/settings/toggles.py | 44 +- .../ui/mici/layouts/settings/toggles.py | 13 + openpilot/selfdrive/ui/mici/widgets/button.py | 7 +- .../controls/lib/accel_controller/__init__.py | 0 .../lib/accel_controller/accel_controller.py | 241 +++ .../lib/accel_controller/constants.py | 72 + .../controls/lib/accel_controller/helpers.py | 22 + .../controls/lib/accel_controller/lead.py | 134 ++ .../lib/accel_controller/lead_controller.py | 386 ++++ .../lib/accel_controller/tests/__init__.py | 0 .../tests/test_accel_controller.py | 789 ++++++++ .../tests/test_accel_controller_interfaces.py | 613 ++++++ .../tests/test_lead_controller.py | 341 ++++ .../lib/longitudinal_mpc_lib/__init__.py | 0 .../lib/longitudinal_mpc_lib/long_mpc.py | 46 + .../controls/lib/longitudinal_planner.py | 73 +- .../test_accel_controller_closed_loop.py | 1764 +++++++++++++++++ .../sunnypilot/selfdrive/test/__init__.py | 0 .../test/longitudinal_maneuvers/__init__.py | 0 .../test/longitudinal_maneuvers/plant.py | 395 ++++ .../longitudinal_maneuvers/tests/__init__.py | 0 .../tests/test_plant_sp.py | 157 ++ .../sunnypilot/sunnylink/settings_ui.json | 52 + .../settings_ui_src/pages/cruise.yaml | 26 + .../sunnylink/tests/test_settings_schema.py | 16 + 31 files changed, 5249 insertions(+), 16 deletions(-) create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/__init__.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/helpers.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead_controller.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/__init__.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_lead_controller.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/__init__.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py create mode 100644 openpilot/sunnypilot/selfdrive/test/__init__.py create mode 100644 openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/__init__.py create mode 100644 openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py create mode 100644 openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/__init__.py create mode 100644 openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index c20bf923be..f10aaf2c86 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -203,6 +203,7 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { aTarget @5 :Float32; events @6 :List(OnroadEventSP.Event); e2eAlerts @7 :E2eAlerts; + accelController @8 :AccelController; struct DynamicExperimentalControl { state @0 :DynamicExperimentalControlState; @@ -305,6 +306,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 { diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 7df58a0ba9..1b4b0579fc 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -232,6 +232,10 @@ inline static std::unordered_map 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"}}, diff --git a/openpilot/common/tests/test_params.py b/openpilot/common/tests/test_params.py index bbd411bc37..a1c683b0e7 100644 --- a/openpilot/common/tests/test_params.py +++ b/openpilot/common/tests/test_params.py @@ -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("LiveParametersV2") 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("LiveParametersV2") is None assert self.params.get("LiveParametersV2", return_default=True) is None diff --git a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index de1ba0c278..17ad0afa16 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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() @@ -266,7 +268,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) @@ -326,7 +329,7 @@ class LongitudinalMpc: # when the leads are no factor. v_lower = v_ego + (T_IDXS * CRUISE_MIN_ACCEL * 1.05) # TODO does this make sense when max_a is negative? - v_upper = v_ego + (T_IDXS * CRUISE_MAX_ACCEL * 1.05) + v_upper = v_ego + (T_IDXS * self.cruise_accel_max(CRUISE_MAX_ACCEL) * 1.05) v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, v_upper) cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) @@ -340,6 +343,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 @@ -359,6 +363,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]) for i in range(N+1): diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 7e3446a768..f65377bc74 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -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 @@ -110,15 +110,15 @@ 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 + is_e2e, v_cruise = LongitudinalPlannerSP.update_accel_controller(self, sm, v_cruise, prev_accel_constraint, accel_clip[1], reset_state) + 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.set_cur_state(self.v_desired_filter.x, self.mpc_accel_seed) self.mpc.update(sm['radarState'], v_cruise, personality=sm['selfdriveState'].personality) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) @@ -135,13 +135,13 @@ 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 + 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) 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: @@ -149,6 +149,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) diff --git a/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py b/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py index abbd6be1d6..56ef52c878 100755 --- a/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/openpilot/selfdrive/test/longitudinal_maneuvers/plant.py @@ -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: diff --git a/openpilot/selfdrive/ui/layouts/settings/toggles.py b/openpilot/selfdrive/ui/layouts/settings/toggles.py index ee76b7e4cc..f82d05e0dd 100644 --- a/openpilot/selfdrive/ui/layouts/settings/toggles.py +++ b/openpilot/selfdrive/ui/layouts/settings/toggles.py @@ -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) diff --git a/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py b/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py index 52bbb65e6a..61150f8f00 100644 --- a/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py +++ b/openpilot/selfdrive/ui/mici/layouts/settings/toggles.py @@ -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() diff --git a/openpilot/selfdrive/ui/mici/widgets/button.py b/openpilot/selfdrive/ui/mici/widgets/button.py index 3dd7ad9a8a..7e3f25bf09 100644 --- a/openpilot/selfdrive/ui/mici/widgets/button.py +++ b/openpilot/selfdrive/ui/mici/widgets/button.py @@ -383,13 +383,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): diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/__init__.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py new file mode 100644 index 0000000000..ba55455ef3 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/accel_controller.py @@ -0,0 +1,241 @@ +import math + +from opendbc.car.interfaces import ACCEL_MAX +from openpilot.cereal import custom +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot import get_sanitize_int_param +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( + LEAD_SAMPLE_FILTER_FRAMES, COMFORT_DECEL, LEAD_DROPOUT_COAST_TIME, MPC_DECEL_JERK_COST_MULTIPLIER, + MPC_DECEL_JERK_LONG_TREND_FRAMES, MPC_DECEL_JERK_LONG_TREND_RATE, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, + MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, MPC_DECEL_JERK_MAX_TARGET_REDUCTION, MPC_DECEL_TREND_FRAMES, + PARAM_READ_INTERVAL, LEAD_RELEASE_CONFIRM_TIME, RADAR_STALE_TIMEOUT, SPEED_DEADBAND, VEGO_NOISE_TOLERANCE, + 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 +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead_controller import LeadController +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource + +AccelControllerState = custom.LongitudinalPlanSP.AccelController.State + + +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_confirm_frames = max(LEAD_SAMPLE_FILTER_FRAMES, math.ceil(LEAD_RELEASE_CONFIRM_TIME / dt)) + self.dropout_frames = max(self.lead_confirm_frames, math.ceil(LEAD_DROPOUT_COAST_TIME / dt)) + self.dropout_release_frames = max(1, self.dropout_frames - self.lead_confirm_frames) + 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_long_samples: list[float] = [] + self._required_decel_lead = -1 + self._required_decel_lead_track_id = -1 + self._lead_trend_warmup = False + + self.lead_controller = LeadController() + self._stale_frames = 0 + self._cruise_accel_limited = False + + self.is_active = False + self.output_v_target = 0.0 + self.mpc_accel_max: tuple[float, ...] | None = None + self.cruise_accel_max: 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 + + @property + def launching(self) -> bool: + return self.lead_controller.launching + + @property + def departure_launching(self) -> bool: + return self.lead_controller.departure_launching + + 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 + + def reset(self) -> None: + self.lead_controller = LeadController() + self._stale_frames = 0 + self._jerk_smoothing_blocked = False + self._required_decel_samples.clear() + self._required_decel_long_samples.clear() + self._required_decel_lead = self._required_decel_lead_track_id = -1 + self._lead_trend_warmup = False + self._cruise_accel_limited = False + self.is_active = False + self.output_v_target = 0.0 + self.mpc_accel_max = None + self.cruise_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, radar_fresh: bool = True, + previous_mpc_source=None, planner_speed: float | None = None, planner_accel: float = 0.0, + previous_plan_accel: float = 0.0) -> None: + self.profile = sanitize_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 = 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) + + lead_plan = None + if enabled_context: + if radar_fresh: + self._stale_frames = 0 + lead_plan = calculate_lead_plan(radar_state, sanitized_v_ego, a_ego, self.delay, self.profile, follow_personality) + else: + self._stale_frames += 1 + new_acc_handoff = (self.lead_controller.target_speed is None and previous_mpc_source == LongitudinalPlanSource.e2e + and math.isfinite(previous_plan_accel)) + if new_acc_handoff: + lead_plan = LeadPlan() + + if lead_plan is not None: + self.lead_controller.update(lead_plan, base_speed, sanitized_v_ego, COMFORT_DECEL[self.profile], profile_max_accel, self.dt, + self.lead_confirm_frames, self.dropout_frames, planner_speed, planner_accel, + previous_mpc_source, previous_plan_accel) + stale_limit = self.dropout_frames if self.lead_controller.launching else self.radar_stale_frames + if not radar_fresh and self._stale_frames >= stale_limit and not self.lead_controller.stop_hold: + self.lead_controller.reset() + else: + self._stale_frames = 0 + + active = enabled_context and (radar_fresh or self.lead_controller.target_speed is not None) + self.is_active = active + if not active: + self.lead_controller.reset() + self._cruise_accel_limited = False + self.output_v_target = base_speed + self.mpc_accel_max = None + self.cruise_accel_max = None + self.state = AccelControllerState.inactive + self.selected_lead = -1 + self.selected_lead_track_id = -1 + self.required_decel = 0.0 + return + + lead_controller = self.lead_controller + self.output_v_target = lead_controller.target_speed if lead_controller.target_speed is not None else base_speed + self.selected_lead = lead_controller.selected_lead + self.selected_lead_track_id = lead_controller.selected_lead_track_id + self.required_decel = lead_controller.required_decel + + recovery_limit_active = lead_controller.lead_recovery and lead_controller.recovery_accel_limit is not None and not lead_controller.e2e_braking_handoff + lead_recovery_accel = lead_controller.lead_recovery and planner_accel >= 0.0 + dropout_coast = lead_controller.should_coast_on_dropout + profile_limit_active = (not lead_controller.stop_hold and (not lead_controller.has_lead or lead_recovery_accel + or lead_controller.departure_launching or lead_controller.leadless_departure) + and not lead_controller.e2e_braking_handoff) + if recovery_limit_active: + effective_accel_max = min(positive_accel_max, lead_controller.recovery_accel_limit) + elif profile_limit_active: + effective_accel_max = positive_accel_max + else: + effective_accel_max = math.inf + self.mpc_accel_max = (build_accel_ceiling(effective_accel_max, planner_accel) + if recovery_limit_active or profile_limit_active else None) + + lead_context = lead_controller.has_lead or math.isfinite(lead_controller.lead_speed_ceiling) + start_cruise_accel_limit = (lead_controller.target_speed is not None and not lead_controller.restricting and not lead_controller.releasing + and lead_controller.has_lead and previous_mpc_source == LongitudinalPlanSource.cruise) + keep_cruise_accel_limit = (self._cruise_accel_limited and lead_context and not lead_controller.restricting and not lead_controller.releasing + and not lead_controller.e2e_braking_handoff) + self._cruise_accel_limited = start_cruise_accel_limit or keep_cruise_accel_limit + if dropout_coast: + release_frame = max(0, lead_controller.lead_loss_frames - self.lead_confirm_frames) + self.cruise_accel_max = positive_accel_max * release_frame / self.dropout_release_frames + else: + self.cruise_accel_max = positive_accel_max if self._cruise_accel_limited else None + + if lead_controller.stop_hold: + self.state = AccelControllerState.stopHold + elif lead_controller.restricting: + self.state = AccelControllerState.restrict + elif lead_controller.releasing: + self.state = AccelControllerState.release + elif lead_controller.target_speed >= base_speed - SPEED_DEADBAND: + self.state = AccelControllerState.free + else: + self.state = AccelControllerState.hold + + 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() + self._required_decel_long_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_long_samples.append(self.required_decel) + if len(self._required_decel_long_samples) > MPC_DECEL_JERK_LONG_TREND_FRAMES: + self._required_decel_long_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) + long_history = self._required_decel_long_samples + long_history_ready = len(long_history) == MPC_DECEL_JERK_LONG_TREND_FRAMES + sustained_tightening = (long_history_ready + and (long_history[-1] - long_history[0]) / (self.dt * (len(long_history) - 1)) > MPC_DECEL_JERK_LONG_TREND_RATE) + 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 and not sustained_tightening) + 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 or sustained_tightening)): + 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, departure_authorized: bool = True) -> bool: + if not self.is_active: + return should_stop + if self.lead_controller.departure_launching and departure_authorized: + return False + return should_stop or self.lead_controller.stop_hold diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py new file mode 100644 index 0000000000..817032af01 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/constants.py @@ -0,0 +1,72 @@ +import math + +import numpy as np + +from openpilot.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.56, 1.30, 0.72, 0.32, 0.24], + AccelProfile.normal: [1.58, 1.51, 0.98, 0.53, 0.35], + AccelProfile.sport: [2.00, 1.91, 1.16, 0.73, 0.47], +} + +BRAKING_ACCEL_THRESHOLD = -0.11 + +LEAD_SAMPLE_FILTER_FRAMES = 5 +LEAD_RELEASE_CONFIRM_TIME = 0.50 +LEAD_DROPOUT_COAST_TIME = 1.50 + +SPEED_DEADBAND = 0.15 + +TARGET_RELEASE_SLEW = 8.75 +LAUNCH_TARGET_HEADROOM = 3.0 +LAUNCH_END_SPEED = 3.0 + +LEAD_RECOVERY_HEADROOM = 1.25 +LEAD_RECOVERY_ACCEL_SLEW = 0.25 +LEAD_RECOVERY_DECEL_RATE = 0.50 + +STOP_HOLD_EGO_SPEED = 0.30 +STOP_HOLD_SPEED_FLOOR = 0.15 +STOP_HOLD_EXIT_FRAMES = 4 +STOP_HOLD_CREEP_DISTANCE = 0.30 +STOP_HOLD_MAX_LEAD_DISTANCE = 30.0 +DISTANCE_JUMP_CONFIRM_FRAMES = 3 + +STOP_GAP_RESERVE = 0.75 + +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 +ACCEL_LIMIT_HORIZON_JERK = 1.0 + +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_LONG_TREND_FRAMES = 6 +MPC_DECEL_JERK_LONG_TREND_RATE = 0.02 +MPC_DECEL_JERK_MAX_TARGET_REDUCTION = 9.0 +MPC_DECEL_TREND_FRAMES = 4 + + +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)])) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/helpers.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/helpers.py new file mode 100644 index 0000000000..8339bf1826 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/helpers.py @@ -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) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py new file mode 100644 index 0000000000..c323b9275b --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead.py @@ -0,0 +1,134 @@ +""" +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 openpilot.cereal import log +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import ( + LongitudinalMpc, 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_HOLD_SPEED_FLOOR, sanitize_profile, +) + + +class LeadPlan(NamedTuple): + speed_ceiling: 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: int = -1 + departure_lead_track_id: int = -1 + departure_lead_speed: float = math.inf + departure_lead_raw_speed: float = math.inf + departure_lead_distance: float = math.inf + departure_lead_separation: float = math.inf + departure_speed_ceiling: float = math.inf + closing_speed: float = 0.0 + required_decel: float = 0.0 + has_nearly_stopped_lead: bool = False + lead_status: bool = False + + +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, float] | None: + if not lead.present: + 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 + raw_v_lead = float(getattr(lead, "vLead", v_lead)) + if not math.isfinite(raw_v_lead): + raw_v_lead = 0.0 + return d_rel, max(v_lead, 0.0), max(raw_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.present 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, LeadPlan]] = [] + + for lead_index, lead in enumerate(leads): + values = _lead_values(lead) + if values is None: + continue + + d_rel, v_lead, raw_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) + usable_gap = max(safety_gap - STOP_GAP_RESERVE, 0.0) + speed_ceiling = v_lead_delay + math.sqrt(2.0 * comfort_decel * usable_gap) + departure_speed_ceiling = 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, speed_ceiling, departure_speed_ceiling, 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 + candidate = LeadPlan( + speed_ceiling=speed_ceiling, selected_lead=lead_index, selected_lead_track_id=track_id, + selected_lead_speed=v_lead_delay, selected_lead_accel=a_lead, departure_lead=lead_index, + departure_lead_track_id=track_id, departure_lead_speed=v_lead_delay, departure_lead_raw_speed=raw_v_lead, + departure_lead_distance=d_rel, departure_lead_separation=separation, + departure_speed_ceiling=departure_speed_ceiling, closing_speed=closing_speed, required_decel=required_decel, + has_nearly_stopped_lead=v_lead_delay < STOP_HOLD_SPEED_FLOOR, lead_status=lead_status, + ) + candidates.append(candidate) + departure_candidates.append((departure_distance, candidate)) + + if not candidates: + return LeadPlan(lead_status=lead_status) + + selected = min(candidates, key=lambda candidate: candidate.speed_ceiling) + departure = min(departure_candidates, key=lambda candidate: candidate[0])[1] + return selected._replace( + departure_lead=departure.selected_lead, departure_lead_track_id=departure.selected_lead_track_id, + departure_lead_speed=departure.selected_lead_speed, departure_lead_raw_speed=departure.departure_lead_raw_speed, + departure_lead_distance=departure.departure_lead_distance, + departure_lead_separation=departure.departure_lead_separation, + departure_speed_ceiling=departure.departure_speed_ceiling, + has_nearly_stopped_lead=departure.has_nearly_stopped_lead, + ) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead_controller.py new file mode 100644 index 0000000000..e08a81ae74 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/lead_controller.py @@ -0,0 +1,386 @@ +""" +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 + +import numpy as np + +from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalPlanSource +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( + BRAKING_ACCEL_THRESHOLD, LEAD_SAMPLE_FILTER_FRAMES, DISTANCE_JUMP_CONFIRM_FRAMES, LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, + LEAD_RECOVERY_ACCEL_SLEW, LEAD_RECOVERY_HEADROOM, LEAD_RECOVERY_DECEL_RATE, SPEED_DEADBAND, STOP_HOLD_CREEP_DISTANCE, + STOP_HOLD_EGO_SPEED, STOP_HOLD_EXIT_FRAMES, STOP_HOLD_MAX_LEAD_DISTANCE, STOP_HOLD_SPEED_FLOOR, TARGET_RELEASE_SLEW, +) +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan + + +def _median(samples: list[float]) -> float: + return sorted(samples)[len(samples) // 2] + + +def _slew(current: float, target: float, rate: float, dt: float) -> float: + return float(np.clip(target, current - rate * dt, current + rate * dt)) + + +def _max_distance_step(lead_speed: float, dt: float) -> float: + return max(STOP_HOLD_CREEP_DISTANCE / 2.0, 3.0 * max(lead_speed, 0.0) * dt) + + +def _same_lead(first: int, first_track_id: int, second: int, second_track_id: int) -> bool: + if first < 0 or second < 0: + return False + if first_track_id >= 0 or second_track_id >= 0: + return first_track_id >= 0 and first_track_id == second_track_id + return first == second + + +class LeadController: + def __init__(self) -> None: + self.lead_speed_samples = [math.inf] * LEAD_SAMPLE_FILTER_FRAMES + self.lead_accel_samples = [0.0] * LEAD_SAMPLE_FILTER_FRAMES + + self.lead_speed_ceiling = math.inf + self.release_confirm_frames = 0 + self.lead_loss_frames = 0 + self._dropout_was_restricting = False + self._dropout_was_braking = False + + self.target_speed: float | None = None + self.e2e_braking_handoff = False + self.lead_recovery = False + self.recovery_accel_limit: float | None = None + + self.stop_hold = False + self.launching = False + self.departure_launching = False + self.leadless_departure = False + self.held_lead = -1 + self.held_lead_track_id = -1 + self.held_lead_trusted = False + self.departure_confirm_frames = 0 + self.no_departure_lead_frames = 0 + self.departure_distance_ref: float | None = None + self.last_departure_distance: float | None = None + self.pending_distance_jump: float | None = None + self.distance_jump_frames = 0 + self.braking_for_lead = False + self._lead_frames = 0 + + self.restricting = False + self.releasing = False + self.has_lead = False + self.required_decel = 0.0 + self.selected_lead = -1 + self.selected_lead_track_id = -1 + self.raw_speed_ceiling = math.inf + + @property + def filtered_lead_speed(self) -> float: + return _median(self.lead_speed_samples) + + @property + def filtered_lead_accel(self) -> float: + return _median(self.lead_accel_samples) + + @property + def should_coast_on_dropout(self) -> bool: + return not self.has_lead and self._dropout_was_restricting and self._dropout_was_braking and math.isfinite(self.lead_speed_ceiling) + + def reset(self) -> None: + self.__init__() + + def _update_lead_bookkeeping(self, lead_plan: LeadPlan, was_restricting: bool) -> None: + self.has_lead = lead_plan.selected_lead >= 0 + self.raw_speed_ceiling = lead_plan.speed_ceiling if self.has_lead else math.inf + if self.has_lead: + self.lead_loss_frames = 0 + self._dropout_was_braking = False + else: + if self.lead_loss_frames == 0: + self._dropout_was_restricting = was_restricting + self.lead_loss_frames += 1 + self.selected_lead = lead_plan.selected_lead + self.selected_lead_track_id = lead_plan.selected_lead_track_id if self.has_lead else -1 + + def _update_speed_sample(self, lead_plan: LeadPlan) -> None: + if not self.has_lead: + self.lead_speed_samples.append(math.inf) + self.lead_speed_samples.pop(0) + self.lead_accel_samples.append(0.0) + self.lead_accel_samples.pop(0) + return + if self.raw_speed_ceiling <= self.lead_speed_ceiling + 1e-9: + self.lead_speed_samples.append(lead_plan.selected_lead_speed) + self.lead_speed_samples.pop(0) + self.lead_accel_samples.append(lead_plan.selected_lead_accel) + self.lead_accel_samples.pop(0) + + def _update_speed_ceiling(self, lead_confirm_frames: int, dropout_frames: int) -> None: + candidate = self.raw_speed_ceiling + if candidate <= self.lead_speed_ceiling: + self.lead_speed_ceiling = candidate + self.release_confirm_frames = 0 + return + + if not self.has_lead: + hold_frames = dropout_frames if self._dropout_was_restricting else lead_confirm_frames + if self.lead_loss_frames <= hold_frames: + return + self.lead_speed_ceiling = candidate + self.release_confirm_frames = 0 + return + + if candidate >= self.lead_speed_ceiling + SPEED_DEADBAND: + self.release_confirm_frames += 1 + else: + self.release_confirm_frames = 0 + if self.release_confirm_frames > lead_confirm_frames: + self.lead_speed_ceiling = candidate + self.release_confirm_frames = 0 + + def _guarded_distance(self, raw: float, lead_speed: float, dt: float) -> float: + if self.last_departure_distance is not None: + delta = raw - self.last_departure_distance + if abs(delta) > _max_distance_step(lead_speed, dt): + consistent = self.pending_distance_jump is not None and delta * self.pending_distance_jump > 0.0 + self.distance_jump_frames = self.distance_jump_frames + 1 if consistent else 1 + self.pending_distance_jump = delta + if self.distance_jump_frames >= DISTANCE_JUMP_CONFIRM_FRAMES: + self.pending_distance_jump = None + self.distance_jump_frames = 0 + self.departure_confirm_frames = 0 + if abs(delta) >= STOP_HOLD_CREEP_DISTANCE: + self.held_lead_trusted = False + else: + raw = self.last_departure_distance + else: + self.pending_distance_jump = None + self.distance_jump_frames = 0 + self.last_departure_distance = raw + return raw + + def _reset_distance_guard(self) -> None: + self.last_departure_distance = self.pending_distance_jump = None + self.distance_jump_frames = 0 + + def _reset_departure_confirmation(self) -> None: + self.departure_confirm_frames = 0 + self.departure_distance_ref = None + self.no_departure_lead_frames = 0 + + def _set_held_lead(self, lead_plan: LeadPlan, replacement: bool = False) -> None: + self.held_lead = lead_plan.departure_lead + self.held_lead_track_id = lead_plan.departure_lead_track_id + self.held_lead_trusted = not replacement and self.held_lead_track_id >= 0 + self._reset_distance_guard() + if self.held_lead >= 0: + self.last_departure_distance = lead_plan.departure_lead_distance + self._reset_departure_confirmation() + + def _update_stop_hold(self, lead_plan: LeadPlan, v_ego: float, base_speed: float, dt: float, lead_confirm_frames: int) -> bool: + if self.stop_hold: + has_departure_lead = _same_lead(self.held_lead, self.held_lead_track_id, + lead_plan.departure_lead, lead_plan.departure_lead_track_id) + if lead_plan.departure_lead >= 0 and not has_departure_lead: + continuous_vision_lead = (self.held_lead_track_id < 0 and lead_plan.departure_lead_track_id < 0 + and self.last_departure_distance is not None + and abs(lead_plan.departure_lead_distance - self.last_departure_distance) + <= _max_distance_step(lead_plan.departure_lead_speed, dt)) + if continuous_vision_lead: + self.held_lead = lead_plan.departure_lead + self.held_lead_trusted = False + else: + self._set_held_lead(lead_plan, replacement=True) + has_departure_lead = True + if has_departure_lead or lead_plan.lead_status: + self.no_departure_lead_frames = 0 + else: + self.no_departure_lead_frames += 1 + lead_speed = lead_plan.departure_lead_speed if has_departure_lead else 0.0 + raw_lead_speed = lead_plan.departure_lead_raw_speed if has_departure_lead else 0.0 + evidence = has_departure_lead and min(lead_speed, raw_lead_speed) > STOP_HOLD_SPEED_FLOOR + distance = None + if has_departure_lead: + distance = self._guarded_distance(lead_plan.departure_lead_distance, lead_speed, dt) + + if evidence: + if self.departure_confirm_frames == 0: + self.departure_distance_ref = distance + self.departure_confirm_frames += 1 + else: + self.departure_confirm_frames = 0 + self.departure_distance_ref = None + if not has_departure_lead: + self.held_lead_trusted = False + self._reset_distance_guard() + + growth = 0.0 + if self.departure_confirm_frames > 0 and self.departure_distance_ref is not None and distance is not None: + growth = distance - self.departure_distance_ref + + dwell_ready = self.departure_confirm_frames >= STOP_HOLD_EXIT_FRAMES + departing_with_lead = has_departure_lead and dwell_ready and growth + 1e-9 >= STOP_HOLD_CREEP_DISTANCE + departing_no_lead = (not lead_plan.lead_status and self.no_departure_lead_frames >= lead_confirm_frames + and base_speed > STOP_HOLD_SPEED_FLOOR) + + if departing_with_lead or departing_no_lead: + trusted_departure = departing_with_lead and self.held_lead_trusted + self.stop_hold = False + self.launching = True + self.departure_launching = trusted_departure + self.leadless_departure = not trusted_departure + self.held_lead = self.held_lead_track_id = -1 + self.held_lead_trusted = False + self._reset_departure_confirmation() + self.target_speed = min(v_ego, base_speed) + else: + self.target_speed = 0.0 + return self.stop_hold + + departure_separation = lead_plan.departure_lead_separation if lead_plan.departure_lead >= 0 else math.inf + stopped_lead_hold = lead_plan.has_nearly_stopped_lead and ( + lead_plan.departure_speed_ceiling < STOP_HOLD_SPEED_FLOOR + or (self.braking_for_lead and departure_separation <= STOP_HOLD_MAX_LEAD_DISTANCE) + ) + retained_stop_hold = not self.has_lead and math.isfinite(self.lead_speed_ceiling) and self.lead_speed_ceiling < STOP_HOLD_SPEED_FLOOR + if not self.launching and v_ego < STOP_HOLD_EGO_SPEED and ( + stopped_lead_hold or retained_stop_hold + ): + self.stop_hold = True + self._set_held_lead(lead_plan) + self.launching = self.departure_launching = self.leadless_departure = False + self.lead_recovery = False + self.recovery_accel_limit = None + self.target_speed = 0.0 + return True + + return False + + def _update_launch(self, lead_plan: LeadPlan, base_speed: float, v_ego: float, dt: float, lead_confirm_frames: int) -> None: + if not self.launching: + return + if v_ego >= LAUNCH_END_SPEED: + self.launching = self.departure_launching = self.leadless_departure = False + return + invalid_lead = lead_plan.lead_status and not self.has_lead + renewed_stop = self.has_lead and lead_plan.has_nearly_stopped_lead + if invalid_lead or renewed_stop: + self.launching = self.departure_launching = self.leadless_departure = False + if v_ego < STOP_HOLD_EGO_SPEED: + self.stop_hold = True + self._set_held_lead(lead_plan) + self.target_speed = 0.0 + return + self.releasing = True + if self.departure_launching: + self.target_speed = base_speed + elif self.leadless_departure: + self.target_speed = min(base_speed, max(self.target_speed or 0.0, v_ego) + TARGET_RELEASE_SLEW * dt) + elif not self.has_lead and self.lead_loss_frames >= lead_confirm_frames: + self.target_speed = min(base_speed, self.lead_speed_ceiling) + else: + launch_target = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM) + self.target_speed = min(base_speed, max(self.target_speed or 0.0, launch_target) + TARGET_RELEASE_SLEW * dt) + + def _update_recovery(self, ceiling: float, base_speed: float, v_ego: float, profile_max_accel: float, dt: float) -> None: + if math.isfinite(self.filtered_lead_speed): + recovery_speed = min(base_speed, self.filtered_lead_speed + LEAD_RECOVERY_HEADROOM) + desired_accel_limit = float(np.clip(recovery_speed - v_ego, 0.0, profile_max_accel)) + else: + desired_accel_limit = 0.0 + if self.filtered_lead_accel < BRAKING_ACCEL_THRESHOLD: + # Avoid stacking the recovery slew on top of a braking lead. + desired_accel_limit = profile_max_accel + if self.recovery_accel_limit is None: + self.recovery_accel_limit = profile_max_accel + self.recovery_accel_limit = _slew(self.recovery_accel_limit, desired_accel_limit, LEAD_RECOVERY_ACCEL_SLEW, dt) + + if ceiling <= self.target_speed - SPEED_DEADBAND: + self.target_speed = max(ceiling, self.target_speed - LEAD_RECOVERY_DECEL_RATE * dt) + self.restricting = True + elif ceiling >= self.target_speed + SPEED_DEADBAND: + self.target_speed = min(ceiling, self.target_speed + profile_max_accel * dt) + self.releasing = True + + def _update_target_law(self, lead_plan: LeadPlan, base_speed: float, v_ego: float, comfort_decel: float, + profile_max_accel: float, dt: float, planner_speed: float, + dropout_frames: int, was_restricting: bool) -> None: + ceiling = min(base_speed, self.lead_speed_ceiling) + new_recovery = self.has_lead and lead_plan.closing_speed <= 0.0 + still_within_dropout = not self.has_lead and self.lead_loss_frames <= dropout_frames + self.lead_recovery = new_recovery or (self.lead_recovery and (self.has_lead or still_within_dropout)) + if self.lead_recovery: + self._update_recovery(ceiling, base_speed, v_ego, profile_max_accel, dt) + return + + self.recovery_accel_limit = None + synced_to_planner = ceiling < self.target_speed and planner_speed < self.target_speed + if synced_to_planner: + self.target_speed = max(planner_speed, self.target_speed - comfort_decel * dt) + + if ceiling <= self.target_speed - SPEED_DEADBAND or (was_restricting and ceiling < self.target_speed): + if not synced_to_planner: + self.target_speed = max(ceiling, self.target_speed - comfort_decel * dt) + self.restricting = True + elif ceiling >= self.target_speed + SPEED_DEADBAND: + self.target_speed = min(ceiling, self.target_speed + TARGET_RELEASE_SLEW * dt) + self.releasing = True + + def update(self, lead_plan: LeadPlan, base_speed: float, v_ego: float, comfort_decel: float, profile_max_accel: float, + dt: float, lead_confirm_frames: int, dropout_frames: int, planner_speed: float, + planner_accel: float, previous_mpc_source, previous_plan_accel: float) -> float: + was_restricting = self.restricting + was_braking_for_lead = self.braking_for_lead + previous_lead = self.selected_lead + previous_track_id = self.selected_lead_track_id + holding_below_cruise = (not self.lead_recovery and self.target_speed is not None and math.isfinite(self.lead_speed_ceiling) + and self.lead_speed_ceiling < base_speed - SPEED_DEADBAND + and self.lead_speed_ceiling - v_ego < LEAD_RECOVERY_HEADROOM) + self.restricting = self.releasing = False + self._update_lead_bookkeeping(lead_plan, was_restricting or holding_below_cruise) + lead_changed = not _same_lead(previous_lead, previous_track_id, self.selected_lead, self.selected_lead_track_id) + if not self.has_lead or lead_changed: + self._lead_frames = 0 + if self.braking_for_lead and lead_changed: + self.braking_for_lead = False + if not self.has_lead and self.lead_loss_frames == 1: + self._dropout_was_braking = was_braking_for_lead and planner_accel <= BRAKING_ACCEL_THRESHOLD + self._update_speed_ceiling(lead_confirm_frames, dropout_frames) + self._update_speed_sample(lead_plan) + self.required_decel = lead_plan.required_decel + + self._lead_frames += int(self.has_lead) + if (self._lead_frames >= lead_confirm_frames and math.isfinite(self.lead_speed_ceiling) + and self.has_lead and planner_accel <= BRAKING_ACCEL_THRESHOLD): + self.braking_for_lead = True + elif not self.has_lead and self.lead_loss_frames >= lead_confirm_frames: + self.braking_for_lead = False + + if self.target_speed is None: + self.target_speed = min(base_speed, v_ego) + e2e_handoff = previous_mpc_source == LongitudinalPlanSource.e2e + self.e2e_braking_handoff = e2e_handoff and math.isfinite(previous_plan_accel) and previous_plan_accel <= BRAKING_ACCEL_THRESHOLD + stop_hold_reason = lead_plan.has_nearly_stopped_lead or (math.isfinite(self.lead_speed_ceiling) and self.lead_speed_ceiling < STOP_HOLD_SPEED_FLOOR) + if v_ego < STOP_HOLD_EGO_SPEED and not stop_hold_reason: + self.target_speed = min(base_speed, v_ego + LAUNCH_TARGET_HEADROOM) + self.launching = True + self.departure_launching = False + elif self.e2e_braking_handoff and planner_accel > BRAKING_ACCEL_THRESHOLD: + self.e2e_braking_handoff = False + + self.target_speed = min(self.target_speed, base_speed) + + if self._update_stop_hold(lead_plan, v_ego, base_speed, dt, lead_confirm_frames): + return self.target_speed + + self._update_launch(lead_plan, base_speed, v_ego, dt, lead_confirm_frames) + if self.launching or self.stop_hold: + return self.target_speed + + self._update_target_law(lead_plan, base_speed, v_ego, comfort_decel, profile_max_accel, dt, planner_speed, + dropout_frames, was_restricting) + return self.target_speed diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/__init__.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py new file mode 100644 index 0000000000..addec9eaac --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller.py @@ -0,0 +1,789 @@ +import math +from types import SimpleNamespace + +import numpy as np +import pytest + +from openpilot.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, LEAD_SAMPLE_FILTER_FRAMES, COMFORT_DECEL, + LAUNCH_END_SPEED, LAUNCH_TARGET_HEADROOM, LEAD_RECOVERY_ACCEL_SLEW, LEAD_RECOVERY_HEADROOM, MPC_DECEL_JERK_COST_MULTIPLIER, + STOP_GAP_RESERVE, STOP_HOLD_EXIT_FRAMES, TARGET_RELEASE_SLEW, AccelProfile, profile_accel_max, sanitize_profile, +) +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(present=status, dRel=d_rel, vLead=v_lead_k, 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, + } + args.update(overrides) + controller.profile = args.pop("profile") + controller.enabled = args.pop("enabled") + controller.update(radar_state or make_radar(), **args) + lead_controller = controller.lead_controller + return SimpleNamespace( + target_speed=controller.output_v_target, active=controller.is_active, launching=lead_controller.launching, + departure_launching=lead_controller.departure_launching, mpc_accel_max=controller.mpc_accel_max, + cruise_accel_max=controller.cruise_accel_max, state=controller.state, selected_lead=controller.selected_lead, + required_decel=controller.required_decel, stop_hold=lead_controller.stop_hold, lead_recovery=lead_controller.lead_recovery, + recovery_accel_limit=lead_controller.recovery_accel_limit, lead_speed_ceiling=lead_controller.lead_speed_ceiling, restricting=lead_controller.restricting, + releasing=lead_controller.releasing, + ) + + +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.0, frames=6, track_id=-1): + stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=track_id)) + result = None + for _ in range(frames): + result = update(controller, stopped, base_speed=base_speed, v_ego=v_ego) + assert result.stop_hold + return result + + +class TestProfiles: + @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 profile_accel_max(profile, speed) == expected + + limits = [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 = (profile_accel_max(profile, speed) for profile in ACCEL_PROFILES) + assert eco < normal < sport + + def test_invalid_profile_defaults_to_normal(self): + assert sanitize_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_confirm_frames)] + result = results[-1] + assert profile_accel_max(AccelProfile.sport, 10.0) > 0.30 + 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_confirm_frames)][-1] + eco = update(controller, v_ego=10.0, profile=AccelProfile.eco, stock_accel_max=1.20) + + assert effective_accel_max(sport) == pytest.approx(profile_accel_max(AccelProfile.sport, 10.0)) + assert effective_accel_max(eco) == pytest.approx(profile_accel_max(AccelProfile.eco, 10.0)) + + def test_stock_limit_reduction_applies_immediately(self): + controller = make_controller() + for _ in range(controller.lead_confirm_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_confirm_frames + 10): + clean = update(clean_controller, v_ego=10.0, stock_accel_max=1.5) + 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_lead_recovery_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(controller.lead_confirm_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.lead_controller.lead_recovery + + 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 profile_accel_max(AccelProfile.sport, 0.0) == ACCEL_MAX + assert result.mpc_accel_max is None + + +class TestBuildAccelCeiling: + @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) + + +class TestMpcCeilingIntegration: + 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.lead_controller.target_speed is None + + def test_closing_on_a_lead_has_no_ceiling_regardless_of_planner_accel_sign(self): + # Leave the lead MPC unconstrained while ego is closing. + controller = make_controller() + radar = restrictive_radar() + for planner_accel in (-0.2, 0.2, -0.2): + result = update(controller, radar, v_ego=10.0, planner_accel=planner_accel) + assert result.mpc_accel_max is None + assert not controller.lead_controller.lead_recovery + + bypassed = update(controller, radar, planner_accel=-0.2, acc_selected=False) + assert not bypassed.active and bypassed.mpc_accel_max is None + + def test_eco_cruise_limit_remains_active_while_closing_on_a_lead(self): + controller = make_controller() + radar = make_radar(make_lead(status=True, d_rel=80.0, v_lead_k=19.25)) + result = update(controller, radar, base_speed=20.0, v_ego=20.0, profile=AccelProfile.eco, planner_accel=0.16, + previous_mpc_source=LongitudinalPlanSource.cruise) + + assert result.state == AccelControllerState.free + assert result.mpc_accel_max is None + assert result.cruise_accel_max == pytest.approx(profile_accel_max(AccelProfile.eco, 20.0)) + + def test_lead_cruise_limit_does_not_follow_mpc_source_or_accel_sign(self): + controller = make_controller() + expected = profile_accel_max(AccelProfile.eco, 20.0) + inputs = ( + (LongitudinalPlanSource.cruise, 0.02, 19.9, 100), + (LongitudinalPlanSource.lead0, -0.02, 20.1, -1), + (LongitudinalPlanSource.cruise, -0.10, 19.9, 101), + (LongitudinalPlanSource.lead0, -0.12, 20.1, -1), + (LongitudinalPlanSource.lead1, 0.02, 20.1, -1), + ) + + for source, planner_accel, lead_speed, track_id in inputs: + radar = make_radar(make_lead(status=True, d_rel=150.0, v_lead_k=lead_speed, radar_track_id=track_id)) + result = update(controller, radar, base_speed=20.0, v_ego=20.0, profile=AccelProfile.eco, + planner_accel=planner_accel, previous_mpc_source=source) + assert result.cruise_accel_max == pytest.approx(expected) + + def test_profile_ceiling_stays_continuous_while_a_lead_begins_pulling_away(self): + controller = make_controller() + for _ in range(controller.lead_confirm_frames): + 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 result.lead_recovery + assert result.mpc_accel_max is not None + profile_ceiling = profile_accel_max(AccelProfile.normal, 10.0) + assert profile_ceiling - LEAD_RECOVERY_ACCEL_SLEW * DT_MDL - 1e-9 <= effective_accel_max(result) <= profile_ceiling + 1e-9 + + +class TestLead: + def test_speed_ceiling_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) + usable_gap = max(safety_gap - STOP_GAP_RESERVE, 0.0) + expected = v_lead + math.sqrt(2.0 * COMFORT_DECEL[AccelProfile.normal] * usable_gap) + + assert result.speed_ceiling == pytest.approx(expected) + + def test_profile_order_controls_approach_timing(self): + radar = make_radar(make_lead(status=True, d_rel=50.0, v_lead_k=8.0)) + ceilings = [get_lead_plan(make_controller(), radar, 10.0, 0.0, profile).speed_ceiling for profile in ACCEL_PROFILES] + assert ceilings[0] < ceilings[1] < ceilings[2] + + @pytest.mark.parametrize("v_lead_k", (0.0, 8.0), ids=("stopped", "moving")) + def test_reserve_is_flat_not_speed_or_decel_scaled(self, v_lead_k): + lead = get_lead_plan(make_controller(), make_radar(make_lead(status=True, d_rel=60.0, v_lead_k=v_lead_k)), + 5.0, 0.0, AccelProfile.normal) + comfort_decel = COMFORT_DECEL[AccelProfile.normal] + safety_gap = (lead.departure_speed_ceiling - lead.departure_lead_speed) ** 2 / (2.0 * comfort_decel) + usable_gap = (lead.speed_ceiling - lead.selected_lead_speed) ** 2 / (2.0 * comfort_decel) + assert safety_gap - usable_gap == pytest.approx(STOP_GAP_RESERVE) + assert lead.departure_speed_ceiling > lead.speed_ceiling + + 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 + + def test_departure_lead_prefers_nearer_lead_over_speed_governing_lead(self): + radar = 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)) + result = get_lead_plan(make_controller(), radar, 0.0, 0.0, AccelProfile.normal) + assert result.selected_lead == 1 + assert result.departure_lead == 0 + + @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.speed_ceiling) + + @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.speed_ceiling) + + 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 TestTargetAndSpeedCeiling: + def test_lead_recovery_accel_limit_ignores_a_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(controller.lead_confirm_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_restriction_target_speed_is_rate_limited(self): + controller = make_controller() + targets = [update(controller, restrictive_radar()).target_speed for _ in range(15)] + max_step = COMFORT_DECEL[AccelProfile.normal] * DT_MDL + + steps = -np.diff(targets[1:]) + assert np.all(steps <= max_step + 1e-9) + assert controller.state == AccelControllerState.restrict + assert targets[-1] < targets[1] + + @pytest.mark.parametrize("previous_mpc_source", (None, LongitudinalPlanSource.cruise, LongitudinalPlanSource.lead0)) + def test_target_speed_syncs_down_to_planner_speed_regardless_of_previous_mpc_source(self, previous_mpc_source): + controller = make_controller() + for _ in range(15): + restricted = update(controller, restrictive_radar()) + planner_speed = restricted.target_speed - 2.0 + + synced = update(controller, restrictive_radar(), previous_mpc_source=previous_mpc_source, planner_speed=planner_speed, + planner_accel=-0.2) + assert synced.target_speed == pytest.approx(restricted.target_speed - COMFORT_DECEL[AccelProfile.normal] * DT_MDL) + + def test_short_dropout_holds_then_releases_at_a_bounded_rate(self): + controller = make_controller() + for _ in range(15): + restricted = update(controller, restrictive_radar()) + + held = [update(controller) for _ in range(controller.dropout_frames - 1)] + assert all(result.target_speed <= restricted.target_speed + 1e-9 for result in held) + + released = [update(controller) for _ in range(60)] + targets = [restricted.target_speed, *(result.target_speed for result in released)] + assert np.max(np.diff(targets)) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9 + assert released[-1].target_speed >= 25.0 - 0.15 - 1e-9 + assert released[-1].state == AccelControllerState.free + + def test_restricting_lead_dropout_coasts_before_release(self): + controller = make_controller() + for _ in range(15): + update(controller, restrictive_radar(), planner_accel=-0.5) + + coast = update(controller, planner_accel=-0.5) + + assert coast.cruise_accel_max == 0.0 + assert coast.mpc_accel_max is not None + assert effective_accel_max(coast) == pytest.approx(profile_accel_max(AccelProfile.normal, 10.0)) + + ceilings = [coast.cruise_accel_max] + ceilings.extend(update(controller, planner_accel=-0.5).cruise_accel_max for _ in range(controller.dropout_frames - 1)) + assert ceilings == sorted(ceilings) + assert ceilings[-1] == pytest.approx(profile_accel_max(AccelProfile.normal, 10.0)) + + def test_should_coast_on_dropout_lifecycle(self): + controller = make_controller() + for _ in range(15): + update(controller, restrictive_radar(), planner_accel=-0.5) + + update(controller, planner_accel=-0.5) + assert controller.lead_controller.should_coast_on_dropout + + update(controller, restrictive_radar(), planner_accel=0.2) + assert not controller.lead_controller.should_coast_on_dropout + update(controller, planner_accel=0.2) + assert not controller.lead_controller.should_coast_on_dropout + + controller.reset() + assert not controller.lead_controller.should_coast_on_dropout + + def test_e2e_braking_handoff_clears_when_braking_ends(self): + controller = make_controller() + update(controller, previous_mpc_source=LongitudinalPlanSource.e2e, previous_plan_accel=-1.0, planner_accel=-0.5) + assert controller.lead_controller.e2e_braking_handoff + + still_braking = update(controller, planner_accel=-0.12) + assert controller.lead_controller.e2e_braking_handoff + assert still_braking.mpc_accel_max is None + + coasting = update(controller, planner_accel=-0.10) + assert not controller.lead_controller.e2e_braking_handoff + assert coasting.mpc_accel_max is not None + + +class TestMatchedLead: + def test_recovery_accel_limit_unthrottled_when_ego_well_below_lead_speed(self): + controller = make_controller() + radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0)) + for _ in range(20): + update(controller, radar, v_ego=10.0, planner_accel=-0.2) + result = update(controller, radar, v_ego=3.0, planner_accel=-0.2) + + assert result.lead_recovery + assert result.recovery_accel_limit == pytest.approx(profile_accel_max(AccelProfile.normal, 3.0)) + assert effective_accel_max(result) == pytest.approx(profile_accel_max(AccelProfile.normal, 3.0)) + + def test_recovery_accel_limit_throttles_toward_recovery_headroom_when_near_lead_speed(self): + controller = make_controller() + slow_radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=3.0)) + for _ in range(20): + update(controller, slow_radar, v_ego=5.0, planner_accel=-0.2) + for _ in range(40): + result = update(controller, slow_radar, v_ego=3.0, planner_accel=-0.2) + + assert result.lead_recovery + assert result.recovery_accel_limit == pytest.approx(LEAD_RECOVERY_HEADROOM) + assert result.recovery_accel_limit < profile_accel_max(AccelProfile.normal, 3.0) + + def test_recovery_accel_limit_slew_bounded_and_independent_of_planner_accel_sign(self): + braking_controller, accelerating_controller = make_controller(), make_controller() + radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0)) + for controller in (braking_controller, accelerating_controller): + for _ in range(20): + 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) + before = braking_controller.lead_controller.recovery_accel_limit + assert accelerating_controller.lead_controller.recovery_accel_limit == pytest.approx(before) + + braking = update(braking_controller, radar, v_ego=8.0, planner_accel=-0.2) + accelerating = update(accelerating_controller, radar, v_ego=8.0, planner_accel=0.2) + + assert abs(braking.recovery_accel_limit - before) <= LEAD_RECOVERY_ACCEL_SLEW * DT_MDL + 1e-9 + assert accelerating.recovery_accel_limit == pytest.approx(braking.recovery_accel_limit) + + +class TestLaunchAndDeparture: + 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 + TARGET_RELEASE_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) <= TARGET_RELEASE_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_departure_launch_keeps_profile_ceiling_through_lead_recovery_transition(self): + controller = make_controller() + enter_stop_hold(controller, base_speed=12.0, track_id=100) + for frame in range(STOP_HOLD_EXIT_FRAMES): + departing = make_radar(make_lead(status=True, d_rel=6.5 + 0.1 * frame, v_lead_k=5.0, radar_track_id=100)) + launching = update(controller, departing, base_speed=12.0, v_ego=0.1, profile=AccelProfile.eco, planner_accel=1.4) + + exited = update(controller, departing, base_speed=12.0, v_ego=LAUNCH_END_SPEED, profile=AccelProfile.eco, planner_accel=1.4) + + assert launching.departure_launching and effective_accel_max(launching) == pytest.approx(profile_accel_max(AccelProfile.eco, 0.1)) + profile_ceiling = profile_accel_max(AccelProfile.eco, LAUNCH_END_SPEED) + assert exited.lead_recovery + assert profile_ceiling - LEAD_RECOVERY_ACCEL_SLEW * DT_MDL <= effective_accel_max(exited) <= profile_ceiling + + def test_e2e_braking_handoff_arms_only_on_seed_frame_from_previous_plan_accel(self): + armed = make_controller() + update(armed, base_speed=20.0, v_ego=15.0, previous_mpc_source=LongitudinalPlanSource.e2e, + previous_plan_accel=-1.0, planner_accel=0.5) + assert armed.lead_controller.e2e_braking_handoff + + not_armed = make_controller() + update(not_armed, base_speed=20.0, v_ego=15.0, previous_mpc_source=LongitudinalPlanSource.e2e, + previous_plan_accel=0.5, planner_accel=-1.0) + assert not not_armed.lead_controller.e2e_braking_handoff + + def test_stop_hold_needs_four_confirmed_departure_frames_with_real_radar(self): + controller = make_controller() + held = enter_stop_hold(controller, track_id=100) + assert held.stop_hold and held.target_speed == 0.0 and held.mpc_accel_max is None + + results = [update(controller, make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100)), + base_speed=8.0, v_ego=0.1) for frame in range(STOP_HOLD_EXIT_FRAMES + 4)] + launch_index = next(index for index, result in enumerate(results) if result.launching) + + assert launch_index == STOP_HOLD_EXIT_FRAMES - 1 + assert all(result.stop_hold and not result.launching for result in results[:launch_index]) + assert results[launch_index].departure_launching + assert results[launch_index].target_speed == 8.0 + + def test_renewed_stop_mid_launch_aborts_back_to_stop_hold(self): + controller = make_controller() + enter_stop_hold(controller, track_id=100) + for frame in range(STOP_HOLD_EXIT_FRAMES + 2): + moving = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100)) + update(controller, moving, base_speed=8.0, v_ego=0.1) + assert controller.lead_controller.launching and controller.lead_controller.departure_launching + + renewed_stop_lead = make_radar(make_lead(status=True, d_rel=6.5, v_lead_k=0.05, radar_track_id=100)) + result = update(controller, renewed_stop_lead, base_speed=8.0, v_ego=0.1) + assert result.stop_hold + assert result.target_speed == 0.0 + assert not result.launching + + def test_invalid_lead_mid_launch_aborts_launch_without_reentering_stop_hold(self): + controller = make_controller() + enter_stop_hold(controller, track_id=100) + for frame in range(STOP_HOLD_EXIT_FRAMES + 2): + moving = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100)) + update(controller, moving, base_speed=8.0, v_ego=0.5) + assert controller.lead_controller.launching + + invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=2.0)) + result = update(controller, invalid, base_speed=8.0, v_ego=0.5) + assert not result.stop_hold + assert not result.launching + assert result.active + + def test_genuine_departure_survives_lead_slot_and_track_flicker(self): + controller = make_controller() + enter_stop_hold(controller, track_id=100) + results = [] + for frame in range(STOP_HOLD_EXIT_FRAMES + 4): + 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) + radar = make_radar(moving, secondary) if frame % 2 == 0 else make_radar(secondary, moving) + 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 launch_index == STOP_HOLD_EXIT_FRAMES - 1 + assert all(result.stop_hold for result in results[:launch_index]) + assert results[-1].launching and results[-1].departure_launching + + def test_confirmed_creep_departure_departs_within_budget(self): + controller = make_controller() + enter_stop_hold(controller) + 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(not result.stop_hold for result in results[launch_index:]) + + def test_departure_dropout_holds_without_resurrecting_stop_hold(self): + controller = make_controller() + enter_stop_hold(controller) + for frame in range(STOP_HOLD_EXIT_FRAMES + 2): + moving = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0)) + update(controller, moving, base_speed=8.0, v_ego=0.1) + assert controller.lead_controller.launching + + dropout = [update(controller, base_speed=8.0, v_ego=0.1) for _ in range(controller.lead_confirm_frames + 5)] + assert all(not result.stop_hold for result in dropout) + assert all(result.launching for result in dropout) + + def test_departure_launch_expires_after_bounded_radar_staleness(self): + controller = make_controller() + enter_stop_hold(controller, track_id=100) + for frame in range(STOP_HOLD_EXIT_FRAMES + 2): + moving = make_radar(make_lead(status=True, d_rel=6.0 + (frame + 1) * 0.1, v_lead_k=2.0, radar_track_id=100)) + update(controller, moving, base_speed=8.0, v_ego=0.1) + assert controller.lead_controller.departure_launching + + held = [update(controller, moving, base_speed=8.0, v_ego=0.1, radar_fresh=False) + for _ in range(controller.dropout_frames - 1)] + expired = update(controller, moving, base_speed=8.0, v_ego=0.1, radar_fresh=False) + + assert all(result.active and result.departure_launching for result in held) + assert not expired.active + assert not controller.lead_controller.departure_launching + assert controller.update_should_stop(True) + + 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.stop_hold + assert missing.target_speed == 0.0 + assert missing.mpc_accel_max is None + + def test_leadless_departure_does_not_override_should_stop(self): + controller = make_controller() + enter_stop_hold(controller) + for _ in range(controller.lead_confirm_frames): + result = update(controller, base_speed=8.0, v_ego=0.0) + + assert result.launching + assert not result.departure_launching + assert controller.update_should_stop(True) + + def test_invalid_lead_does_not_release_stop_hold(self): + controller = make_controller() + enter_stop_hold(controller, track_id=100) + invalid = make_radar(make_lead(status=True, d_rel=math.nan, v_lead_k=2.0, radar_track_id=100)) + + results = [update(controller, invalid, base_speed=8.0, v_ego=0.31) for _ in range(controller.lead_confirm_frames + 2)] + first_absent = update(controller, base_speed=8.0, v_ego=0.31) + + assert all(result.stop_hold and result.target_speed == 0.0 for result in results) + assert first_absent.stop_hold and first_absent.target_speed == 0.0 + assert controller.update_should_stop(False) + + def test_vision_lead_departure_does_not_override_should_stop(self): + controller = make_controller() + enter_stop_hold(controller) + for frame in range(STOP_HOLD_EXIT_FRAMES): + moving = make_radar(make_lead(status=True, d_rel=6.0 + 0.1 * frame, v_lead_k=2.0)) + result = update(controller, moving, base_speed=8.0, v_ego=0.0) + + assert result.launching + assert not result.departure_launching + assert controller.update_should_stop(True) + + def test_replacement_lead_cannot_claim_a_confirmed_departure(self): + controller = make_controller() + stopped = make_lead(status=True, d_rel=6.0, v_lead_k=0.0, radar_track_id=100) + moving = make_lead(status=True, d_rel=20.0, v_lead_k=2.0, radar_track_id=200) + for _ in range(6): + held = update(controller, make_radar(stopped, moving), base_speed=8.0, v_ego=0.0, planner_accel=-0.5) + assert held.stop_hold + + for _ in range(STOP_HOLD_EXIT_FRAMES): + moving.dRel += 0.1 + result = update(controller, make_radar(lead_two=moving), base_speed=8.0, v_ego=0.0, planner_accel=-0.5) + + assert result.launching + assert not result.departure_launching + assert controller.update_should_stop(True) + + def test_moving_departure_does_not_reenter_stop_hold_once_launching(self): + controller = make_controller() + enter_stop_hold(controller, track_id=100) + distance = 6.0 + results = [] + for speed in (0.81, 0.82, 0.83, 0.84, 0.79, 0.76, 0.74, 0.72, 0.70): + 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(not result.stop_hold 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_far_stopped_lead_should_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(not result.stop_hold for result in results) + + def test_fast_speed_glitch_without_distance_progress_should_stay_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) + 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 + 2)] + + assert all(result.stop_hold and result.target_speed == 0.0 and not result.launching for result in results) + + +class TestFreshnessAndReset: + def test_frozen_output_during_a_single_non_fresh_radar_frame(self): + controller = make_controller() + radar = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=8.0, a_lead_k=-0.5)) + for _ in range(15): + fresh = update(controller, radar, v_ego=10.0, planner_accel=-0.2) + + held = update(controller, radar, v_ego=10.0, planner_accel=-0.2, radar_fresh=False) + assert held.target_speed == pytest.approx(fresh.target_speed) + assert held.state == fresh.state + assert held.selected_lead == fresh.selected_lead + assert effective_accel_max(held) == pytest.approx(effective_accel_max(fresh)) + + def test_stale_timeout_fully_resets_live_state(self): + controller = make_controller() + radar = restrictive_radar() + for _ in range(15): + update(controller, radar) + held = [update(controller, radar_fresh=False) for _ in range(controller.radar_stale_frames - 1)] + timed_out = update(controller, radar_fresh=False) + + assert all(result.active 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 + assert controller.lead_controller.target_speed is None + + def test_acc_bypass_starts_a_new_stale_episode(self): + controller = make_controller() + radar = restrictive_radar() + update(controller, radar) + for _ in range(controller.radar_stale_frames - 1): + update(controller, radar, radar_fresh=False) + + bypassed = update(controller, radar, acc_selected=False, radar_fresh=False) + handoff = update(controller, radar, radar_fresh=False, previous_mpc_source=LongitudinalPlanSource.e2e, + previous_plan_accel=-0.5, planner_accel=0.5) + + assert not bypassed.active + assert handoff.active + assert controller.lead_controller.e2e_braking_handoff + + @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(15): + 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.lead_controller.target_speed is None + + def test_acc_bypass_does_not_retain_state_for_live_actuation(self): + controller = make_controller() + for _ in range(20): + bypassed = update(controller, restrictive_radar(), acc_selected=False) + assert not bypassed.active + assert controller.lead_controller.target_speed is None + live = update(controller) + + assert live.active + assert 10.0 < live.target_speed <= 10.0 + TARGET_RELEASE_SLEW * DT_MDL + 1e-9 + + def test_explicit_reset_clears_lead_state(self): + controller = make_controller() + for _ in range(15): + 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 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 + lead_controller = controller.lead_controller + assert lead_controller.target_speed is None and lead_controller.recovery_accel_limit is None + assert not lead_controller.stop_hold and not lead_controller.launching and not lead_controller.lead_recovery + assert math.isinf(lead_controller.lead_speed_ceiling) and math.isinf(lead_controller.filtered_lead_speed) + assert lead_controller.lead_speed_samples == [math.inf] * LEAD_SAMPLE_FILTER_FRAMES + assert controller.state == AccelControllerState.inactive + assert controller.selected_lead == controller.selected_lead_track_id == -1 + + +class TestJerkCostMultiplier: + @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.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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py new file mode 100644 index 0000000000..238f951d96 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_accel_controller_interfaces.py @@ -0,0 +1,613 @@ +import inspect +import math +from types import SimpleNamespace + +import numpy as np +import pytest + +from openpilot.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, cruise_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.cruise_accel_max = cruise_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.lead_controller = SimpleNamespace(departure_launching=departure_launching, stop_hold=state == AccelControllerState.stopHold) + self.required_decel = required_decel + self.dt = DT_MDL + self._jerk_smoothing_blocked = False + self._required_decel_samples = [] + self._required_decel_long_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, departure_authorized=True): + return AccelController.update_should_stop(self, should_stop, departure_authorized) + + +def planner_for_mpc_test(*, target_speed=15.0, active=True, is_e2e=False, mpc_accel_max=None, + cruise_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._long_active_last_cycle = True + planner.previous_plan_accel = 0.0 + planner.mpc_accel_seed = 0.0 + 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, + cruise_accel_max=cruise_accel_max, + ) + return planner, is_e2e_calls + + +def prepare_controller_mpc(planner, *, mpc_v_cruise=20.0, force_decel=False, stock_accel_max=ACCEL_MAX): + configs = [] + sm = { + "radarState": radar_state(), + "controlsState": SimpleNamespace(forceDecel=force_decel), + "carState": SimpleNamespace(vCruise=20.0, vEgo=10.0, aEgo=0.0), + "selfdriveState": SimpleNamespace(personality=0), + } + planner.mpc.set_accel_controller_params = lambda *args: configs.append(args) + is_e2e, target = planner.update_accel_controller(sm, mpc_v_cruise, True, stock_accel_max, False) + assert len(configs) == 1 + return is_e2e, target, configs[0] + + +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_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) + assert mpc.cruise_accel_max(1.6) == 1.6 + + mpc.set_accel_controller_params(None, 1.0, 0.4) + assert mpc.cruise_accel_max(1.6) == 0.4 + mpc.set_accel_controller_params(None, 1.0, 0.0) + assert mpc.cruise_accel_max(1.6) == 0.0 + + 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_accel_controller_hook_only_configures_mpc(): + radar = radar_state() + planner, _ = planner_for_mpc_test(active=False) + calls = [] + planner.mpc = SimpleNamespace( + source=MpcLongitudinalPlanSource.cruise, + last_solution_status=0, + set_accel_controller_params=lambda accel_max, multiplier, cruise_accel_max: calls.append( + ("configure", accel_max, multiplier, cruise_accel_max)), + set_weights=lambda constraint, personality: calls.append(("weights", constraint, personality)), + set_cur_state=lambda speed, accel: calls.append(("state", speed, accel)), + update=lambda radar_arg, target, *, personality: calls.append(("update", radar_arg, target, personality)), + ) + sm = { + "radarState": radar, + "controlsState": SimpleNamespace(forceDecel=False), + "carState": SimpleNamespace(vCruise=20.0, vEgo=10.0, aEgo=0.0), + "selfdriveState": SimpleNamespace(personality=2), + } + is_e2e, target = planner.update_accel_controller(sm, 17.5, True, ACCEL_MAX, False) + + assert not is_e2e and target == 17.5 + assert calls == [("configure", None, 1.0, None)] + + +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, target, config = prepare_controller_mpc(planner) + + assert not is_e2e + assert len(mode_calls) == 1 + assert target == 15.0 + assert config == (ceiling, 1.0, None) + + +@pytest.mark.parametrize("cruise_accel_max", (0.0, 0.3)) +def test_cruise_accel_ceiling_is_forwarded_to_mpc(cruise_accel_max): + planner, _ = planner_for_mpc_test(cruise_accel_max=cruise_accel_max) + _, _, config = prepare_controller_mpc(planner) + assert config == (None, 1.0, cruise_accel_max) + + +@pytest.mark.parametrize(("stock_accel_max", "expected"), ((1.2, 1.2), (-0.3, 0.0))) +def test_controller_receives_stock_allow_throttle_ceiling(stock_accel_max, expected): + planner, _ = planner_for_mpc_test() + planner.allow_throttle = False + prepare_controller_mpc(planner, stock_accel_max=stock_accel_max) + + assert planner.accel_controller.update_kwargs["stock_accel_max"] == expected + + +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, + ) + _, target, config = prepare_controller_mpc(planner) + + assert target == 20.0 + assert config == (None, 1.0, None) + + +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, + ) + _, target, config = prepare_controller_mpc(planner) + + assert target == 0.0 + assert config == (None, 1.0, None) + + +@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) + planner._accel_controller_actuating = active + planner._radar_fresh_this_cycle = True + planner.mpc = SimpleNamespace(last_solution_status=0) + assert planner.update_should_stop(True) is expected + assert planner.update_should_stop(False) is (active and not departure_launching) + + +def test_stale_radar_cannot_authorize_departure(): + planner = LongitudinalPlannerSP.__new__(LongitudinalPlannerSP) + planner.accel_controller = ControllerStub(active=True, departure_launching=True) + planner._accel_controller_actuating = True + planner._radar_fresh_this_cycle = False + planner.mpc = SimpleNamespace(last_solution_status=0) + + assert planner.update_should_stop(True) + + +@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, target, config = prepare_controller_mpc(planner) + + assert returned_e2e is is_e2e + assert len(mode_calls) == 1 + assert target == 20.0 + assert config == (None, 1.0, None) + + +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) + _, target, config = prepare_controller_mpc(planner, mpc_v_cruise=0.0, force_decel=True) + + assert len(mode_calls) == 1 + assert target == 0.0 + assert config == (None, 1.0, None) + + +def test_previous_mpc_failure_gets_one_stock_recovery_cycle_without_resetting_controller_state(): + ceiling = tuple(np.linspace(0.8, 0.4, N + 1)) + planner, mode_calls = planner_for_mpc_test(mpc_accel_max=ceiling, departure_launching=True) + controller = planner.accel_controller + planner.mpc.last_solution_status = 4 + + _, failed_target, failed_config = prepare_controller_mpc(planner) + assert controller.reset_calls == 0 + assert controller.update_kwargs["acc_selected"] + assert len(mode_calls) == 1 + assert failed_target == 20.0 + assert failed_config == (None, 1.0, None) + assert planner.update_should_stop(True) + + planner.mpc.last_solution_status = 0 + _, recovered_target, recovered_config = prepare_controller_mpc(planner) + assert controller.reset_calls == 0 + assert len(mode_calls) == 2 + assert recovered_target == 15.0 + assert recovered_config == (ceiling, 1.0, None) + assert not planner.update_should_stop(True) + + +@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, + ) + _, target, config = prepare_controller_mpc(planner) + + assert target == 15.0 + assert config == (None, MPC_DECEL_JERK_COST_MULTIPLIER, None) + + +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_config = prepare_controller_mpc(planner) + controller = planner.accel_controller + assert initial_config[1] == MPC_DECEL_JERK_COST_MULTIPLIER + + controller.required_decel = MPC_DECEL_JERK_MAX_REQUIRED_DECEL + _, _, ineligible_config = prepare_controller_mpc(planner) + assert ineligible_config[1] == 1.0 + + controller.required_decel = 0.30 + _, _, flicker_config = prepare_controller_mpc(planner) + assert flicker_config[1] == 1.0 + + controller.state = AccelControllerState.free + controller.output_v_target = 20.0 + prepare_controller_mpc(planner) + controller.state = AccelControllerState.restrict + controller.output_v_target = 15.0 + _, _, rearmed_config = prepare_controller_mpc(planner) + assert rearmed_config[1] == 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, + ) + _, _, config = prepare_controller_mpc(planner) + controller = planner.accel_controller + multipliers = [config[1]] + for required_decel in (0.20, 0.23, 0.25): + controller.required_decel = required_decel + _, _, config = prepare_controller_mpc(planner) + multipliers.append(config[1]) + + assert multipliers == [MPC_DECEL_JERK_COST_MULTIPLIER] * 3 + [1.0] + + controller.required_decel = 0.20 + _, _, config = prepare_controller_mpc(planner) + assert config[1] == 1.0 + + controller.state = AccelControllerState.free + controller.output_v_target = 20.0 + prepare_controller_mpc(planner) + controller.state = AccelControllerState.restrict + controller.output_v_target = 15.0 + controller.required_decel = 0.18 + _, _, config = prepare_controller_mpc(planner) + assert config[1] == 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, + ) + _, _, config = prepare_controller_mpc(planner) + controller = planner.accel_controller + multipliers = [config[1]] + for required_decel in (0.24, 0.19, 0.22): + controller.required_decel = required_decel + _, _, config = prepare_controller_mpc(planner) + multipliers.append(config[1]) + + 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, + ) + _, _, config = prepare_controller_mpc(planner) + + assert config[1] == 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.previous_plan_accel = -1.1 + planner.v_desired_filter = SimpleNamespace(x=9.5) + prepare_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["previous_plan_accel"] == -1.1 + assert received["radar_fresh"] is True + + +@pytest.mark.parametrize(("previous_plan_accel", "a_desired", "expected"), ( + (-1.1, 0.0, -1.1), (0.057, 1.032, 0.057), (0.4, -1.2, -1.2), +)) +def test_e2e_to_acc_handoff_uses_one_sided_mpc_seed(previous_plan_accel, a_desired, expected): + planner, _ = planner_for_mpc_test(mpc_source=MpcLongitudinalPlanSource.e2e) + planner.previous_plan_accel = previous_plan_accel + planner.a_desired = a_desired + prepare_controller_mpc(planner) + + assert planner.mpc_accel_seed == expected + assert planner.a_desired == a_desired + + +def test_disabled_controller_does_not_change_e2e_to_acc_seed(): + planner, _ = planner_for_mpc_test(active=False, mpc_source=MpcLongitudinalPlanSource.e2e) + planner.accel_controller.enabled = False + planner.previous_plan_accel = -1.1 + planner.a_desired = 0.0 + prepare_controller_mpc(planner) + + assert planner.mpc_accel_seed == planner.a_desired == 0.0 + + +def test_inactive_previous_cycle_does_not_restore_an_old_e2e_brake_plan(): + planner, _ = planner_for_mpc_test(mpc_source=MpcLongitudinalPlanSource.e2e) + planner._long_active_last_cycle = False + planner.previous_plan_accel = -1.1 + planner.a_desired = 0.0 + prepare_controller_mpc(planner) + + assert planner.mpc_accel_seed == planner.a_desired == 0.0 + assert math.isinf(planner.accel_controller.update_kwargs["previous_plan_accel"]) + + +def test_failed_e2e_solution_does_not_seed_the_next_mpc_cycle(): + planner, _ = planner_for_mpc_test(mpc_source=MpcLongitudinalPlanSource.e2e) + planner.mpc.last_solution_status = 1 + planner.previous_plan_accel = -1.1 + planner.a_desired = 0.0 + prepare_controller_mpc(planner) + + assert planner.mpc_accel_seed == planner.a_desired == 0.0 + assert math.isinf(planner.accel_controller.update_kwargs["previous_plan_accel"]) + + +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._long_active_last_cycle = False + planner.previous_plan_accel = 0.0 + 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, + set_accel_controller_params=lambda *_args: None, + ) + planner.is_e2e = lambda _sm: False + planner.accel_controller = ControllerStub(target_speed=20.0, active=False) + + sm = PlannerSM(100) + for expected in (True, False): + planner.update(sm) + planner.update_accel_controller(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_accel_controller(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"} diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_lead_controller.py b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_lead_controller.py new file mode 100644 index 0000000000..4b4c5dad82 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/accel_controller/tests/test_lead_controller.py @@ -0,0 +1,341 @@ +import math + +import pytest + +from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( + LEAD_SAMPLE_FILTER_FRAMES, COMFORT_DECEL, DISTANCE_JUMP_CONFIRM_FRAMES, LEAD_DROPOUT_COAST_TIME, LEAD_RELEASE_CONFIRM_TIME, + STOP_HOLD_EXIT_FRAMES, TARGET_RELEASE_SLEW, AccelProfile, +) +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead import LeadPlan +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.lead_controller import LeadController + +DT = DT_MDL +COMFORT_DECEL_NORMAL = COMFORT_DECEL[AccelProfile.normal] +PROFILE_MAX_ACCEL = 1.5 + + +def _frames(seconds: float) -> int: + return math.ceil(seconds / DT) + + +LEAD_CONFIRM_FRAMES = max(LEAD_SAMPLE_FILTER_FRAMES, _frames(LEAD_RELEASE_CONFIRM_TIME)) +DROPOUT_FRAMES = max(LEAD_CONFIRM_FRAMES, _frames(LEAD_DROPOUT_COAST_TIME)) + + +def _lead_plan(speed: float, distance: float, speed_ceiling: float = 0.0, closing_speed: float = 0.0, + required_decel: float = 0.0, track_id: int = 1) -> LeadPlan: + return LeadPlan( + speed_ceiling=speed_ceiling, selected_lead=0, selected_lead_track_id=track_id, selected_lead_speed=speed, selected_lead_accel=0.0, + departure_lead=0, departure_lead_track_id=track_id, departure_lead_speed=speed, departure_lead_distance=distance, + departure_lead_raw_speed=speed, + departure_lead_separation=distance, departure_speed_ceiling=speed_ceiling, + closing_speed=closing_speed, required_decel=required_decel, + has_nearly_stopped_lead=speed < 0.15, lead_status=True, + ) + + +def _no_lead() -> LeadPlan: + return LeadPlan(lead_status=False) + + +def _run(lead_controller: LeadController, lead_plan: LeadPlan, base_speed: float, v_ego: float, planner_speed: float | None = None, + planner_accel: float = 0.0, previous_mpc_source=None, previous_plan_accel: float = 0.0) -> float: + return lead_controller.update(lead_plan, base_speed, v_ego, COMFORT_DECEL_NORMAL, PROFILE_MAX_ACCEL, DT, + LEAD_CONFIRM_FRAMES, DROPOUT_FRAMES, v_ego if planner_speed is None else planner_speed, + planner_accel, previous_mpc_source, previous_plan_accel) + + +def test_repeated_stop_reseeds_departure_distance(): + lead_controller = LeadController() + for _ in range(20): + _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0) + for frame in range(20): + _run(lead_controller, _lead_plan(2.0, 6.0 + 2.0 * (frame + 1) * DT), base_speed=8.0, v_ego=min(3.5, frame * 0.3)) + + for _ in range(10): + _run(lead_controller, _lead_plan(0.0, 3.0), base_speed=8.0, v_ego=0.0) + assert lead_controller.stop_hold + + released_frame = None + for frame in range(60): + _run(lead_controller, _lead_plan(0.2, 3.0 + 0.2 * (frame + 1) * DT), base_speed=8.0, v_ego=0.0) + if not lead_controller.stop_hold: + released_frame = frame + break + + assert released_frame is not None + assert released_frame * DT <= 2.0 + + +def test_departure_dropout_reseeds_distance_guard(): + lead_controller = LeadController() + for _ in range(20): + _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0) + _run(lead_controller, _no_lead(), base_speed=8.0, v_ego=0.0) + + released_frame = None + for frame in range(60): + _run(lead_controller, _lead_plan(0.2, 3.0 + 0.2 * (frame + 1) * DT), base_speed=8.0, v_ego=0.0) + if not lead_controller.stop_hold: + released_frame = frame + break + + assert released_frame is not None + assert released_frame * DT <= 2.0 + + +def test_departure_dropout_revokes_track_identity_trust(): + lead_controller = LeadController() + for _ in range(20): + _run(lead_controller, _lead_plan(0.0, 6.0, track_id=100), base_speed=8.0, v_ego=0.0) + + _run(lead_controller, _no_lead(), base_speed=8.0, v_ego=0.0) + for frame in range(4): + _run(lead_controller, _lead_plan(2.0, 20.0 + 0.1 * frame, speed_ceiling=8.0, track_id=100), base_speed=8.0, v_ego=0.0) + + assert lead_controller.launching + assert lead_controller.leadless_departure + assert not lead_controller.departure_launching + + +def test_range_discontinuity_revokes_track_identity_trust(): + lead_controller = LeadController() + for _ in range(20): + _run(lead_controller, _lead_plan(0.0, 6.0, track_id=100), base_speed=8.0, v_ego=0.0) + + for frame in range(DISTANCE_JUMP_CONFIRM_FRAMES + STOP_HOLD_EXIT_FRAMES + 2): + _run(lead_controller, _lead_plan(2.0, 20.0 + 0.1 * frame, speed_ceiling=8.0, track_id=100), base_speed=8.0, v_ego=0.0) + if not lead_controller.stop_hold: + break + + assert lead_controller.launching + assert lead_controller.leadless_departure + assert not lead_controller.departure_launching + + +def test_fast_lead_speed_requires_full_departure_distance(): + lead_controller = LeadController() + for _ in range(20): + _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0) + + for distance in (6.00, 6.01, 6.02, 6.03): + target = _run(lead_controller, _lead_plan(1.0, distance), base_speed=8.0, v_ego=0.0) + + assert lead_controller.stop_hold + assert target == 0.0 + + +def test_lead_disappearance_releases_stop_hold_monotonically(): + lead_controller = LeadController() + for _ in range(20): + _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0) + + targets = [_run(lead_controller, _no_lead(), base_speed=8.0, v_ego=0.0) for _ in range(DROPOUT_FRAMES + 10)] + first_release = next(index for index, target in enumerate(targets) if target > 0.0) + release_targets = targets[first_release:] + + steps = [after - before for before, after in zip(release_targets[:-1], release_targets[1:], strict=True)] + assert all(0.0 <= step <= TARGET_RELEASE_SLEW * DT + 1e-9 for step in steps) + + +def test_no_lead_departure_stays_bounded_when_lead_returns(): + lead_controller = LeadController() + for _ in range(20): + _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0) + for _ in range(LEAD_CONFIRM_FRAMES): + _run(lead_controller, _no_lead(), base_speed=8.0, v_ego=0.0) + + before = lead_controller.target_speed + target = _run(lead_controller, _lead_plan(2.0, 6.0, speed_ceiling=8.0), base_speed=8.0, v_ego=0.5) + + assert target <= max(before, 0.5) + TARGET_RELEASE_SLEW * DT + 1e-9 + + +def test_leadless_stop_release_never_exceeds_current_base_speed(): + lead_controller = LeadController() + for _ in range(20): + _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0) + + targets = [_run(lead_controller, _no_lead(), base_speed=0.5, v_ego=4.0) for _ in range(LEAD_CONFIRM_FRAMES)] + + assert max(targets) <= 0.5 + + +def test_retained_speed_ceiling_does_not_bind_a_moving_replacement(): + lead_controller = LeadController() + _run(lead_controller, _lead_plan(0.0, 5.0, speed_ceiling=0.1, track_id=100), base_speed=8.0, v_ego=0.4) + + _run(lead_controller, _lead_plan(2.0, 20.0, speed_ceiling=8.0, track_id=200), base_speed=8.0, v_ego=0.2) + + assert lead_controller.lead_speed_ceiling == pytest.approx(0.1) + assert not lead_controller.stop_hold + + +def test_braking_state_does_not_cross_confirmed_track_replacement(): + lead_controller = LeadController() + for _ in range(LEAD_CONFIRM_FRAMES + 2): + _run(lead_controller, _lead_plan(8.0, 20.0, speed_ceiling=5.0, track_id=10), base_speed=15.0, v_ego=10.0, planner_accel=-0.5) + assert lead_controller.braking_for_lead + + _run(lead_controller, _lead_plan(0.0, 25.0, speed_ceiling=4.0, track_id=99), base_speed=8.0, v_ego=0.0, planner_accel=-0.5) + + assert not lead_controller.stop_hold + + +def test_braking_state_does_not_cross_vision_slot_replacement(): + lead_controller = LeadController() + for _ in range(LEAD_CONFIRM_FRAMES + 2): + _run(lead_controller, _lead_plan(8.0, 20.0, speed_ceiling=5.0, track_id=-1), base_speed=15.0, v_ego=10.0, planner_accel=-0.5) + assert lead_controller.braking_for_lead + + replacement = _lead_plan(0.0, 25.0, speed_ceiling=4.0, track_id=-1)._replace(selected_lead=1, departure_lead=1) + _run(lead_controller, replacement, base_speed=8.0, v_ego=0.0, planner_accel=-0.5) + + assert not lead_controller.stop_hold + + +def test_radar_track_keeps_braking_confirmation_across_slots(): + lead_controller = LeadController() + for frame in range(LEAD_CONFIRM_FRAMES + 2): + plan = _lead_plan(0.0, 7.0, speed_ceiling=0.8, track_id=100) + if frame % 2: + plan = plan._replace(selected_lead=1, departure_lead=1) + _run(lead_controller, plan, base_speed=8.0, v_ego=0.0, planner_accel=-0.5) + + assert lead_controller.braking_for_lead + assert lead_controller.stop_hold + + +def test_vision_slot_churn_releases_conservatively(): + lead_controller = LeadController() + for _ in range(20): + _run(lead_controller, _lead_plan(0.0, 6.0, track_id=-1), base_speed=8.0, v_ego=0.0) + + for frame in range(STOP_HOLD_EXIT_FRAMES + 2): + plan = _lead_plan(2.0, 6.0 + 0.1 * (frame + 1), speed_ceiling=8.0, track_id=-1) + if frame % 2: + plan = plan._replace(selected_lead=1, departure_lead=1) + _run(lead_controller, plan, base_speed=8.0, v_ego=0.0) + if not lead_controller.stop_hold: + break + + assert lead_controller.launching + assert lead_controller.leadless_departure + assert not lead_controller.departure_launching + + +def test_radar_track_dropout_keeps_braking_context(): + lead_controller = LeadController() + for _ in range(LEAD_CONFIRM_FRAMES + 2): + _run(lead_controller, _lead_plan(8.0, 20.0, speed_ceiling=5.0, track_id=10), base_speed=15.0, v_ego=10.0, planner_accel=-0.5) + assert lead_controller.braking_for_lead + + _run(lead_controller, _no_lead(), base_speed=15.0, v_ego=10.0, planner_accel=-0.5) + + assert lead_controller.should_coast_on_dropout + + +def test_range_replacement_starts_a_new_departure_baseline(): + lead_controller = LeadController() + for _ in range(20): + _run(lead_controller, _lead_plan(0.0, 6.0), base_speed=8.0, v_ego=0.0) + for _ in range(2): + _run(lead_controller, _lead_plan(0.2, 6.0), base_speed=8.0, v_ego=0.0) + + released_frame = None + for frame in range(60): + distance = 3.0 + 0.2 * frame * DT + _run(lead_controller, _lead_plan(0.2, distance), base_speed=8.0, v_ego=0.0) + if not lead_controller.stop_hold: + released_frame = frame + break + + assert released_frame is not None + assert released_frame * DT <= 2.0 + + +def test_stop_hold_clears_the_previous_recovery_limit(): + lead_controller = LeadController() + for _ in range(LEAD_CONFIRM_FRAMES + 120): + _run(lead_controller, _lead_plan(5.0, 20.0, speed_ceiling=8.0), base_speed=12.0, v_ego=10.0) + assert lead_controller.recovery_accel_limit == pytest.approx(0.0) + + _run(lead_controller, _lead_plan(0.0, 5.0), base_speed=8.0, v_ego=0.2, planner_accel=-0.5) + assert lead_controller.stop_hold + assert lead_controller.recovery_accel_limit is None + + for frame in range(9): + _run(lead_controller, _lead_plan(1.0, 5.0 + 0.04 * frame, speed_ceiling=8.0), base_speed=8.0, v_ego=0.0) + assert lead_controller.departure_launching + + _run(lead_controller, _lead_plan(5.0, 8.0, speed_ceiling=8.0), base_speed=8.0, v_ego=3.1) + assert lead_controller.recovery_accel_limit is not None + assert lead_controller.recovery_accel_limit > 1.4 + + +def test_speed_ceiling_tightens_immediately(): + lead_controller = LeadController() + _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=25.0), base_speed=30.0, v_ego=20.0) + assert lead_controller.lead_speed_ceiling == pytest.approx(25.0) + _run(lead_controller, _lead_plan(15.0, 40.0, speed_ceiling=10.0), base_speed=30.0, v_ego=20.0) + assert lead_controller.lead_speed_ceiling == pytest.approx(10.0) + + +def test_speed_ceiling_requires_confirmation_before_release(): + lead_controller = LeadController() + for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2): + _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=20.0), base_speed=30.0, v_ego=20.0) + assert lead_controller.lead_speed_ceiling == pytest.approx(20.0) + + glitch_len = LEAD_CONFIRM_FRAMES - 2 + for _ in range(glitch_len): + _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=28.0), base_speed=30.0, v_ego=20.0) + assert lead_controller.lead_speed_ceiling == pytest.approx(20.0), "brief relief spike must not be trusted" + + for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2): + _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=20.0), base_speed=30.0, v_ego=20.0) + assert lead_controller.lead_speed_ceiling == pytest.approx(20.0) + + for _ in range(LEAD_CONFIRM_FRAMES + LEAD_SAMPLE_FILTER_FRAMES + 2): + _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=28.0), base_speed=30.0, v_ego=20.0) + assert lead_controller.lead_speed_ceiling == pytest.approx(28.0), "sustained relief must eventually be trusted" + + +def test_conflicting_relief_evidence_stays_restricted(): + lead_controller = LeadController() + for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2): + _run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=20.0), base_speed=30.0, v_ego=20.0) + + targets = [] + for frame in range(300): + speed_ceiling = 28.0 if frame % 2 == 0 else 20.0 + targets.append(_run(lead_controller, _lead_plan(20.0, 100.0, speed_ceiling=speed_ceiling), base_speed=30.0, v_ego=20.0)) + + assert lead_controller.lead_speed_ceiling == pytest.approx(20.0) + assert max(targets) == pytest.approx(20.0) + + +def test_lead_dropout_holds_speed_ceiling_before_release(): + lead_controller = LeadController() + for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2): + _run(lead_controller, _lead_plan(6.0, 50.0, speed_ceiling=6.0), base_speed=18.0, v_ego=10.0) + assert lead_controller.lead_speed_ceiling == pytest.approx(6.0) + + for _ in range(DROPOUT_FRAMES - 1): + _run(lead_controller, _no_lead(), base_speed=18.0, v_ego=10.0) + assert lead_controller.lead_speed_ceiling == pytest.approx(6.0), "must coast, not snap, before the dropout window elapses" + + for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2): + _run(lead_controller, _no_lead(), base_speed=18.0, v_ego=10.0) + assert math.isinf(lead_controller.lead_speed_ceiling), "must release promptly once the dropout window has elapsed" + + +def test_target_release_is_rate_limited(): + lead_controller = LeadController() + for _ in range(LEAD_SAMPLE_FILTER_FRAMES + 2): + _run(lead_controller, _lead_plan(20.0, 60.0, speed_ceiling=22.0), base_speed=30.0, v_ego=22.0) + before = lead_controller.target_speed + + target = _run(lead_controller, _lead_plan(28.0, 200.0, speed_ceiling=30.0), base_speed=30.0, v_ego=22.0) + assert target <= before + TARGET_RELEASE_SLEW * DT + 1e-9 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/__init__.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py new file mode 100644 index 0000000000..39f242d5ad --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -0,0 +1,46 @@ +""" +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._cruise_accel_max: 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, + cruise_accel_max: float | None = None) -> None: + self._accel_max_trajectory = accel_max + self._cruise_accel_max = cruise_accel_max + self._jerk_cost_multiplier = jerk_cost_multiplier + + def cruise_accel_max(self, stock_accel_max: float) -> float: + if self._cruise_accel_max is None or not np.isfinite(self._cruise_accel_max): + return stock_accel_max + return min(max(self._cruise_accel_max, 0.0), stock_accel_max) + + 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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index f1e0c36416..8fa0740d03 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -5,10 +5,15 @@ 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 openpilot.cereal import messaging, custom 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.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.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 +27,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,9 +38,15 @@ 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._long_active_last_cycle = False + self._accel_controller_actuating = False self.output_v_target = 0. self.output_a_target = 0. + self.previous_plan_accel = 0. + self.mpc_accel_seed = 0. def is_e2e(self, sm: messaging.SubMaster) -> bool: experimental_mode = sm['selfdriveState'].experimentalMode @@ -43,6 +55,44 @@ class LongitudinalPlannerSP: return experimental_mode and self.dec.mode() == "blended" + def update_accel_controller(self, sm: messaging.SubMaster, v_cruise: float, prev_accel_constraint: bool, + stock_accel_max: float, reset_state: bool) -> tuple[bool, float]: + is_e2e = self.is_e2e(sm) + force_decel = sm['controlsState'].forceDecel + previous_mpc_failed = self.mpc.last_solution_status != 0 + previous_plan_accel = self.previous_plan_accel if self._long_active_last_cycle and not previous_mpc_failed else float("inf") + self.mpc_accel_seed = self.a_desired + + 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, + engaged=not reset_state and not force_decel, cruise_initialized=sm['carState'].vCruise != V_CRUISE_UNSET, + stock_accel_max=max(stock_accel_max, 0.0), + 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, previous_plan_accel=previous_plan_accel, + ) + controller = self.accel_controller + acc_handoff = (controller.is_active and not is_e2e and not previous_mpc_failed and self.mpc.source == MpcLongitudinalPlanSource.e2e + and math.isfinite(previous_plan_accel)) + if acc_handoff: + self.mpc_accel_seed = min(self.a_desired, previous_plan_accel) + actuating = controller.is_active and not is_e2e and not force_decel and not previous_mpc_failed + self._accel_controller_actuating = actuating + 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 + cruise_accel_max = controller.cruise_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.mpc.set_accel_controller_params(accel_max, jerk_cost_multiplier, cruise_accel_max) + self._long_active_last_cycle = not reset_state and not force_decel + return is_e2e, controller_v_cruise + + def update_should_stop(self, should_stop: bool) -> bool: + departure_authorized = self._accel_controller_actuating and self._radar_fresh_this_cycle and self.mpc.last_solution_status == 0 + return self.accel_controller.update_should_stop(should_stop, departure_authorized) + 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 +123,18 @@ 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.previous_plan_accel = self.output_a_target + 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 +156,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 diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py new file mode 100644 index 0000000000..b07b6556a1 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_accel_controller_closed_loop.py @@ -0,0 +1,1764 @@ +from collections.abc import Callable +from dataclasses import dataclass +import gc +import math + +import numpy as np +import pytest + +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 LongitudinalPlanSource, STOP_DISTANCE, get_T_FOLLOW +from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel +from openpilot.sunnypilot.selfdrive.test.longitudinal_maneuvers.plant import PRIUS_TSS2_ROUTE_MODEL, LeadObservation, PlantSP as Plant +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller import accel_controller as accel_controller_module +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.accel_controller import AccelControllerState +from openpilot.sunnypilot.selfdrive.controls.lib.accel_controller.constants import ( + LEAD_DROPOUT_COAST_TIME, LEAD_RECOVERY_DECEL_RATE, MPC_DECEL_JERK_COST_MULTIPLIER, MPC_DECEL_JERK_MAX_REQUIRED_DECEL, + MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE, MPC_DECEL_TREND_FRAMES, LEAD_RELEASE_CONFIRM_TIME, TARGET_RELEASE_SLEW, STOP_HOLD_EXIT_FRAMES, + AccelProfile, profile_accel_max, +) + +ACTUATOR_DYNAMICS = ( + (0.10, 0.20), + (0.15, 0.25), + (0.20, 0.20), + (0.25, 0.30), + (0.30, 0.35), +) +ACTUATOR_IDS = tuple(f"delay-{delay:.2f}-lag-{lag:.2f}" for delay, lag in ACTUATOR_DYNAMICS) +ROUTINE_GAP_TOLERANCE = 0.10 +ROUTINE_DECEL_TOLERANCE = 0.10 +DROPOUT_GAP_TOLERANCE = 0.15 +MOVING_LEAD_GAP_TOLERANCE = 0.12 + + +def _tracked_lead(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return truth | {"radar": True, "radarTrackId": 100} if lead_name == "leadOne" else None + + +@dataclass +class ClosedLoopTrace: + time: np.ndarray + speed: np.ndarray + distance: np.ndarray + distance_lead: np.ndarray + a_target: np.ndarray + acceleration: np.ndarray + should_stop: np.ndarray + fcw: np.ndarray + source: list + dec_mode: list[str] + active: np.ndarray + launching: np.ndarray + departure_launching: np.ndarray + target_speed: np.ndarray + raw_speed_ceiling: np.ndarray + selected_lead: np.ndarray + lead_recovery: np.ndarray + profile_accel_max: np.ndarray + accel_ceiling_active: np.ndarray + state: np.ndarray + required_decel: np.ndarray + planner_seed_accel: np.ndarray + mpc_seed_accel: np.ndarray + mpc_upper_first: np.ndarray + mpc_upper_min: np.ndarray + stock_bounds_valid: np.ndarray + solver_status: np.ndarray + solver_failures: int + solver_failure_times: list[float] + + +def _configure_plant(plant: Plant, *, enabled: bool, profile: int = 1, dec_enabled: bool = False) -> None: + plant.planner.accel_controller.enabled = enabled + plant.planner.accel_controller.profile = profile + plant.planner.accel_controller.update_params = lambda: None + plant.planner.dec._enabled = dec_enabled + plant.planner.dec._read_params = lambda: None + + +def _run( + *, + duration: float, + controller_enabled: bool, + profile: int = 1, + v_lead: float | Callable[[float], float] = 0.0, + v_cruise: float = 30.0, + dec_enabled: bool = False, + radar_fresh_fn: Callable[[int], bool] | None = None, + previous_solver_failure_fn: Callable[[float], bool] | None = None, + **plant_kwargs, +) -> ClosedLoopTrace: + gc.collect() + plant = Plant(**plant_kwargs) + _configure_plant(plant, enabled=controller_enabled, profile=profile, dec_enabled=dec_enabled) + plant.v_lead_prev = float(v_lead) if isinstance(v_lead, (int, float)) else float(v_lead(0.0)) + if radar_fresh_fn is not None: + radar_frame = 0 + + def patterned_radar_freshness(_sm): + nonlocal radar_frame + fresh = radar_fresh_fn(radar_frame) + radar_frame += 1 + return fresh + + plant.planner._update_radar_freshness = patterned_radar_freshness + + solver_failures = 0 + solver_failure_times = [] + seed_calls = [] + original_mpc_reset = plant.planner.mpc.reset + original_mpc_set_cur_state = plant.planner.mpc.set_cur_state + + def count_failed_solve(*args, **kwargs) -> None: + nonlocal solver_failures + if plant.planner.mpc.solution_status != 0: + solver_failures += 1 + solver_failure_times.append(plant.current_time) + original_mpc_reset(*args, **kwargs) + + def record_seed(v_ego, a_ego): + seed_calls.append((float(plant.planner.a_desired), float(a_ego))) + return original_mpc_set_cur_state(v_ego, a_ego) + + plant.planner.mpc.reset = count_failed_solve + plant.planner.mpc.set_cur_state = record_seed + rows = [] + sources = [] + dec_modes = [] + try: + while plant.current_time < duration: + lead_speed = float(v_lead) if isinstance(v_lead, (int, float)) else float(v_lead(plant.current_time)) + if previous_solver_failure_fn is not None: + plant.planner.mpc.last_solution_status = int(previous_solver_failure_fn(plant.current_time)) + seed_calls_before = len(seed_calls) + result = plant.step(v_lead=lead_speed, v_cruise=v_cruise) + controller = plant.planner.accel_controller + if len(seed_calls) > seed_calls_before: + planner_seed_accel, mpc_seed_accel = seed_calls[-1] + else: + planner_seed_accel = mpc_seed_accel = np.nan + lower = plant.planner.mpc.params[:, 0] + upper = plant.planner.mpc.params[:, 1] + raw_speed_ceiling = controller.lead_controller.raw_speed_ceiling + profile_limit = profile_accel_max(controller.profile, result["published_v_ego"]) if controller.is_active else math.inf + bounds_valid = (np.allclose(lower, ACCEL_MIN) and np.all(np.isfinite(upper)) + and np.all(upper >= lower) and np.all(upper <= ACCEL_MAX + 1e-9)) + rows.append(( + plant.current_time, result["speed"], result["distance"], result["distance_lead"], result["a_target"], + result["realized_acceleration"], result["should_stop"], result["fcw"], controller.is_active, + controller.launching, controller.departure_launching, controller.output_v_target, raw_speed_ceiling, controller.selected_lead, + controller.lead_controller.lead_recovery, profile_limit, controller.mpc_accel_max is not None, controller.state, + controller.required_decel, planner_seed_accel, + mpc_seed_accel, upper[0], np.min(upper), bounds_valid, plant.planner.mpc.last_solution_status, + )) + sources.append(result["mpc_source"]) + dec_modes.append(result["dec_mode"]) + finally: + plant.planner.mpc.reset = original_mpc_reset + plant.planner.mpc.set_cur_state = original_mpc_set_cur_state + + data = np.asarray(rows, dtype=float) + trace = ClosedLoopTrace( + time=data[:, 0], speed=data[:, 1], distance=data[:, 2], distance_lead=data[:, 3], a_target=data[:, 4], acceleration=data[:, 5], + should_stop=data[:, 6].astype(bool), fcw=data[:, 7].astype(bool), source=sources, dec_mode=dec_modes, + active=data[:, 8].astype(bool), launching=data[:, 9].astype(bool), departure_launching=data[:, 10].astype(bool), + target_speed=data[:, 11], raw_speed_ceiling=data[:, 12], selected_lead=data[:, 13].astype(int), lead_recovery=data[:, 14].astype(bool), + profile_accel_max=data[:, 15], accel_ceiling_active=data[:, 16].astype(bool), + state=data[:, 17].astype(int), required_decel=data[:, 18], planner_seed_accel=data[:, 19], mpc_seed_accel=data[:, 20], + mpc_upper_first=data[:, 21], mpc_upper_min=data[:, 22], stock_bounds_valid=data[:, 23].astype(bool), + solver_status=data[:, 24].astype(int), + solver_failures=solver_failures, solver_failure_times=solver_failure_times, + ) + gc.collect() + return trace + + +def _first_time_below(trace: ClosedLoopTrace, threshold: float, after: float = 0.0) -> float: + indices = np.flatnonzero((trace.time >= after) & (trace.a_target <= threshold)) + assert len(indices), f"never reached {threshold} m/s²" + return float(trace.time[indices[0]]) + + +def _sustained_time_below(trace: ClosedLoopTrace, threshold: float, *, after: float = 0.5, duration: float = 0.5) -> float: + required_frames = round(duration / DT_MDL) + below = (trace.time >= after) & (trace.a_target <= threshold) + sustained = np.convolve(below.astype(int), np.ones(required_frames, dtype=int), mode="valid") == required_frames + indices = np.flatnonzero(sustained) + assert len(indices), f"never sustained {threshold} m/s² for {duration} s" + return float(trace.time[indices[0]]) + + +def _command_jerk(trace: ClosedLoopTrace, after: float = 0.0) -> np.ndarray: + indices = np.flatnonzero(trace.time >= after) + assert len(indices) >= 2 + return np.diff(trace.a_target[indices]) / DT_MDL + + +def _filtered_realized_jerk(trace: ClosedLoopTrace, after: float = 1.0) -> np.ndarray: + filtered_acceleration = np.convolve(trace.acceleration, np.ones(3) / 3.0, mode="valid") + samples = trace.time[2:-1] >= after + return (np.diff(filtered_acceleration) / DT_MDL)[samples] + + +def _has_brake_coast_brake(values: np.ndarray, brake: float = -0.8, coast: float = -0.35, frames: int = 2) -> bool: + phase = 0 + for index in range(len(values) - frames + 1): + window = values[index:index + frames] + if np.all(window <= brake): + if phase == 2: + return True + phase = 1 + elif phase == 1 and np.all(window >= coast): + phase = 2 + return False + + +def _has_propulsion_after_braking(values: np.ndarray, propulsion: float = 0.2, brake: float = -0.2, frames: int = 2) -> bool: + braking = False + for index in range(len(values) - frames + 1): + window = values[index:index + frames] + if np.all(window <= brake): + braking = True + elif braking and np.all(window >= propulsion): + return True + return False + + +def _has_propulsion_brake_cycle(values: np.ndarray, propulsion: float = 0.2, brake: float = -0.2, frames: int = 2) -> bool: + phases = [] + for index in range(len(values) - frames + 1): + window = values[index:index + frames] + phase = 1 if np.all(window >= propulsion) else -1 if np.all(window <= brake) else 0 + if phase and (not phases or phase != phases[-1]): + phases.append(phase) + if len(phases) >= 3 and phases[-1] == phases[-3]: + return True + return False + + +def _assert_non_actuating_matches_stock(trace: ClosedLoopTrace, baseline: ClosedLoopTrace) -> None: + np.testing.assert_allclose(trace.a_target, baseline.a_target, atol=1e-6, rtol=0.0) + np.testing.assert_array_equal(trace.should_stop, baseline.should_stop) + np.testing.assert_array_equal(trace.fcw, baseline.fcw) + np.testing.assert_array_equal(trace.solver_status, baseline.solver_status) + assert trace.source == baseline.source + assert trace.solver_failures == baseline.solver_failures + assert trace.solver_failure_times == baseline.solver_failure_times + + +def _assert_no_new_solver_failures(trace: ClosedLoopTrace, baseline: ClosedLoopTrace) -> None: + assert trace.solver_failures <= baseline.solver_failures + + +@pytest.mark.parametrize( + "plant_kwargs", + [ + {"enabled": False, "lead_relevancy": True, "speed": 20.0, "distance_lead": 70.0}, + {"e2e": True, "lead_relevancy": False, "speed": 20.0}, + ], + ids=("disengaged", "e2e"), +) +def test_non_actuating_modes_match_stock(plant_kwargs): + common = dict(duration=2.0, v_lead=14.0, **plant_kwargs) + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + + _assert_non_actuating_matches_stock(trace, baseline) + assert not trace.active.any() + np.testing.assert_allclose(trace.mpc_upper_min, baseline.mpc_upper_min) + + +def test_disabled_profiles_are_identical(): + common = {"duration": 2.0, "controller_enabled": False, "lead_relevancy": True, "speed": 20.0, "distance_lead": 70.0, "v_lead": 14.0} + traces = [_run(profile=profile, **common) for profile in range(3)] + for trace in traces[1:]: + _assert_non_actuating_matches_stock(trace, traces[0]) + + +def test_planner_configures_controller_before_one_stock_mpc_solve(): + plant = Plant(enabled=True, lead_relevancy=False, speed=0.0, actuator_delay=0.15, actuator_lag=0.20) + _configure_plant(plant, enabled=True) + plant.step(v_lead=0.0, v_cruise=22.352) + calls = [] + mpc = plant.planner.mpc + controller_update = plant.planner.accel_controller.update + set_params = mpc.set_accel_controller_params + set_weights = mpc.set_weights + set_cur_state = mpc.set_cur_state + update = mpc.update + + def record_controller(radar_state, *args, **kwargs): + calls.append(("controller", radar_state, args, kwargs)) + return controller_update(radar_state, *args, **kwargs) + + def record(name, method): + def wrapped(*args, **kwargs): + calls.append((name, args, kwargs)) + return method(*args, **kwargs) + return wrapped + + plant.planner.accel_controller.update = record_controller + mpc.set_accel_controller_params = record("configure", set_params) + mpc.set_weights = record("weights", set_weights) + mpc.set_cur_state = record("state", set_cur_state) + mpc.update = record("update", update) + plant.step(v_lead=0.0, v_cruise=22.352) + + names = [call[0] for call in calls] + assert names == ["controller", "configure", "weights", "state", "update"] + assert calls[0][1] is calls[4][1][0] + assert calls[2][2]["personality"] == calls[4][2]["personality"] + + +@pytest.mark.parametrize("lead_relevancy", (False, True), ids=("clear-road", "lead")) +def test_force_decel_matches_stock(lead_relevancy): + common = {"duration": 2.0, "force_decel": True, "lead_relevancy": lead_relevancy, "speed": 20.0, + "distance_lead": 70.0, "v_lead": 14.0, "profile": 0} + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + _assert_non_actuating_matches_stock(trace, baseline) + assert not trace.active.any() + np.testing.assert_allclose(trace.mpc_upper_min, ACCEL_MAX) + + +@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport")) +def test_active_controller_uses_one_raw_mpc_solve_and_feasible_stock_bounds(profile): + trace = _run( + duration=4.0, controller_enabled=True, profile=profile, lead_relevancy=False, speed=0.0, + v_cruise=22.352, actuator_delay=0.15, actuator_lag=0.20, + ) + + assert trace.active.all() + assert trace.stock_bounds_valid.all() + np.testing.assert_allclose(trace.mpc_seed_accel, trace.planner_seed_accel, atol=1e-12, rtol=0.0) + assert np.all(trace.mpc_upper_first + 1e-9 >= trace.mpc_seed_accel) + assert np.all(trace.mpc_upper_min >= 0.0) + assert np.any(trace.mpc_upper_min < ACCEL_MAX - 0.05) + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize("lead_delay_frames", (0, 1), ids=("lead-present", "one-frame-lead-delay")) +def test_e2e_to_radar_acc_handoff_keeps_braking_continuous(lead_delay_frames): + transition_frame = round(2.0 / DT_MDL) + + def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None: + frame = round(current_time / DT_MDL) + return None if transition_frame <= frame < transition_frame + lead_delay_frames else truth + + def run_handoff(controller_enabled: bool): + plant = Plant( + lead_relevancy=True, speed=10.0, distance_lead=30.0, actuator_delay=0.15, actuator_lag=0.20, + model_action_fn=lambda current_time, _v_ego, _a_ego: (-1.0 if current_time < 2.0 else 0.0, False), + lead_observation_fn=observe, + ) + _configure_plant(plant, enabled=controller_enabled) + rows = [] + while plant.current_time < 2.4: + plant.e2e = plant.current_time < 2.0 + result = plant.step(v_lead=8.0, v_cruise=20.0) + rows.append((plant.current_time, result["a_target"], plant.planner.mpc.last_solution_status, + plant.planner.accel_controller.is_active)) + return np.asarray(rows, dtype=float).T + + baseline_time, baseline_accel, baseline_status, _ = run_handoff(False) + time_values, acceleration, solver_status, active = run_handoff(True) + np.testing.assert_allclose(time_values, baseline_time, atol=0.0, rtol=0.0) + transition = np.flatnonzero(time_values > 2.0)[0] + baseline_jump = abs(baseline_accel[transition] - baseline_accel[transition - 1]) + controlled_jump = abs(acceleration[transition] - acceleration[transition - 1]) + controlled_jerk = np.max(np.abs(np.diff(acceleration[transition:]) / DT_MDL)) + + assert controlled_jump <= min(baseline_jump + 1e-6, 0.10) + assert controlled_jerk <= 0.25 + assert np.count_nonzero(solver_status[transition:]) <= np.count_nonzero(baseline_status[transition:]) + assert active[transition] + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +@pytest.mark.parametrize("radar_fresh", (True, False), ids=("fresh-radar", "stale-radar")) +def test_positive_e2e_to_acc_handoff_starts_from_previous_plan(actuator_delay, actuator_lag, radar_fresh): + plant = Plant(lead_relevancy=False, speed=3.2, actuator_delay=actuator_delay, actuator_lag=actuator_lag) + _configure_plant(plant, enabled=True, profile=AccelProfile.eco) + plant.planner._update_radar_freshness = lambda _sm: radar_fresh + plant.e2e = False + plant.planner.output_a_target = 0.057 + plant.planner.a_desired = 1.032 + plant.planner.mpc_accel_seed = 1.032 + plant.planner.mpc.source = LongitudinalPlanSource.e2e + plant.planner.mpc.last_solution_status = 0 + plant.planner._long_active_last_cycle = True + + result = plant.step(v_lead=0.0, v_cruise=15.0) + + assert plant.planner.mpc_accel_seed == pytest.approx(0.057) + assert abs(result["a_target"] - 0.057) < 0.10 + assert plant.planner.mpc.last_solution_status == 0 + + +def test_clear_road_launch_is_prompt_and_profiles_separate_above_launch_speed(): + traces = [ + _run( + duration=12.0, controller_enabled=True, profile=profile, lead_relevancy=False, speed=0.0, + v_cruise=22.352, actuator_delay=0.15, actuator_lag=0.20, + ) + for profile in range(3) + ] + + for trace in traces: + positive = np.flatnonzero(trace.a_target > 0.05) + moving = np.flatnonzero(trace.speed > 0.01) + target_steps = np.diff(trace.target_speed) + release = int(np.argmax(target_steps)) + assert len(positive) and trace.time[positive[0]] <= 4 * DT_MDL + assert len(moving) and trace.time[moving[0]] <= 1.0 + assert np.interp(1.0, trace.time, trace.speed) >= 0.33 + assert target_steps[release] > TARGET_RELEASE_SLEW * DT_MDL + assert trace.time[release + 1] <= 0.5 + # The launch ramp has a bounded catch-up step when its speed ceiling is released. + assert abs(_command_jerk(trace)[release]) < 5.5 + assert abs(np.diff(trace.acceleration)[release] / DT_MDL) < 1.1 + assert not np.any(trace.a_target < -0.05) + assert not _has_propulsion_brake_cycle(trace.a_target) + assert trace.solver_failures == 0 + + launch_window = traces[0].time <= 0.5 + np.testing.assert_allclose(traces[1].a_target[launch_window], traces[0].a_target[launch_window], atol=0.10, rtol=0.0) + np.testing.assert_allclose(traces[2].a_target[launch_window], traces[0].a_target[launch_window], atol=0.10, rtol=0.0) + speed_at_eight = [float(np.interp(8.0, trace.time, trace.speed)) for trace in traces] + assert speed_at_eight[0] + 0.75 < speed_at_eight[1] + assert speed_at_eight[1] + 0.30 < speed_at_eight[2] + final_speed = [float(trace.speed[-1]) for trace in traces] + assert final_speed[0] + 1.25 < final_speed[1] + assert final_speed[1] + 0.75 < final_speed[2] + ceiling_at_ten = [float(np.interp(10.0, trace.speed, trace.mpc_upper_min)) for trace in traces] + assert ceiling_at_ten[0] < ceiling_at_ten[1] < ceiling_at_ten[2] + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_departure_launch_keeps_profile_ceiling_through_lead_recovery_transition(actuator_delay, actuator_lag): + trace = _run( + duration=12.0, controller_enabled=True, profile=AccelProfile.eco, lead_relevancy=True, speed=0.0, + distance_lead=6.0, v_lead=lambda t: 0.0 if t < 1.0 else min(7.0, 3.0 * (t - 1.0)), v_cruise=12.0, + actuator_delay=actuator_delay, actuator_lag=actuator_lag, lead_observation_fn=_tracked_lead, + ) + exits = np.flatnonzero(trace.launching[:-1] & ~trace.launching[1:] & trace.lead_recovery[1:]) + 1 + + assert len(exits) == 1 + exit_frame = exits[0] + response = slice(max(0, exit_frame - 5), min(len(trace.time), exit_frame + 15)) + departure = trace.departure_launching + assert np.count_nonzero(departure) > 1 + assert trace.accel_ceiling_active[departure].all() + assert np.min(trace.a_target[response]) > 0.0 + assert np.max(np.abs(np.diff(trace.a_target[response]) / DT_MDL)) < 3.0 + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_high_speed_lead_seed_release_has_no_target_snap(actuator_delay, actuator_lag): + dropout_time = 5.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if lead_name == "leadTwo" or current_time >= dropout_time else truth + + common = { + "duration": 7.0, "profile": AccelProfile.eco, "lead_relevancy": True, "speed": 22.0, + "distance_lead": 100.0, "v_lead": 20.0, "v_cruise": 30.0, "lead_observation_fn": observe, + "actuator_delay": actuator_delay, "actuator_lag": actuator_lag, + } + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + response = (trace.time >= dropout_time - 0.5) & (trace.time <= dropout_time + 2.0) + response_steps = (trace.time[1:] >= dropout_time) & (trace.time[1:] <= dropout_time + 2.0) + steps = np.diff(trace.target_speed) + release = np.flatnonzero(response_steps & (steps > 1e-6)) + + # This lead disappears after settling at a steady speed, rather than while actively restricting. + # so the trust register's short (persist-frames) pre-roll applies rather than the long dropout + # coast - see LeadController._dropout_was_restricting + assert len(release) and trace.time[release[0] + 1] <= dropout_time + LEAD_RELEASE_CONFIRM_TIME + DT_MDL + 1e-9 + assert np.max(steps[response_steps]) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9 + assert trace.target_speed[trace.time >= dropout_time + 2.0][0] == 30.0 + assert np.max(np.abs(_command_jerk(trace)[response_steps])) <= np.max(np.abs(_command_jerk(baseline)[response_steps])) + 1e-9 + release_response = response_steps.copy() + release_response[:release[0]] = False + assert np.max(np.abs(_command_jerk(trace)[release_response])) < 1.0 + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert not trace.fcw.any() and trace.solver_failures == 0 + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_route_537_lead_dropout_does_not_pulse_throttle_before_reacquisition(actuator_delay, actuator_lag): + dropout_start, brief_return, second_dropout, reacquisition = 5.0, 5.7, 5.9, 7.4 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + visible = current_time < dropout_start or brief_return <= current_time < second_dropout or current_time >= reacquisition + return truth if lead_name == "leadOne" and visible else None + + common = { + "duration": 10.0, "profile": AccelProfile.eco, "lead_relevancy": True, "speed": 22.0, + "distance_lead": 70.0, "v_lead": 18.0, "v_cruise": 30.0, "lead_observation_fn": observe, + "actuator_delay": actuator_delay, "actuator_lag": actuator_lag, + } + trace = _run(controller_enabled=True, **common) + dropout = (trace.time >= dropout_start) & (trace.time < reacquisition) + response = (trace.time >= dropout_start - 0.5) & (trace.time <= reacquisition + 2.0) + + # Allow the slowest actuator model to finish its monotonic release. + assert np.max(trace.a_target[dropout]) <= 0.21 + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.min(trace.distance_lead[response] - trace.distance[response]) > 5.0 * STOP_DISTANCE + assert not trace.fcw.any() and trace.solver_failures == 0 + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_route_554_should_coast_on_dropout_coasts_before_release(actuator_delay, actuator_lag): + dropout_time = 5.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return truth if lead_name == "leadOne" and current_time < dropout_time else None + + trace = _run( + duration=10.0, controller_enabled=True, profile=AccelProfile.eco, lead_relevancy=True, speed=10.0, + distance_lead=50.0, v_lead=6.0, v_cruise=18.0, lead_observation_fn=observe, + actuator_delay=actuator_delay, actuator_lag=actuator_lag, + ) + before = np.flatnonzero(trace.time < dropout_time)[-1] + coast = (trace.time >= dropout_time) & (trace.time <= dropout_time + LEAD_DROPOUT_COAST_TIME) + release = np.flatnonzero((trace.time > dropout_time + LEAD_DROPOUT_COAST_TIME) & (trace.state == int(AccelControllerState.release))) + + assert trace.source[before] == LongitudinalPlanSource.cruise + assert trace.state[before] == int(AccelControllerState.restrict) + assert np.max(trace.a_target[coast]) <= 0.0 + assert len(release) and trace.time[release[0]] <= dropout_time + LEAD_DROPOUT_COAST_TIME + 2 * DT_MDL + assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= dropout_time]) + assert not trace.fcw.any() and trace.solver_failures == 0 + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_route_575_should_coast_on_dropout_does_not_surge_before_reacquisition(monkeypatch, actuator_delay, actuator_lag): + dropout_start, reacquisition = 5.0, 6.5 + target_hold_frame = round(4.0 / DT_MDL) + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo" or dropout_start <= current_time < reacquisition: + return None + return truth + + original_update = accel_controller_module.AccelController.update + + def run(dropout_coast: bool) -> ClosedLoopTrace: + route_frame = 0 + + def hold_route_target(self, radar_state, *args, **kwargs): + nonlocal route_frame + original_update(self, radar_state, *args, **kwargs) + route_frame += 1 + if route_frame >= target_hold_frame: + self.lead_controller.target_speed = self.output_v_target = 10.5 + if not dropout_coast and not self.lead_controller.has_lead: + self.cruise_accel_max = None + + monkeypatch.setattr(accel_controller_module.AccelController, "update", hold_route_target) + return _run( + duration=8.0, controller_enabled=True, profile=AccelProfile.eco, lead_relevancy=True, speed=8.5, + distance_lead=35.0, v_lead=5.5, v_cruise=22.352, lead_observation_fn=observe, + actuator_delay=actuator_delay, actuator_lag=actuator_lag, + ) + + baseline = run(False) + trace = run(True) + dropout = (trace.time >= dropout_start) & (trace.time < reacquisition) + response = (trace.time >= dropout_start - DT_MDL) & (trace.time <= reacquisition + 0.5) + dropout_steps = (trace.time[1:] >= dropout_start) & (trace.time[1:] < reacquisition) + reacquire_steps = (trace.time[1:] >= reacquisition) & (trace.time[1:] <= reacquisition + 0.5) + baseline_braking = np.flatnonzero((baseline.time >= reacquisition) & (baseline.a_target < -0.05)) + braking = np.flatnonzero((trace.time >= reacquisition) & (trace.a_target < -0.05)) + + assert np.max(baseline.a_target[dropout]) > 0.10 + assert np.max(trace.a_target[dropout]) < 0.05 + assert np.max(np.abs(_command_jerk(trace)[dropout_steps])) < np.max(np.abs(_command_jerk(baseline)[dropout_steps])) + assert np.max(np.abs(_command_jerk(trace)[reacquire_steps])) <= np.max(np.abs(_command_jerk(baseline)[reacquire_steps])) + assert len(braking) and len(baseline_braking) and trace.time[braking[0]] <= baseline.time[baseline_braking[0]] + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.min(trace.distance_lead[response] - trace.distance[response]) >= np.min(baseline.distance_lead[response] - baseline.distance[response]) + assert not baseline.fcw.any() and baseline.solver_failures == 0 + assert not trace.fcw.any() and trace.solver_failures == 0 + + +@pytest.mark.parametrize( + ("speed", "v_cruise"), + ((0.0, 22.352), (25.0, 30.0), (35.0, 35.0)), +) +def test_decel_smoothing_does_not_change_clear_road_acceleration_at_representative_speeds(monkeypatch, speed, v_cruise): + common = { + "duration": 3.0, "controller_enabled": True, "profile": AccelProfile.sport, "lead_relevancy": False, + "speed": speed, "v_cruise": v_cruise, "actuator_delay": 0.15, "actuator_lag": 0.20, + } + monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_COST_MULTIPLIER", 1.0) + stock_weight = _run(**common) + monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_COST_MULTIPLIER", MPC_DECEL_JERK_COST_MULTIPLIER) + smoothed = _run(**common) + + np.testing.assert_allclose(smoothed.a_target, stock_weight.a_target, atol=1e-9, rtol=0.0) + np.testing.assert_allclose(smoothed.speed, stock_weight.speed, atol=1e-9, rtol=0.0) + np.testing.assert_allclose(smoothed.mpc_upper_min, stock_weight.mpc_upper_min, atol=1e-9, rtol=0.0) + assert smoothed.solver_failures == stock_weight.solver_failures == 0 + + +def test_lead_bound_routine_decel_uses_smoothing_without_delaying_initial_braking(monkeypatch): + def lead_speed(current_time: float) -> float: + return max(22.5, 25.0 - 0.4 * current_time) + + common = { + "duration": 8.0, "controller_enabled": True, "profile": AccelProfile.eco, "lead_relevancy": True, "speed": 29.0, + "distance_lead": 80.0, "v_lead": lead_speed, "v_cruise": 33.528, "actuator_delay": 0.15, "actuator_lag": 0.20, + } + monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_COST_MULTIPLIER", 1.0) + baseline = _run(**common) + monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_COST_MULTIPLIER", MPC_DECEL_JERK_COST_MULTIPLIER) + smoothed = _run(**common) + response = smoothed.time >= 0.5 + baseline_gap = baseline.distance_lead - baseline.distance + gap = smoothed.distance_lead - smoothed.distance + + assert set(np.asarray(smoothed.source)[response]) <= {LongitudinalPlanSource.lead0, LongitudinalPlanSource.lead1} + assert np.max(smoothed.required_decel[response]) < 0.80 + assert float(np.percentile(np.abs(_filtered_realized_jerk(smoothed)), 95)) < float(np.percentile(np.abs(_filtered_realized_jerk(baseline)), 95)) + assert float(np.percentile(np.abs(_command_jerk(smoothed, after=0.5)), 95)) < float(np.percentile(np.abs(_command_jerk(baseline, after=0.5)), 95)) + assert _first_time_below(smoothed, -0.2) <= _first_time_below(baseline, -0.2) + 1e-6 + # Allow one model frame of timing tolerance. + assert _first_time_below(smoothed, -0.5) <= _first_time_below(baseline, -0.5) + 0.25 + DT_MDL + 1e-6 + assert np.min(gap) >= np.min(baseline_gap) - 0.25 + assert np.max(np.abs(_command_jerk(smoothed, after=0.5))) < 3.0 + assert not _has_propulsion_brake_cycle(smoothed.a_target[response]) + assert not smoothed.fcw.any() + assert smoothed.solver_failures == 0 + + +def test_tightening_lead_releases_smoothing_before_late_catchup(monkeypatch): + event_time = 3.0 + lead_jerk = 1.02 + max_lead_decel = 2.22 + ramp_time = max_lead_decel / lead_jerk + + def lead_speed(current_time: float) -> float: + braking_time = max(current_time - event_time, 0.0) + ramp = min(braking_time, ramp_time) + return 16.9 - 0.5 * lead_jerk * ramp**2 - max_lead_decel * max(braking_time - ramp_time, 0.0) + + common = { + "duration": 7.0, "profile": AccelProfile.eco, "lead_relevancy": True, "speed": 15.9, "distance_lead": 35.7, + "v_lead": lead_speed, "v_cruise": 17.4, "actuator_delay": 0.15, "actuator_lag": 0.20, + } + stock = _run(controller_enabled=False, **common) + monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", np.inf) + always_smoothed = _run(controller_enabled=True, **common) + monkeypatch.setattr(accel_controller_module, "MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE", MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE) + trace = _run(controller_enabled=True, **common) + response = trace.time >= event_time + response_jerk = trace.time[1:] >= event_time + required_decel_rate = (trace.required_decel[3:] - trace.required_decel[:-3]) / (3 * DT_MDL) + gap = trace.distance_lead - trace.distance + always_smoothed_gap = always_smoothed.distance_lead - always_smoothed.distance + + assert np.max(required_decel_rate[trace.required_decel[3:] >= 0.15]) > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE + assert _first_time_below(trace, -0.5) <= _first_time_below(stock, -0.5) + 1e-9 + # Releasing smoothing must not brake later than keeping it enabled. + assert _first_time_below(trace, -0.5) <= _first_time_below(always_smoothed, -0.5) + 1e-9 + assert np.max(np.abs(np.diff(trace.a_target)[response_jerk] / DT_MDL)) < 3.0 + assert not _has_brake_coast_brake(trace.a_target[response]) + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.min(gap) >= np.min(always_smoothed_gap) - 1e-6 + assert not stock.fcw.any() and not always_smoothed.fcw.any() and not trace.fcw.any() + assert stock.solver_failures == always_smoothed.solver_failures == trace.solver_failures == 0 + + +def test_same_slot_track_replacement_never_delays_tightening_braking_for_lead(): + event_time = 3.0 + lead_jerk = 1.02 + max_lead_decel = 2.22 + ramp_time = max_lead_decel / lead_jerk + + def lead_speed(current_time: float) -> float: + braking_time = max(current_time - event_time, 0.0) + ramp = min(braking_time, ramp_time) + return 16.9 - 0.5 * lead_jerk * ramp**2 - max_lead_decel * max(braking_time - ramp_time, 0.0) + + def observe(switch_time: float | None): + def observation(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + track_id = 200 if switch_time is not None and current_time >= switch_time else 100 + return truth | {"radar": True, "radarTrackId": track_id} + return observation + + common = { + "duration": 8.0, "controller_enabled": True, "profile": AccelProfile.eco, "lead_relevancy": True, "speed": 15.9, "distance_lead": 35.7, + "v_lead": lead_speed, "v_cruise": 17.4, "actuator_delay": 0.15, "actuator_lag": 0.20, + } + baseline = _run(lead_observation_fn=observe(None), **common) + history = MPC_DECEL_TREND_FRAMES - 1 + rate = np.full_like(baseline.required_decel, -math.inf) + rate[history:] = (baseline.required_decel[history:] - baseline.required_decel[:-history]) / (history * DT_MDL) + candidates = np.flatnonzero( + (baseline.time >= event_time) + & (baseline.state == int(AccelControllerState.restrict)) + & (baseline.required_decel > 0.0) + & (baseline.required_decel < MPC_DECEL_JERK_MAX_REQUIRED_DECEL) + & (rate > MPC_DECEL_JERK_MAX_REQUIRED_DECEL_RATE) + & (baseline.a_target > -1.0) + ) + assert len(candidates) + switch_time = float(baseline.time[candidates[0]]) + switched = _run(lead_observation_fn=observe(switch_time), **common) + response = switched.time >= switch_time + baseline_gap = baseline.distance_lead - baseline.distance + switched_gap = switched.distance_lead - switched.distance + + def minimum_ttc(trace: ClosedLoopTrace, gap: np.ndarray) -> float: + closing_speed = trace.speed - np.asarray([lead_speed(current_time) for current_time in trace.time]) + closing = closing_speed > 0.1 + assert closing.any() + return float(np.min(gap[closing] / closing_speed[closing])) + + assert np.all(baseline.selected_lead[response] == 0) + assert np.all(switched.selected_lead[response] == 0) + for threshold in (-1.0, -2.0): + assert _first_time_below(switched, threshold) <= _first_time_below(baseline, threshold) + 1e-9 + assert np.min(switched_gap) >= np.min(baseline_gap) - 0.02 + assert minimum_ttc(switched, switched_gap) >= minimum_ttc(baseline, baseline_gap) - 0.02 + assert np.min(switched_gap) > 0.0 + assert not switched.fcw.any() + assert baseline.solver_failures == switched.solver_failures == 0 + + +def test_prius_route_model_launches_without_a_dead_pedal(): + trace = _run( + duration=3.0, controller_enabled=True, profile=1, lead_relevancy=False, speed=0.0, + v_cruise=22.352, actuator_model=PRIUS_TSS2_ROUTE_MODEL, + ) + positive = np.flatnonzero(trace.a_target > 0.05) + moving = np.flatnonzero(trace.speed > 0.05) + assert len(positive) and trace.time[positive[0]] <= 4 * DT_MDL + assert len(moving) and trace.time[moving[0]] <= 1.0 + assert trace.solver_failures == 0 + + +def test_stop_hold_survives_short_full_field_dropout(): + def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if 1.0 <= current_time < 1.1 else truth + + common = { + "duration": 2.0, "controller_enabled": True, "lead_relevancy": True, "speed": 0.0, "distance_lead": 6.0, + "v_lead": 0.0, "v_cruise": 8.0, "lead_observation_fn": observe, "actuator_delay": 0.15, "actuator_lag": 0.20, + } + baseline = _run(**(common | {"controller_enabled": False})) + trace = _run(**common) + assert np.max(trace.speed) < 1e-3 + assert np.all(trace.target_speed == 0.0) + assert np.all(trace.state == int(AccelControllerState.stopHold)) + assert not trace.fcw.any() + _assert_no_new_solver_failures(trace, baseline) + + +@pytest.mark.parametrize("replacement_track_id", (100, 200), ids=("same-track", "replacement")) +def test_stop_hold_rejects_persistent_same_slot_range_step(replacement_track_id): + step_time = 1.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + stepped = current_time >= step_time + speed = 0.2 if stepped else 0.0 + return truth | {"dRel": truth["dRel"] + 0.4 * stepped, "vLead": speed, "vLeadK": speed, "vRel": speed, + "aLeadK": 0.0, "radarTrackId": replacement_track_id if stepped else 100, "radar": True} + + trace = _run( + duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=0.0, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + + assert np.all(trace.state == int(AccelControllerState.stopHold)) + assert np.all(trace.target_speed == 0.0) + assert trace.should_stop.all() + assert np.max(trace.speed) < 1e-3 + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_stop_hold_rejects_fast_lead_speed_without_range_growth(): + noise_start = 1.0 + distances = (6.000, 6.004, 6.007, 6.010) + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + frame = round((current_time - noise_start) / DT_MDL) + if 0 <= frame < len(distances): + return truth | {"dRel": distances[frame], "vLead": 1.0, "vLeadK": 1.0, "vRel": 1.0, + "aLeadK": 0.0, "radarTrackId": 100, "radar": True} + return truth | {"aLeadK": 0.0, "radarTrackId": 100, "radar": True} + + trace = _run( + duration=2.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=0.0, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + + assert np.all(trace.state == int(AccelControllerState.stopHold)) + assert np.all(trace.target_speed == 0.0) + assert np.max(trace.speed) < 1e-3 + assert trace.should_stop.all() + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_filtered_lead_speed_cannot_override_stationary_raw_lead(): + glitch_start = 1.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + frame = max(0, round((current_time - glitch_start) / DT_MDL)) + if current_time >= glitch_start: + return truth | {"dRel": 6.0 + 0.01 * frame, "vLead": 0.0, "vLeadK": 1.0, "vRel": 0.0, + "aLeadK": 0.0, "radarTrackId": 100, "radar": True} + return truth | {"aLeadK": 0.0, "radarTrackId": 100, "radar": True} + + trace = _run( + duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=0.0, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + + assert np.all(trace.state == int(AccelControllerState.stopHold)) + assert np.all(trace.target_speed == 0.0) + assert trace.should_stop.all() + assert np.max(trace.speed) < 1e-3 + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_disappearing_stopped_lead_releases_smoothly(actuator_delay, actuator_lag): + departure_time = 1.0 + + def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return truth if current_time < departure_time else None + + trace = _run( + duration=3.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=0.0, v_cruise=8.0, lead_observation_fn=observe, + actuator_delay=actuator_delay, actuator_lag=actuator_lag, + ) + released = np.flatnonzero((trace.time >= departure_time) & (trace.target_speed > 0.0)) + moving = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05)) + confirming = (trace.time >= departure_time) & (trace.time < departure_time + LEAD_RELEASE_CONFIRM_TIME - DT_MDL) + assert len(released) and len(moving) + assert np.all(trace.target_speed[confirming] == 0.0) + + target_steps = np.diff(trace.target_speed[released[0]:]) + assert np.all(target_steps >= -1e-9) + assert np.max(target_steps) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9 + assert trace.time[moving[0]] <= departure_time + LEAD_RELEASE_CONFIRM_TIME + 0.9 + assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time]) + assert not _has_brake_coast_brake(trace.a_target[trace.time >= departure_time]) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_moving_departure_crossing_exit_speed_releases_once(): + departure_time = 1.0 + speeds = (0.81, 0.82, 0.83, 0.84, 0.79, 0.76, 0.74, 0.72) + + def lead_speed(current_time: float) -> float: + frame = round((current_time - departure_time) / DT_MDL) + return 0.0 if frame < 0 else speeds[min(frame, len(speeds) - 1)] + + def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if lead_name == "leadTwo" else truth | {"aLeadK": 0.0, "radarTrackId": 100, "radar": True} + + trace = _run( + duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + after_departure = trace.time >= departure_time + stop_hold = int(AccelControllerState.stopHold) + releases = np.flatnonzero((trace.state[:-1] == stop_hold) & (trace.state[1:] != stop_hold)) + 1 + + assert len(releases) == 1 + assert trace.launching[releases[0]] + assert not np.any(trace.state[releases[0]:] == stop_hold) + assert np.count_nonzero(np.diff(trace.should_stop[after_departure].astype(int))) == 1 + assert not _has_propulsion_brake_cycle(trace.a_target[after_departure]) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_route_51d_duplicate_lead_speed_pulse_cannot_release_stop_hold(): + pulse_start = 1.0 + departure_time = 2.0 + pulse_speeds = (0.1361, 0.1731, 0.2146, 0.2253, 0.2137, 0.1877) + pulse_distances = (6.0, 6.0, 6.0, 5.96, 6.04, 6.04) + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 2.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation: + result = truth | {"radar": True, "radarTrackId": 4887 if lead_name == "leadOne" else 4905} + pulse_frame = round((current_time - pulse_start) / DT_MDL) + if 0 <= pulse_frame < len(pulse_speeds): + speed = pulse_speeds[pulse_frame] if lead_name == "leadOne" else 0.0 + distance = pulse_distances[pulse_frame] if lead_name == "leadOne" else 6.08 + result |= {"dRel": distance, "vLead": speed, "vLeadK": speed, "vRel": speed, "aLeadK": 0.0} + return result + + trace = _run( + duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + pulse = (trace.time >= pulse_start) & (trace.time < pulse_start + len(pulse_speeds) * DT_MDL) + launched = np.flatnonzero((trace.time >= departure_time) & trace.launching) + + assert np.all(trace.state[pulse] == int(AccelControllerState.stopHold)) + assert np.all(trace.target_speed[pulse] == 0.0) + assert np.max(trace.speed[pulse]) < 0.01 + assert len(launched) and trace.time[launched[0]] <= departure_time + STOP_HOLD_EXIT_FRAMES * DT_MDL + 1e-9 + assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time]) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_route_520_slow_lead_pulse_cannot_release_stop_hold_or_dampen_real_departure(): + pulse_start = 1.0 + departure_time = 2.5 + pulse_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) + pulse_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) + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 2.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + pulse_frame = round((current_time - pulse_start) / DT_MDL) + if 0 <= pulse_frame < len(pulse_speeds): + speed = pulse_speeds[pulse_frame] + return truth | {"dRel": 6.0 + pulse_offsets[pulse_frame], "vLead": speed, "vLeadK": speed, "vRel": speed, + "aLeadK": 0.0, "radarTrackId": 2133, "radar": True} + return truth | {"aLeadK": 0.0, "radarTrackId": 2133, "radar": True} + + common = { + "duration": 4.0, "lead_relevancy": True, "speed": 0.0, "distance_lead": 6.0, "v_lead": lead_speed, "v_cruise": 8.0, + "lead_observation_fn": observe, "actuator_model": PRIUS_TSS2_ROUTE_MODEL, + } + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + pulse = (trace.time >= pulse_start) & (trace.time < pulse_start + len(pulse_speeds) * DT_MDL) + release = np.flatnonzero((trace.time >= departure_time) & trace.launching) + + assert np.all(trace.state[pulse] == int(AccelControllerState.stopHold)) + assert np.all(trace.target_speed[pulse] == 0.0) + assert np.max(trace.speed[trace.time < departure_time]) < 0.01 + assert len(release) and trace.time[release[0]] <= departure_time + STOP_HOLD_EXIT_FRAMES * DT_MDL + 1e-9 + assert trace.a_target[release[0]] > 0.05 + assert np.allclose(trace.a_target[release[0]:], baseline.a_target[release[0]:], atol=1e-5, rtol=0.0) + assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time]) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_stopped_lead_requires_four_departure_frames_and_launches_within_one_second(actuator_delay, actuator_lag): + departure_time = 1.0 + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 2.0 + + trace = _run( + duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lead_speed, v_cruise=8.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag, + ) + first_three = (trace.time > departure_time) & (trace.time <= departure_time + 3 * DT_MDL + 1e-9) + release = np.flatnonzero((trace.time >= departure_time) & trace.launching) + moving = np.flatnonzero((trace.time >= departure_time) & (trace.speed > 0.05)) + + assert not trace.launching[first_three].any() + assert trace.should_stop[first_three].all() + assert len(release) and trace.time[release[0]] >= departure_time + 3 * DT_MDL + assert not trace.should_stop[release[0]] + assert len(moving) and trace.time[moving[0]] <= departure_time + 3 * DT_MDL + 1.0 + assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= departure_time]) + assert trace.solver_failures == 0 + + +def test_stop_hold_departure_survives_radar_staleness(): + departure_time = 1.0 + dropout_start_time = 1.3 + dropout_len_frames = round(0.8 / DT_MDL) + dropout_start_frame = round(dropout_start_time / DT_MDL) + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 2.0 + + def radar_fresh_fn(frame: int) -> bool: + return not (dropout_start_frame <= frame < dropout_start_frame + dropout_len_frames) + + common = { + "duration": 4.0, "controller_enabled": True, "lead_relevancy": True, "speed": 0.0, "distance_lead": 6.0, + "v_lead": lead_speed, "v_cruise": 8.0, "actuator_delay": 0.15, "actuator_lag": 0.25, + "lead_observation_fn": _tracked_lead, + } + baseline = _run(**common) + trace = _run(radar_fresh_fn=radar_fresh_fn, **common) + just_before_dropout = (trace.time >= dropout_start_time - 2 * DT_MDL) & (trace.time < dropout_start_time) + after_recovery = trace.time >= dropout_start_time + dropout_len_frames * DT_MDL + gap = trace.distance_lead - trace.distance + baseline_gap = baseline.distance_lead - baseline.distance + + assert trace.departure_launching[just_before_dropout].all() + assert not _has_propulsion_brake_cycle(trace.a_target) + assert not _has_brake_coast_brake(trace.a_target) + assert np.max(np.abs(np.diff(trace.a_target) / DT_MDL)) < 3.0 + assert np.min(gap) >= np.min(baseline_gap) - DROPOUT_GAP_TOLERANCE + assert not trace.fcw.any() + _assert_no_new_solver_failures(trace, baseline) + assert trace.solver_failures == 0 + assert trace.launching[after_recovery].any() + assert trace.time[np.flatnonzero(after_recovery & trace.launching)[0]] <= dropout_start_time + dropout_len_frames * DT_MDL + 1.0 + + +def test_confirmed_departure_full_field_dropout_does_not_worsen_stock_response(): + departure_time = 1.0 + dropout_start = 1.3 + dropout_end = 1.45 + dropped: list[tuple[int, str]] = [] + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 2.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + if dropout_start <= current_time < dropout_end: + dropped.append((round(current_time / DT_MDL), lead_name)) + return None + return truth | {"radar": True, "radarTrackId": 100} + + common = { + "duration": 4.0, "lead_relevancy": True, "speed": 0.0, "distance_lead": 6.0, "v_lead": lead_speed, + "v_cruise": 8.0, "actuator_delay": 0.15, "actuator_lag": 0.25, "lead_observation_fn": observe, + } + baseline = _run(controller_enabled=False, **common) + dropped.clear() + trace = _run(controller_enabled=True, **common) + before_dropout = (trace.time >= dropout_start - 2 * DT_MDL) & (trace.time < dropout_start) + dropout = (trace.time > dropout_start) & (trace.time <= dropout_end) + jerk_window = (trace.time[1:] > dropout_start) & (trace.time[1:] <= dropout_end + 0.25) + dropped_by_frame: dict[int, set[str]] = {} + for frame, lead_name in dropped: + dropped_by_frame.setdefault(frame, set()).add(lead_name) + + assert trace.departure_launching[before_dropout].all() + assert len(dropped_by_frame) == round((dropout_end - dropout_start) / DT_MDL) + assert all(lead_names == {"leadOne"} for lead_names in dropped_by_frame.values()) + assert np.all(trace.selected_lead[dropout] == -1) and np.all(np.isinf(trace.raw_speed_ceiling[dropout])) + assert not np.any(trace.state[dropout] == int(AccelControllerState.stopHold)) + assert not _has_propulsion_brake_cycle(trace.a_target) and not _has_brake_coast_brake(trace.a_target) + assert np.max(np.abs(np.diff(trace.a_target)[jerk_window] / DT_MDL)) <= np.max(np.abs(np.diff(baseline.a_target)[jerk_window] / DT_MDL)) + 0.01 + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - DROPOUT_GAP_TOLERANCE + assert not trace.fcw.any() + np.testing.assert_array_equal(trace.solver_status, baseline.solver_status) + assert trace.solver_failure_times == baseline.solver_failure_times + + +def test_reused_radar_frames_do_not_pulse_stop_state_during_departure(): + departure_time = 1.0 + trace = _run( + duration=3.0, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lambda current_time: 0.0 if current_time < departure_time else 2.0, + v_cruise=8.0, actuator_delay=0.15, actuator_lag=0.25, radar_fresh_fn=lambda frame: frame % 2 == 0, + ) + after_departure = trace.time >= departure_time + should_stop = trace.should_stop[after_departure] + release = np.flatnonzero(after_departure & trace.launching) + moving = np.flatnonzero(after_departure & (trace.speed > 0.05)) + + assert np.count_nonzero(np.diff(should_stop.astype(int))) <= 1 + assert len(release) and len(moving) + assert trace.time[moving[0]] <= trace.time[release[0]] + 1.0 + assert not _has_propulsion_brake_cycle(trace.a_target[after_departure]) + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize("departure_frames", [1, 2, 3]) +def test_short_false_departure_does_not_launch_the_vehicle(departure_frames): + trace = _run( + duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lambda current_time: 2.0 if 1.0 <= current_time < 1.0 + departure_frames * DT_MDL else 0.0, + v_cruise=8.0, actuator_delay=0.10, actuator_lag=0.20, + ) + + assert np.max(trace.speed) < 0.01 + assert not trace.launching.any() + assert trace.should_stop.all() + assert trace.state[-1] == int(AccelControllerState.stopHold) + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_lead_recovery_recovery_preserves_profile_ordering(actuator_delay, actuator_lag): + traces = [ + _run( + duration=32.0, controller_enabled=True, profile=profile, lead_relevancy=True, speed=20.0, + distance_lead=100.0, v_lead=10.0, v_cruise=30.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag, + ) + for profile in range(3) + ] + response = (traces[0].time >= 15.0) & (traces[0].time <= 28.5) + mean_accel = [float(np.mean(trace.a_target[response])) for trace in traces] + final_speed = [float(trace.speed[np.flatnonzero(response)[-1]]) for trace in traces] + + assert mean_accel[0] + 0.06 < mean_accel[1] + assert mean_accel[1] + 0.025 < mean_accel[2] + assert final_speed[0] < final_speed[1] < final_speed[2] + assert max(final_speed) < 13.5 + assert all(not _has_propulsion_brake_cycle(trace.a_target[response]) for trace in traces) + assert all(trace.solver_failures == 0 for trace in traces) + + +def test_creeping_lead_departure_is_prompt_and_safe(): + departure_time = 1.0 + + def lead_speed(current_time: float) -> float: + if current_time < departure_time: + return 0.0 + if current_time < departure_time + 0.5: + return 1.6 * (current_time - departure_time) + return min(2.5, 0.8 + 0.7 * (current_time - departure_time - 0.5)) + + def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if lead_name == "leadTwo" else truth | {"aLeadK": 0.0, "radarTrackId": 2133, "radar": True} + + common = { + "duration": 6.0, "profile": 0, "lead_relevancy": True, "speed": 0.0, "distance_lead": 3.6, "v_lead": lead_speed, + "v_cruise": 22.352, "lead_observation_fn": observe, "actuator_delay": 0.15, "actuator_lag": 0.20, + } + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + after_departure = trace.time >= departure_time + lead_speeds = np.array([lead_speed(max(0.0, current_time - DT_MDL)) for current_time in trace.time]) + baseline_moving = np.flatnonzero((baseline.time >= departure_time) & (baseline.speed > 0.05)) + moving = np.flatnonzero(after_departure & (trace.speed > 0.05)) + + assert len(baseline_moving) and len(moving) + assert trace.time[moving[0]] <= baseline.time[baseline_moving[0]] + assert np.all(trace.speed[after_departure] <= lead_speeds[after_departure] + 0.20) + assert not _has_brake_coast_brake(trace.a_target[after_departure]) + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - 1e-3 + assert trace.solver_failures == 0 + + +def test_constant_creep_departure_does_not_pulse_between_launch_and_stop_hold(): + departure_time = 1.0 + + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < departure_time else 0.2 + + def observe(_current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if lead_name == "leadTwo" else truth | {"aLeadK": 0.0, "radarTrackId": 2133, "radar": True} + + trace = _run( + duration=8.0, controller_enabled=True, profile=0, lead_relevancy=True, speed=0.0, distance_lead=3.6, + v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + launched = np.flatnonzero((trace.time >= departure_time) & trace.launching) + assert len(launched) + assert trace.time[launched[0]] <= departure_time + 2.0 + after_launch = slice(launched[0], None) + + assert not np.any(trace.state[after_launch] == int(AccelControllerState.stopHold)) + assert not _has_propulsion_brake_cycle(trace.a_target[after_launch]) + assert np.max(trace.speed[after_launch]) <= 0.4 + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_invalid_departure_geometry_aborts_launch_until_reconfirmed(): + def lead_speed(current_time: float) -> float: + return 0.0 if current_time < 1.0 else 2.0 + + def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation: + return truth | {"vLeadK": -2.0} if 1.45 <= current_time < 1.70 else truth + + trace = _run( + duration=2.5, controller_enabled=True, lead_relevancy=True, speed=0.0, distance_lead=6.0, + v_lead=lead_speed, v_cruise=8.0, lead_observation_fn=observe, actuator_delay=0.15, actuator_lag=0.20, + ) + invalid = (trace.time >= 1.50) & (trace.time < 1.75) + assert invalid.any() + assert not trace.launching[invalid].any() + assert np.max(trace.speed[invalid]) < 0.10 + assert np.max(trace.target_speed[invalid]) == 0.0 + assert np.all(np.isfinite(trace.a_target)) + assert trace.solver_failures == 0 + + +def test_moving_full_field_dropout_never_releases_speed_or_adds_solver_failures(): + dropout_start = 2.0 + dropout_end = 2.15 + + def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation | None: + return None if dropout_start <= current_time < dropout_end else truth + + common = { + "duration": 4.0, "lead_relevancy": True, "speed": 22.0, "distance_lead": 85.0, "v_lead": 14.0, "v_cruise": 30.0, + "lead_observation_fn": observe, "actuator_delay": 0.20, "actuator_lag": 0.25, + } + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + before = trace.target_speed[np.flatnonzero(trace.time < dropout_start)[-1]] + response = (trace.time >= dropout_start) & (trace.time <= dropout_end + 0.5) + + assert np.max(trace.target_speed[response]) <= before + 1e-6 + assert not _has_propulsion_after_braking(trace.a_target[response]) + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - DROPOUT_GAP_TOLERANCE + _assert_no_new_solver_failures(trace, baseline) + + +def test_false_range_relief_matches_clean_controller_response(): + glitch_start = 3.0 + glitch_end = 3.15 + + def observe(current_time: float, _lead_name: str, truth: LeadObservation) -> LeadObservation: + return truth | {"dRel": truth["dRel"] + 5.0} if glitch_start <= current_time < glitch_end else truth + + common = { + "duration": 4.0, "lead_relevancy": True, "speed": 22.0, "distance_lead": 85.0, "v_lead": 14.0, + "v_cruise": 30.0, "actuator_delay": 0.20, "actuator_lag": 0.25, + } + baseline = _run(controller_enabled=False, lead_observation_fn=observe, **common) + clean = _run(controller_enabled=True, **common) + trace = _run(controller_enabled=True, lead_observation_fn=observe, **common) + response = (trace.time >= glitch_start) & (trace.time <= glitch_end + 0.5) + jerk_response = (trace.time[1:] >= glitch_start) & (trace.time[1:] <= glitch_end + 0.5) + + assert np.max(np.abs(trace.a_target[response] - clean.a_target[response])) < 0.07 + assert np.max(np.abs(np.diff(trace.a_target)[jerk_response] / DT_MDL)) < 3.0 + assert not _has_propulsion_after_braking(trace.a_target[response]) + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + _assert_no_new_solver_failures(trace, baseline) + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_route_52f_radar_vision_switch_does_not_release_restricted_pace(actuator_delay, actuator_lag): + glitch_start = 20.0 + glitch_end = 24.0 + + def lead_speed(current_time: float) -> float: + return 25.0 if current_time < glitch_end else min(33.0, 25.0 + 2.0 * (current_time - glitch_end)) + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + if not glitch_start <= current_time < glitch_end: + return truth | {"radar": True, "radarTrackId": 1119} + + phase = int((current_time - glitch_start) / 0.20) + if phase % 2 == 0: + return truth | {"vLeadK": truth["vLeadK"] - 0.3, "vRel": truth["vRel"] - 0.3, + "radar": True, "radarTrackId": 1119 if phase % 4 == 0 else 1176} + distance_offset = (15.0, 30.0, 60.0)[phase % 3] + return truth | {"dRel": truth["dRel"] + distance_offset, "vLead": truth["vLead"] + 1.0, + "vLeadK": truth["vLeadK"] + 1.0, "vRel": truth["vRel"] + 1.0, + "radar": False, "radarTrackId": -1} + + common = { + "duration": 28.0, "controller_enabled": True, "profile": AccelProfile.eco, "lead_relevancy": True, "speed": 28.0, + "distance_lead": 40.0, "v_lead": lead_speed, "v_cruise": 34.72, "actuator_delay": actuator_delay, "actuator_lag": actuator_lag, + } + baseline = _run(**common) + trace = _run(lead_observation_fn=observe, **common) + before = trace.target_speed[np.flatnonzero(trace.time < glitch_start)[-1]] + glitch = (trace.time >= glitch_start) & (trace.time < glitch_end) + response = (trace.time >= glitch_start - 0.5) & (trace.time <= glitch_end + 1.0) + recovered = (trace.time >= glitch_end + 0.8) & (trace.time <= glitch_end + 2.0) + gap = trace.distance_lead - trace.distance + baseline_gap = baseline.distance_lead - baseline.distance + glitch_sources = {trace.source[index] for index in np.flatnonzero(glitch)} + + assert {LongitudinalPlanSource.cruise, LongitudinalPlanSource.lead0} <= glitch_sources + assert np.max(trace.target_speed[glitch]) <= before + 0.05 + assert np.max(trace.target_speed[recovered]) > before + 0.1 + assert np.max(np.diff(trace.target_speed)[response[1:]]) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9 + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0 + assert np.min(gap[response]) >= np.min(baseline_gap[response]) - DROPOUT_GAP_TOLERANCE + assert not trace.fcw.any() + _assert_no_new_solver_failures(trace, baseline) + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_route_533_same_track_relief_has_no_target_snap(actuator_delay, actuator_lag): + glitch_start = 20.0 + glitch_end = 24.0 + + def lead_speed(current_time: float) -> float: + return 25.0 if current_time < glitch_end else min(33.0, 25.0 + 2.0 * (current_time - glitch_end)) + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + observed = truth | {"radar": True, "radarTrackId": 1119} + if not glitch_start <= current_time < glitch_end: + return observed + phase = int((current_time - glitch_start) / 0.20) + if phase % 2 == 0: + return observed | {"vLeadK": truth["vLeadK"] - 0.3, "vRel": truth["vRel"] - 0.3} + return observed | {"dRel": truth["dRel"] + (8.0, 12.0, 18.0)[phase % 3], "vLead": truth["vLead"] + 0.5, + "vLeadK": truth["vLeadK"] + 0.5, "vRel": truth["vRel"] + 0.5} + + common = { + "duration": 28.0, "controller_enabled": True, "profile": AccelProfile.eco, "lead_relevancy": True, "speed": 28.0, + "distance_lead": 40.0, "v_lead": lead_speed, "v_cruise": 34.72, "actuator_delay": actuator_delay, "actuator_lag": actuator_lag, + } + clean = _run(**common) + trace = _run(lead_observation_fn=observe, **common) + glitch = (trace.time >= glitch_start) & (trace.time < glitch_end) + response = (trace.time >= glitch_start - 0.5) & (trace.time <= glitch_end + 1.0) + clean_gap = clean.distance_lead - clean.distance + gap = trace.distance_lead - trace.distance + glitch_sources = {trace.source[index] for index in np.flatnonzero(glitch)} + + assert {LongitudinalPlanSource.cruise, LongitudinalPlanSource.lead0} <= glitch_sources + assert np.max(np.diff(trace.target_speed)[response[1:]]) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9 + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert not _has_brake_coast_brake(trace.a_target[response]) + assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0 + assert float(np.percentile(np.abs(_filtered_realized_jerk(trace)), 95)) <= float(np.percentile(np.abs(_filtered_realized_jerk(clean)), 95)) + 0.02 + assert np.min(gap[response]) >= np.min(clean_gap[response]) - DROPOUT_GAP_TOLERANCE + assert not trace.fcw.any() + _assert_no_new_solver_failures(trace, clean) + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_route_532_sustained_switch_churn_has_no_target_snap(actuator_delay, actuator_lag): + glitch_start = 20.0 + glitch_end = 32.0 + + def lead_speed(current_time: float) -> float: + return 25.0 if current_time < glitch_end else min(33.0, 25.0 + 2.0 * (current_time - glitch_end)) + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + if not glitch_start <= current_time < glitch_end: + return truth | {"radar": True, "radarTrackId": 1119} + phase = int((current_time - glitch_start) / 0.20) + if phase % 2 == 0: + return truth | {"vLeadK": truth["vLeadK"] - 0.3, "vRel": truth["vRel"] - 0.3, + "radar": True, "radarTrackId": 1119 if phase % 4 == 0 else 1176} + return truth | {"dRel": truth["dRel"] + (15.0, 30.0, 60.0)[phase % 3], "vLead": truth["vLead"] + 1.0, + "vLeadK": truth["vLeadK"] + 1.0, "vRel": truth["vRel"] + 1.0, "radar": False, "radarTrackId": -1} + + common = { + "duration": 36.0, "profile": AccelProfile.eco, "lead_relevancy": True, "speed": 28.0, + "distance_lead": 40.0, "v_lead": lead_speed, "v_cruise": 34.72, "actuator_delay": actuator_delay, "actuator_lag": actuator_lag, + } + baseline = _run(controller_enabled=False, lead_observation_fn=observe, **common) + trace = _run(controller_enabled=True, lead_observation_fn=observe, **common) + before = trace.target_speed[np.flatnonzero(trace.time < glitch_start)[-1]] + protected = (trace.time >= glitch_start) & (trace.time < glitch_start + 4.0) + response = (trace.time >= glitch_start - 0.5) & (trace.time <= glitch_end + 1.0) + gap = trace.distance_lead - trace.distance + + assert np.max(trace.target_speed[protected]) <= before + 0.05 + assert np.max(np.diff(trace.target_speed)[response[1:]]) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9 + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert not _has_brake_coast_brake(trace.a_target[response]) + assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0 + assert np.min(gap[response]) > STOP_DISTANCE + 10.0 + assert not trace.fcw.any() + _assert_no_new_solver_failures(trace, baseline) + + +def _high_speed_track_churn(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation: + frame = round(current_time / DT_MDL) + speed_noise = 0.5 * math.sin(2.0 * math.pi * current_time / 8.0) + distance_noise = 1.5 * math.sin(2.0 * math.pi * current_time / 6.0) + return truth | { + "dRel": truth["dRel"] + distance_noise + (1.0 if lead_name == "leadTwo" else 0.0), + "vLead": truth["vLead"] + speed_noise, + "vLeadK": truth["vLeadK"] + speed_noise, + "vRel": truth["vRel"] + speed_noise, + "radarTrackId": 100 + frame % 3 if lead_name == "leadOne" and frame % 2 == 0 else -1, + } + + +def test_high_speed_track_churn_keeps_speed_and_release_bounded(): + trace = _run( + duration=30.0, controller_enabled=True, profile=AccelProfile.eco, lead_relevancy=True, speed=30.0, + distance_lead=75.0, v_lead=29.5, v_cruise=34.72, lead_observation_fn=_high_speed_track_churn, + actuator_delay=0.15, actuator_lag=0.25, + ) + steady = trace.time >= 20.0 + target_steps = np.diff(trace.target_speed[steady]) + command_jerk = np.abs(_command_jerk(trace, 20.0)) + realized_jerk = np.abs(_filtered_realized_jerk(trace, 20.0)) + + # This scenario remains outside lead-recovery mode; assert its closed-loop response. + assert np.ptp(trace.speed[steady]) < 2.0 + assert np.max(target_steps) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-8 + assert not _has_propulsion_brake_cycle(trace.a_target[steady]) + assert float(np.percentile(command_jerk, 95)) < 1.6 and np.max(command_jerk) < 2.0 + assert float(np.percentile(realized_jerk, 95)) < 0.5 and np.max(realized_jerk) < 1.0 + assert np.min(trace.distance_lead - trace.distance) > STOP_DISTANCE + 10.0 + assert trace.solver_failures == 0 and not trace.fcw.any() + + +def test_previous_solver_failure_does_not_snap_target(): + common = { + "duration": 25.0, "controller_enabled": True, "profile": AccelProfile.eco, "lead_relevancy": True, "speed": 30.0, + "distance_lead": 75.0, "v_lead": 29.5, "v_cruise": 34.72, "lead_observation_fn": _high_speed_track_churn, + "actuator_delay": 0.15, "actuator_lag": 0.25, + } + clean = _run(**common) + trace = _run(previous_solver_failure_fn=lambda t: 20.0 <= t < 20.15, **common) + recovery = (trace.time >= 19.5) & (trace.time <= 21.0) + target_steps = np.diff(trace.target_speed[recovery]) + + # Assert recovery behavior because this scenario remains outside lead-recovery mode. + assert trace.active[recovery].all() + assert np.ptp(trace.speed[recovery]) < 2.0 + assert np.max(target_steps) <= TARGET_RELEASE_SLEW * DT_MDL + 1e-9 + assert not _has_propulsion_brake_cycle(trace.a_target[recovery]) + assert np.max(np.abs(_command_jerk(trace)[recovery[1:]])) < 5.0 + assert np.min(trace.distance_lead - trace.distance) >= np.min(clean.distance_lead - clean.distance) - ROUTINE_GAP_TOLERANCE + assert trace.solver_failures == clean.solver_failures == 0 and not trace.fcw.any() + + +@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport")) +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_route_507_braking_lead_slot_switch_has_no_false_relief_cycle(profile, actuator_delay, actuator_lag): + glitch_start = 67.0 + glitch_end = 67.5 + + def lead_speed(current_time: float) -> float: + braking_time = np.clip(current_time - 60.0, 0.0, 7.0) + return 10.0 - 0.42 * braking_time + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if glitch_start <= current_time < glitch_end: + if lead_name == "leadOne": + return None + return truth | { + "dRel": truth["dRel"] + 20.0, + "vLead": truth["vLead"] + 0.25, + "vLeadK": truth["vLeadK"] + 0.25, + "vRel": truth["vRel"] + 0.25, + "aLeadK": 0.0, + "radar": True, + "radarTrackId": 200, + } + if lead_name == "leadTwo": + return None + return truth | {"aLeadK": -0.42 if 60.0 <= current_time < glitch_start else 0.0, "radar": True, "radarTrackId": 100} + + common = { + "duration": 73.0, "controller_enabled": True, "profile": profile, "lead_relevancy": True, "speed": 20.0, + "distance_lead": 100.0, "v_lead": lead_speed, "v_cruise": 30.0, "actuator_delay": actuator_delay, "actuator_lag": actuator_lag, + } + clean = _run(**common) + trace = _run(lead_observation_fn=observe, **common) + response = (trace.time >= 66.0) & (trace.time <= 72.0) + jerk_response = (trace.time[1:] >= 66.0) & (trace.time[1:] <= 72.0) + clean_gap = clean.distance_lead - clean.distance + gap = trace.distance_lead - trace.distance + + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert not _has_brake_coast_brake(trace.a_target[response]) + assert np.max(-np.diff(trace.target_speed)[jerk_response]) <= LEAD_RECOVERY_DECEL_RATE * DT_MDL + 1e-9 + assert not trace.fcw.any() + assert trace.solver_failures == 0 + assert np.max(np.abs(np.diff(trace.a_target)[jerk_response] / DT_MDL)) < 3.0 + assert np.min(gap[response]) >= np.min(clean_gap[response]) - DROPOUT_GAP_TOLERANCE + + +@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport")) +def test_profile_ceiling_and_speed_stay_smooth_through_slot_switch_noise(profile): + glitch_start = 24.0 + glitch_end = 28.0 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if not glitch_start <= current_time < glitch_end: + return truth if lead_name == "leadOne" else None + + selected_slot = "leadOne" if round(current_time / DT_MDL) % 2 == 0 else "leadTwo" + if lead_name != selected_slot: + return None + sign = 1.0 if lead_name == "leadOne" else -1.0 + speed_offset = 0.25 * sign + return truth | { + "dRel": max(0.0, truth["dRel"] + 1.5 * sign), + "vLead": max(0.0, truth["vLead"] + speed_offset), + "vLeadK": max(0.0, truth["vLeadK"] + speed_offset), + "vRel": truth["vRel"] + speed_offset, + "aLeadK": 0.0, + "radar": True, + "radarTrackId": 100 if lead_name == "leadOne" else 200, + } + + common = { + "duration": 32.0, "controller_enabled": True, "profile": profile, "lead_relevancy": True, "speed": 20.0, + "distance_lead": 100.0, "v_lead": 10.0, "v_cruise": 30.0, "actuator_delay": 0.15, "actuator_lag": 0.25, + } + baseline = _run(**common) + trace = _run(lead_observation_fn=observe, **common) + glitch = (trace.time >= glitch_start) & (trace.time < glitch_end) + response = (trace.time >= glitch_start - 0.5) & (trace.time <= glitch_end + 1.0) + applied_accel_max = trace.mpc_upper_min[response] + selected_leads = trace.selected_lead[glitch] + controller_limited = trace.accel_ceiling_active[response] + stock_accel_max = np.asarray([get_max_accel(speed) for speed in trace.speed[response]]) + + assert set(selected_leads) == {0, 1} + assert np.count_nonzero(np.diff(selected_leads)) > 20 + assert controller_limited.any() + assert np.all(applied_accel_max[controller_limited] <= trace.profile_accel_max[response][controller_limited] + 1e-6) + assert np.all(applied_accel_max[controller_limited] <= stock_accel_max[controller_limited] + 1e-6) + assert np.max(trace.a_target[response]) > 0.2 + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert not _has_brake_coast_brake(trace.a_target[response]) + assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0 + assert not trace.fcw.any() + _assert_no_new_solver_failures(trace, baseline) + + +@pytest.mark.parametrize("profile", range(3), ids=("eco", "normal", "sport")) +def test_lead_recovery_dropout_keeps_the_profile_acceleration_ceiling(profile): + dropout_start = 25.0 + dropout_end = 25.15 + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo" or dropout_start <= current_time < dropout_end: + return None + return truth + + common = { + "duration": 28.0, "controller_enabled": True, "profile": profile, "lead_relevancy": True, "speed": 20.0, + "distance_lead": 100.0, "v_lead": 10.0, "v_cruise": 30.0, "actuator_delay": 0.15, "actuator_lag": 0.25, + } + clean = _run(**common) + trace = _run(lead_observation_fn=observe, **common) + dropout = (trace.time >= dropout_start) & (trace.time <= dropout_end) + response = (trace.time >= dropout_start - 0.5) & (trace.time <= dropout_end + 0.75) + + assert trace.accel_ceiling_active[dropout].all() + assert np.max(np.abs(trace.a_target[response] - clean.a_target[response])) < 0.08 + assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0 + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert not trace.fcw.any() + _assert_no_new_solver_failures(trace, clean) + + +def test_low_speed_lead_stop_has_no_release_then_rebrake(): + def lead_speed(current_time: float) -> float: + return max(0.0, 1.9 - 1.16 * current_time) + + def observe(current_time: float, lead_name: str, truth: LeadObservation) -> LeadObservation | None: + if lead_name == "leadTwo": + return None + moving = lead_speed(current_time) > 0.0 + return truth | {"vLeadK": truth["vLeadK"] if moving else -0.01, "aLeadK": -1.16 if moving else 0.0, + "radarTrackId": 7, "radar": True} + + common = { + "duration": 6.0, "profile": 0, "lead_relevancy": True, "speed": 4.5, "distance_lead": 18.0, "v_lead": lead_speed, + "v_cruise": 23.056, "lead_observation_fn": observe, "actuator_delay": 0.15, "actuator_lag": 0.20, + } + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + stop_hold = trace.state == int(AccelControllerState.stopHold) + stopped_response = trace.time >= trace.time[np.flatnonzero(trace.speed < 1e-3)[0]] + + assert stop_hold.any() + assert np.max(trace.speed[stopped_response]) <= np.max(baseline.speed[stopped_response]) + 0.01 + assert np.max(trace.a_target[stopped_response]) <= np.max(baseline.a_target[stopped_response]) + 0.05 + assert not _has_brake_coast_brake(trace.a_target[trace.time >= 1.0]) + assert np.min(trace.a_target) >= np.min(baseline.a_target) - ROUTINE_GAP_TOLERANCE + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - ROUTINE_GAP_TOLERANCE + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_high_speed_stopped_lead_approach_holds_the_completed_stop(actuator_delay, actuator_lag): + trace = _run( + duration=14.0, controller_enabled=True, profile=1, lead_relevancy=True, speed=20.0, + distance_lead=130.0, v_lead=0.0, v_cruise=30.0, actuator_delay=actuator_delay, actuator_lag=actuator_lag, + ) + stopped = np.flatnonzero(trace.speed < 0.05) + assert len(stopped) + after_stop = slice(stopped[0], None) + gap = trace.distance_lead - trace.distance + + assert STOP_DISTANCE <= np.min(gap[after_stop]) <= 25.0 + assert np.max(trace.speed[after_stop]) < 0.10 + assert not _has_propulsion_brake_cycle(trace.a_target[after_stop]) + assert trace.state[-1] == int(AccelControllerState.stopHold) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_decelerating_moving_lead_is_stock_safe_without_propulsion_reversal(): + def lead_speed(current_time: float) -> float: + if current_time < 2.0: + return 15.0 + progress = min((current_time - 2.0) / 6.0, 1.0) + return 15.0 - 5.0 * (3.0 * progress**2 - 2.0 * progress**3) + + common = { + "duration": 14.0, "profile": 1, "lead_relevancy": True, "speed": 20.0, "distance_lead": 110.0, + "v_lead": lead_speed, "v_cruise": 30.0, "actuator_delay": 0.20, "actuator_lag": 0.25, + } + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + response = trace.time >= 1.0 + baseline_p95 = float(np.percentile(np.abs(_filtered_realized_jerk(baseline)), 95)) + trace_p95 = float(np.percentile(np.abs(_filtered_realized_jerk(trace)), 95)) + + assert not _has_brake_coast_brake(trace.a_target[response]) + assert not _has_propulsion_after_braking(trace.a_target[response]) + assert trace_p95 <= baseline_p95 + 0.02 + assert np.min(trace.acceleration) >= np.min(baseline.acceleration) - ROUTINE_DECEL_TOLERANCE + assert np.min(trace.distance_lead - trace.distance) >= np.min(baseline.distance_lead - baseline.distance) - MOVING_LEAD_GAP_TOLERANCE + assert not trace.fcw.any() + _assert_no_new_solver_failures(trace, baseline) + + +def test_severe_closing_never_delays_stock_braking_or_reduces_clearance(): + common = { + "duration": 12.0, "lead_relevancy": True, "speed": 20.0, "distance_lead": 160.0, "v_lead": 3.5, + "actuator_delay": 0.20, "actuator_lag": 0.20, + } + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + for threshold in (-1.0, -2.0): + assert _first_time_below(trace, threshold) <= _first_time_below(baseline, threshold) + 1e-9 + + baseline_gap = baseline.distance_lead - baseline.distance + controlled_gap = trace.distance_lead - trace.distance + baseline_closing = baseline.speed - 3.5 + controlled_closing = trace.speed - 3.5 + baseline_ttc = np.min(baseline_gap[baseline_closing > 0.1] / baseline_closing[baseline_closing > 0.1]) + controlled_ttc = np.min(controlled_gap[controlled_closing > 0.1] / controlled_closing[controlled_closing > 0.1]) + onset = (trace.time[1:] > 0.5) & (trace.time[1:] < 3.0) + + assert np.min(controlled_gap) >= np.min(baseline_gap) - 0.02 + assert controlled_ttc >= baseline_ttc - 0.02 + assert np.min(controlled_gap) > 0.0 + assert np.max(np.abs(np.diff(trace.a_target)[onset] / DT_MDL)) < 4.0 + assert trace.solver_failures == 0 + + +@pytest.mark.parametrize(("actuator_delay", "actuator_lag"), ACTUATOR_DYNAMICS, ids=ACTUATOR_IDS) +def test_far_lead_profiles_start_early_in_order_without_solver_failures(actuator_delay, actuator_lag): + common = { + "duration": 11.0, "lead_relevancy": True, "speed": 25.0, "distance_lead": 200.0, "v_lead": 15.0, + "actuator_delay": actuator_delay, "actuator_lag": actuator_lag, + } + baseline = _run(controller_enabled=False, **common) + traces = [_run(controller_enabled=True, profile=profile, **common) for profile in range(3)] + baseline_onset = _sustained_time_below(baseline, -0.10) + baseline_jerk_p95 = float(np.percentile(np.abs(_filtered_realized_jerk(baseline)), 95)) + required_improvement = max(0.002, 0.02 * baseline_jerk_p95) + onsets = [_sustained_time_below(trace, -0.10) for trace in traces] + + assert onsets[0] <= baseline_onset - 0.5 + 1e-9 + assert onsets[1] <= baseline_onset + 1e-9 + assert onsets[2] <= baseline_onset + 1e-9 + assert onsets[0] <= onsets[1] + DT_MDL + 1e-9 + assert onsets[1] <= onsets[2] + DT_MDL + 1e-9 + first_finite_ceilings = [trace.raw_speed_ceiling[np.flatnonzero(np.isfinite(trace.raw_speed_ceiling))[0]] for trace in traces] + assert first_finite_ceilings[0] < first_finite_ceilings[1] < first_finite_ceilings[2] + for trace in traces: + assert trace.acceleration.min() >= baseline.acceleration.min() - 0.1 + assert float(np.percentile(np.abs(_filtered_realized_jerk(trace)), 95)) <= baseline_jerk_p95 - required_improvement + assert np.max(np.abs(_command_jerk(trace, after=0.5))) < 1.0 + assert not _has_brake_coast_brake(trace.a_target[trace.time >= 1.0]) + assert not _has_propulsion_brake_cycle(trace.a_target[trace.time >= 1.0]) + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_steady_slow_lead_has_no_gas_brake_cycle(): + duration = 60.0 + lead_speed = 10.0 + common = { + "duration": duration, "lead_relevancy": True, "speed": 20.0, "distance_lead": 100.0, "v_lead": lead_speed, + "v_cruise": 30.0, "actuator_delay": 0.15, "actuator_lag": 0.25, + } + baseline = _run(controller_enabled=False, **common) + trace = _run(controller_enabled=True, **common) + response = trace.time >= 1.0 + settled = trace.time >= duration - 5.0 + desired_gap = STOP_DISTANCE + get_T_FOLLOW() * lead_speed + baseline_gap = baseline.distance_lead - baseline.distance + gap = trace.distance_lead - trace.distance + max_settled_gap = max(np.mean(baseline_gap[settled]) + 10.0, desired_gap + 30.0) + + assert np.mean(trace.speed[settled]) >= lead_speed - 2.0 + assert np.mean(gap[settled]) <= max_settled_gap + assert not _has_brake_coast_brake(trace.a_target[response]) + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.min(gap) >= desired_gap - 1.6 + assert np.min(trace.a_target) >= np.min(baseline.a_target) - ROUTINE_GAP_TOLERANCE + _assert_no_new_solver_failures(trace, baseline) + + +def test_lead_recovery_slowdown_stays_smooth_without_a_second_braking_stage(): + slowdown_time = 70.0 + settled_lead_speed = 7.0 + + def lead_speed(current_time: float) -> float: + return 10.0 if current_time < slowdown_time else max(settled_lead_speed, 10.0 - 0.5 * (current_time - slowdown_time)) + + trace = _run( + duration=100.0, controller_enabled=True, profile=1, lead_relevancy=True, speed=20.0, + distance_lead=100.0, v_lead=lead_speed, v_cruise=30.0, actuator_delay=0.15, actuator_lag=0.25, + ) + steady_follow = (trace.time >= slowdown_time - 5.0) & (trace.time < slowdown_time) + response = trace.time >= slowdown_time + settled = trace.time >= 95.0 + gap = trace.distance_lead - trace.distance + desired_gap = STOP_DISTANCE + get_T_FOLLOW() * settled_lead_speed + + assert abs(np.mean(trace.speed[steady_follow]) - 10.0) < 0.5 + assert trace.accel_ceiling_active[steady_follow].all() + np.testing.assert_allclose(trace.mpc_upper_min[steady_follow], trace.profile_accel_max[steady_follow], atol=1e-6) + assert not np.any(trace.state[response] == int(AccelControllerState.stopHold)) + assert not trace.launching[response].any() + assert not _has_brake_coast_brake(trace.a_target[response]) + assert not _has_propulsion_brake_cycle(trace.a_target[response]) + assert np.max(np.abs(_command_jerk(trace)[response[1:]])) < 3.0 + assert float(np.percentile(np.abs(_filtered_realized_jerk(trace, after=slowdown_time)), 95)) < 0.30 + assert np.min(gap) > STOP_DISTANCE + assert np.mean(trace.speed[settled]) >= settled_lead_speed - 1.5 + assert np.mean(gap[settled]) <= desired_gap + 20.0 + assert not trace.fcw.any() + assert trace.solver_failures == 0 + + +def test_acceleration_output_remains_inside_stock_limits(): + trace = _run( + duration=12.0, controller_enabled=True, profile=AccelProfile.sport, lead_relevancy=False, + speed=0.0, v_cruise=30.0, actuator_delay=0.15, actuator_lag=0.20, + ) + stock_max = np.asarray([get_max_accel(speed) for speed in trace.speed]) + assert np.all(trace.a_target >= ACCEL_MIN - 1e-9) + assert np.all(trace.a_target <= stock_max + 0.06) + assert trace.stock_bounds_valid.all() + assert trace.solver_failures == 0 diff --git a/openpilot/sunnypilot/selfdrive/test/__init__.py b/openpilot/sunnypilot/selfdrive/test/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/__init__.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py new file mode 100644 index 0000000000..e22ad6367a --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/plant.py @@ -0,0 +1,395 @@ +""" +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 openpilot.cereal import log, messaging +from opendbc.car.interfaces import ACCEL_MAX, ACCEL_MIN +from openpilot.common.realtime import DT_CTRL, DT_MDL, Ratekeeper +from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.selfdrive.controls.lib.longcontrol import LongControl, 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, + run_long_control: bool = False, + ): + 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, run_long_control)) + + 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) + self.long_control = LongControl(CP, CP_SP) if run_long_control else None + + if self.actuator_model is not None and self.speed >= 0.01: + self.breakaway_confirmed = True + self.integration_dt = DT_CTRL if run_long_control else self.ts + delay_steps = 0 if self.transport_delay is None else round(self.transport_delay / self.integration_dt) + 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.integration_dt + 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.integration_dt + 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.integration_dt / self.actuator_lag) + self.acceleration += alpha * (response_command - self.acceleration) + else: + self.acceleration = response_command + return delayed_command, self.acceleration + + def _integrate_ego(self, dt: float, stop_at_standstill: bool = False) -> None: + self.speed += self.acceleration * dt + if self.speed <= 0.0 or stop_at_standstill and self.speed < 0.01 and self.actuator_command <= 0.0: + self.speed = self.acceleration = 0.0 + self.distance += self.speed * dt + + 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), + "vLead": float(v_lead), + "vLeadK": float(v_lead), + "aLeadK": float(a_lead), + "present": 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 = self.long_control.long_control_state if self.long_control is not None else ( + LongCtrlState.pid if self.enabled else LongCtrlState.off) + ss.selfdriveState.experimentalMode = self.e2e + ss.selfdriveState.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 + if self.long_control is None: + 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) + self._update_actuator(self.actuator_command) + self._integrate_ego(self.ts) + else: + for _ in range(round(self.ts / DT_CTRL)): + car_state.carState.vEgo = self.speed + car_state.carState.aEgo = self.acceleration + car_state.carState.standstill = self.speed < 0.01 + self.actuator_command = self.long_control.update( + self.enabled, car_state.carState, self.a_target, self.planner.output_should_stop, (ACCEL_MIN, ACCEL_MAX), + ) + self._update_actuator(self.actuator_command) + self._integrate_ego(DT_CTRL, stop_at_standstill=True) + self.should_stop = self.planner.output_should_stop + fcw = self.planner.fcw + self.distance_lead = self.distance_lead + v_lead * 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 + return { + "distance": self.distance, + "speed": self.speed, + "acceleration": self.acceleration, + "realized_acceleration": self.acceleration, + "a_target": self.a_target, + "actuator_command": self.actuator_command, + "published_a_ego": published_a_ego, + "published_v_ego": published_v_ego, + "should_stop": self.should_stop, + "long_control_state": (int(self.long_control.long_control_state) if self.long_control is not None + else control.controlsState.longControlState.raw), + "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, + "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), + } diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/__init__.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py new file mode 100644 index 0000000000..1475c6dfb6 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/test/longitudinal_maneuvers/tests/test_plant_sp.py @@ -0,0 +1,157 @@ +from collections.abc import Callable +import math +from typing import cast + +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)) + + +def stopped_lead(_current_time: float) -> float: + return 0.0 + + +PARITY_SCENARIOS = { + "approach_stopped_lead": {"lead_relevancy": True, "speed": 15.0, "distance_lead": 60.0, "v_cruise": 20.0, "v_lead": stopped_lead, "steps": 80}, + "stop_then_depart": {"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: Callable[[float], float], steps: int, **kwargs): + plant = cls(**kwargs) + plant.v_lead_prev = v_lead(0.0) + solver_failures = 0 + original_reset = plant.planner.mpc.reset + + def counting_reset(*args, **kw): + nonlocal solver_failures + if plant.planner.mpc.solution_status != 0: + solver_failures += 1 + return original_reset(*args, **kw) + + plant.planner.mpc.reset = counting_reset + results = [] + for _ in range(steps): + lead_speed = v_lead(plant.current_time) + 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: str): + kwargs = dict(PARITY_SCENARIOS[scenario]) + v_cruise = cast(float, kwargs.pop("v_cruise")) + v_lead = cast(Callable[[float], float], kwargs.pop("v_lead")) + steps = cast(int, 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, + "present": 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 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) diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index fb4ba7cc2b..a0957ee83c 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -652,6 +652,58 @@ } ] }, + { + "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": [ + { + "type": "capability", + "field": "has_longitudinal_control", + "equals": true + } + ], + "enablement": [ + { + "type": "capability", + "field": "has_longitudinal_control", + "equals": true + } + ] + }, + { + "key": "AccelPersonality", + "widget": "multiple_button", + "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" + } + ], + "enablement": [ + { + "type": "capability", + "field": "has_longitudinal_control", + "equals": true + }, + { + "type": "param", + "key": "AccelPersonalityEnabled", + "equals": true + } + ] + }, { "key": "IntelligentCruiseButtonManagement", "widget": "toggle", diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml index 3ef73e0fb0..11688c306b 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/cruise.yaml @@ -43,6 +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: 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 + enablement: + - $ref: '#/macros/longitudinal' + - type: param + key: AccelPersonalityEnabled + equals: true - key: IntelligentCruiseButtonManagement widget: toggle title: Intelligent Cruise Button Management (ICBM) (Alpha) diff --git a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py index 579d72b60b..510cd2da84 100644 --- a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py +++ b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py @@ -278,6 +278,22 @@ class TestKnownPanels: enhanced_enable_keys = {r.get("key") for r in enhanced.get("enablement", []) if r.get("type") == "param"} assert "NeuralNetworkLateralControl" in enhanced_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):