From a76ac2a3bcdc6687461e4ea09bc9010b4b62b1b0 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 15 Aug 2026 18:43:41 -0400 Subject: [PATCH] dec: restore Dynamic Experimental Control gate on longitudinal e2e output --- .../controls/lib/longitudinal_planner.py | 13 +- .../lib/dec/tests/test_dec_planner_gate.py | 112 ++++++++++++++++++ 2 files changed, 115 insertions(+), 10 deletions(-) create mode 100644 openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 12b4f9da61..8b62808dc0 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/openpilot/selfdrive/controls/lib/longitudinal_planner.py @@ -139,23 +139,16 @@ class LongitudinalPlanner(LongitudinalPlannerSP): output_a_target_e2e = sm['modelV2'].action.desiredAcceleration output_should_stop_e2e = sm['modelV2'].action.shouldStop - if self.is_e2e(sm): - 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: - self.mpc.source = LongitudinalPlanSource.e2e - else: - output_a_target = output_a_target_mpc - self.output_should_stop = output_should_stop_mpc + is_e2e = self.is_e2e(sm) - self.a_cruise = get_cruise_accel(sm['selfdriveState'].experimentalMode, v_cruise, v_ego, + self.a_cruise = get_cruise_accel(is_e2e, v_cruise, v_ego, self.a_cruise, steer_angle_without_offset, self.CP, self.dt, accel_coast, self.allow_throttle) cruise_should_stop = should_stop(v_ego, self.a_cruise) candidates = [(output_a_target_mpc, self.mpc.source, output_should_stop_mpc), (self.a_cruise, LongitudinalPlanSource.cruise, cruise_should_stop)] - if sm['selfdriveState'].experimentalMode: + if is_e2e: candidates.append((output_a_target_e2e, LongitudinalPlanSource.e2e, output_should_stop_e2e)) output_a_target, self.mpc.source, _ = min(candidates, key=lambda c: c[0]) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py new file mode 100644 index 0000000000..1f5c577028 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/dec/tests/test_dec_planner_gate.py @@ -0,0 +1,112 @@ +""" +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 typing import cast + +from openpilot.cereal import custom, messaging +from opendbc.car import structs +from openpilot.common.test import OpenpilotTestCase +from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner, LongitudinalPlanSource +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController + +V_EGO = 20.0 +E2E_ACCEL = -3.0 # low enough that e2e wins the min() whenever it is a candidate + + +class MockDec: + def __init__(self, active: bool, mode: str): + self._active = active + self._mode = mode + + def update(self, sm): + pass + + def active(self) -> bool: + return self._active + + def mode(self) -> str: + return self._mode + + def enabled(self) -> bool: + return True + + +class MockSubMaster(dict): + def __init__(self, services: dict): + super().__init__(services) + self.valid = dict.fromkeys(services, True) + self.logMonoTime = dict.fromkeys(services, 0) + self.updated = dict.fromkeys(services, True) + self.recv_frame = dict.fromkeys(services, 1) + + def all_checks(self, service_list=None) -> bool: + return True + + +def build_sm(experimental_mode: bool) -> MockSubMaster: + services = {} + for service in ("radarState", "controlsState", "vehicleParameters", "carStateSP", + "liveMapDataSP", "gpsLocationExternal", "gpsLocation"): + services[service] = getattr(messaging.new_message(service), service) + + car_state = messaging.new_message('carState') + car_state.carState.vEgo = V_EGO + car_state.carState.vCruise = 100.0 + car_state.carState.vCruiseCluster = 100.0 + services['carState'] = car_state.carState.as_reader() + + selfdrive_state = messaging.new_message('selfdriveState') + selfdrive_state.selfdriveState.experimentalMode = experimental_mode + selfdrive_state.selfdriveState.enabled = True + services['selfdriveState'] = selfdrive_state.selfdriveState.as_reader() + + car_control = messaging.new_message('carControl') + car_control.carControl.enabled = True + services['carControl'] = car_control.carControl.as_reader() + + model = messaging.new_message('modelV2') + model.modelV2.orientationRate.z = [0.01] * 33 # nonzero: a straight path divides by zero in SCC vision + model.modelV2.velocity.x = [V_EGO] * 33 + model.modelV2.position.x = [float(i) for i in range(33)] + model.modelV2.action.desiredAcceleration = E2E_ACCEL + services['modelV2'] = model.modelV2.as_reader() + + return MockSubMaster(services) + + +def build_planner(dec_active: bool, dec_mode: str) -> LongitudinalPlanner: + CP = structs.CarParams() + CP.steerRatio = 15.0 + CP.wheelbase = 2.7 + CP.longitudinalActuatorDelay = 0.2 + CP_SP = custom.CarParamsSP.new_message().as_reader() + + planner = LongitudinalPlanner(CP, CP_SP, init_v=V_EGO) + planner.dec = cast(DynamicExperimentalController, MockDec(dec_active, dec_mode)) + return planner + + +class TestDecPlannerGate(OpenpilotTestCase): + """The e2e candidate must be gated on is_e2e(), not raw experimentalMode.""" + + def _source(self, experimental_mode: bool, dec_active: bool, dec_mode: str) -> LongitudinalPlanSource: + planner = build_planner(dec_active, dec_mode) + planner.update(build_sm(experimental_mode)) + return planner.mpc.source + + def test_no_e2e_when_experimental_mode_off(self): + assert self._source(False, False, 'acc') != LongitudinalPlanSource.e2e + + def test_e2e_when_dec_inactive(self): + # DEC off: behavior must match upstream + assert self._source(True, False, 'acc') == LongitudinalPlanSource.e2e + + def test_e2e_when_dec_blended(self): + assert self._source(True, True, 'blended') == LongitudinalPlanSource.e2e + + def test_no_e2e_when_dec_holds_acc(self): + # the regression + assert self._source(True, True, 'acc') != LongitudinalPlanSource.e2e