From c3bfd1702830535ab0be99eb84ea0c8b80f82c07 Mon Sep 17 00:00:00 2001 From: Kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sun, 12 Jan 2025 22:17:15 -0700 Subject: [PATCH] Revert "Longitudinal: Dynamic Experimental Control" (#571) Revert "Longitudinal: Dynamic Experimental Control (#564)" This reverts commit bba3c39e2fa8f1daacefc92aed606f0b983fc0a5. --- cereal/custom.capnp | 9 +- cereal/log.capnp | 2 +- cereal/services.py | 1 - common/params.cc | 2 - .../controls/lib/longitudinal_planner.py | 9 +- selfdrive/controls/plannerd.py | 2 +- selfdrive/ui/qt/offroad/settings.cc | 6 - selfdrive/ui/translations/main_ar.ts | 8 - selfdrive/ui/translations/main_de.ts | 8 - selfdrive/ui/translations/main_es.ts | 8 - selfdrive/ui/translations/main_fr.ts | 8 - selfdrive/ui/translations/main_ja.ts | 8 - selfdrive/ui/translations/main_ko.ts | 8 - selfdrive/ui/translations/main_pt-BR.ts | 8 - selfdrive/ui/translations/main_th.ts | 8 - selfdrive/ui/translations/main_tr.ts | 8 - selfdrive/ui/translations/main_zh-CHS.ts | 8 - selfdrive/ui/translations/main_zh-CHT.ts | 8 - sunnypilot/selfdrive/__init__.py | 0 sunnypilot/selfdrive/controls/__init__.py | 0 sunnypilot/selfdrive/controls/lib/__init__.py | 0 .../selfdrive/controls/lib/dec/__init__.py | 0 sunnypilot/selfdrive/controls/lib/dec/dec.py | 399 ------------------ .../selfdrive/controls/lib/dec/helpers.py | 46 -- .../controls/lib/dec/tests/__init__.py | 0 .../dec/tests/pytest_dynamic_controller.py | 257 ----------- system/manager/manager.py | 1 - 27 files changed, 4 insertions(+), 818 deletions(-) delete mode 100644 sunnypilot/selfdrive/__init__.py delete mode 100644 sunnypilot/selfdrive/controls/__init__.py delete mode 100644 sunnypilot/selfdrive/controls/lib/__init__.py delete mode 100644 sunnypilot/selfdrive/controls/lib/dec/__init__.py delete mode 100644 sunnypilot/selfdrive/controls/lib/dec/dec.py delete mode 100644 sunnypilot/selfdrive/controls/lib/dec/helpers.py delete mode 100644 sunnypilot/selfdrive/controls/lib/dec/tests/__init__.py delete mode 100644 sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index bffc1e4d3b..3ddeed75f6 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -82,14 +82,7 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { } } -struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { - mpcSource @0 :MpcSource; - dynamicExperimentalControl @1 :Bool; - - enum MpcSource { - acc @0; - blended @1; - } +struct CustomReserved2 @0xf35cc4560bbf6ec2 { } struct CustomReserved3 @0xda96579883444c35 { diff --git a/cereal/log.capnp b/cereal/log.capnp index 80a5034392..d5fbad6fe8 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2633,7 +2633,7 @@ struct Event { # *********** Custom: reserved for forks *********** selfdriveStateSP @107 :Custom.SelfdriveStateSP; modelManagerSP @108 :Custom.ModelManagerSP; - longitudinalPlanSP @109 :Custom.LongitudinalPlanSP; + customReserved2 @109 :Custom.CustomReserved2; customReserved3 @110 :Custom.CustomReserved3; customReserved4 @111 :Custom.CustomReserved4; customReserved5 @112 :Custom.CustomReserved5; diff --git a/cereal/services.py b/cereal/services.py index ae4a35b3e4..346bf81a09 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -77,7 +77,6 @@ _services: dict[str, tuple] = { # sunnypilot "modelManagerSP": (False, 1., 1), "selfdriveStateSP": (True, 100., 10), - "longitudinalPlanSP": (True, 20., 10), # debug "uiDebug": (True, 0., 1), diff --git a/common/params.cc b/common/params.cc index 3c11e034cb..7c1b21c00c 100644 --- a/common/params.cc +++ b/common/params.cc @@ -223,8 +223,6 @@ std::unordered_map keys = { {"SunnylinkDongleId", PERSISTENT}, {"SunnylinkdPid", PERSISTENT}, {"SunnylinkEnabled", PERSISTENT}, - - {"DynamicExperimentalControl", PERSISTENT}, }; } // namespace diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2272ebfeb8..eba8019117 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,8 +16,6 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_ from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog -from openpilot.sunnypilot.selfdrive.controls.lib.dec.helpers import DecPlanner - LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] @@ -69,11 +67,10 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05): return a_target, should_stop -class LongitudinalPlanner(DecPlanner): +class LongitudinalPlanner: def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP self.mpc = LongitudinalMpc(dt=dt) - DecPlanner.__init__(self, self.CP, self.mpc) self.fcw = False self.dt = dt self.allow_throttle = True @@ -108,10 +105,7 @@ class LongitudinalPlanner(DecPlanner): return x, v, a, j, throttle_prob def update(self, sm): - DecPlanner.update(self, sm) self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' - if dec_mpc_mode := self.get_mpc_mode(sm): - self.mpc.mode = dec_mpc_mode if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) @@ -212,4 +206,3 @@ class LongitudinalPlanner(DecPlanner): longitudinalPlan.allowThrottle = self.allow_throttle pm.send('longitudinalPlan', plan_send) - self.publish_longitudinal_plan_sp(sm, pm) diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 16ac80db60..bcfc4d0c14 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -18,7 +18,7 @@ def main(): ldw = LaneDepartureWarning() longitudinal_planner = LongitudinalPlanner(CP) - pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP']) + pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance']) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState'], poll='modelV2', ignore_avg_freq=['radarState']) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 7f710328cc..b633bdf33a 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -40,12 +40,6 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { "", "../assets/img_experimental_white.svg", }, - { - "DynamicExperimentalControl", - tr("Enable Dynamic Experimental Control"), - tr("Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal."), - "../assets/offroad/icon_blank.png", - }, { "DisengageOnAccelerator", tr("Disengage on Accelerator Pedal"), diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts index 4f7a2222bd..bd1452612c 100644 --- a/selfdrive/ui/translations/main_ar.ts +++ b/selfdrive/ui/translations/main_ar.ts @@ -1411,14 +1411,6 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. تمكين مراقبة السائق حتى عندما لا يكون نظام OpenPilot مُفعّلاً. - - Enable Dynamic Experimental Control - - - - Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. - - Updater diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index 6eaab99989..f015d2ea5f 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -1395,14 +1395,6 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. - - Enable Dynamic Experimental Control - - - - Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. - - Updater diff --git a/selfdrive/ui/translations/main_es.ts b/selfdrive/ui/translations/main_es.ts index 58fb2696c6..658f131a36 100644 --- a/selfdrive/ui/translations/main_es.ts +++ b/selfdrive/ui/translations/main_es.ts @@ -1395,14 +1395,6 @@ Esto puede tardar un minuto. Enable the openpilot longitudinal control (alpha) toggle to allow Experimental mode. Activar el control longitudinal (fase experimental) para permitir el modo Experimental. - - Enable Dynamic Experimental Control - - - - Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. - - Updater diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts index b9be52acd8..7e2f6649d6 100644 --- a/selfdrive/ui/translations/main_fr.ts +++ b/selfdrive/ui/translations/main_fr.ts @@ -1395,14 +1395,6 @@ Cela peut prendre jusqu'à une minute. Enable driver monitoring even when openpilot is not engaged. - - Enable Dynamic Experimental Control - - - - Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. - - Updater diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 4f757d5f0c..0229a36bd6 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -1389,14 +1389,6 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. - - Enable Dynamic Experimental Control - - - - Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. - - Updater diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index ebc217e556..0b1c82e92c 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -1391,14 +1391,6 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. Openpilot이 활성화되지 않은 경우에도 드라이버 모니터링을 활성화합니다. - - Enable Dynamic Experimental Control - - - - Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. - - Updater diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index 4d24c47d48..2a2693961e 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -1395,14 +1395,6 @@ Isso pode levar até um minuto. Enable driver monitoring even when openpilot is not engaged. Habilite o monitoramento do motorista mesmo quando o openpilot não estiver acionado. - - Enable Dynamic Experimental Control - - - - Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. - - Updater diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index 08ce8b7647..c2557a6faa 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -1391,14 +1391,6 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. - - Enable Dynamic Experimental Control - - - - Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. - - Updater diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts index 0037777958..ac22574b97 100644 --- a/selfdrive/ui/translations/main_tr.ts +++ b/selfdrive/ui/translations/main_tr.ts @@ -1389,14 +1389,6 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. - - Enable Dynamic Experimental Control - - - - Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. - - Updater diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 517ad5fd1d..d284bbac6e 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -1391,14 +1391,6 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. 即使在openpilot未激活时也启用驾驶员监控。 - - Enable Dynamic Experimental Control - - - - Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. - - Updater diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 6f82c97e9f..37c9d0c70f 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -1391,14 +1391,6 @@ This may take up to a minute. Enable driver monitoring even when openpilot is not engaged. 即使在openpilot未激活時也啟用駕駛監控。 - - Enable Dynamic Experimental Control - - - - Enable toggle to allow the model to determine when to use sunnypilot ACC or sunnypilot End to End Longitudinal. - - Updater diff --git a/sunnypilot/selfdrive/__init__.py b/sunnypilot/selfdrive/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/selfdrive/controls/__init__.py b/sunnypilot/selfdrive/controls/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/selfdrive/controls/lib/__init__.py b/sunnypilot/selfdrive/controls/lib/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/selfdrive/controls/lib/dec/__init__.py b/sunnypilot/selfdrive/controls/lib/dec/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py deleted file mode 100644 index eb7660965e..0000000000 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ /dev/null @@ -1,399 +0,0 @@ -# The MIT License -# -# Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors. -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in -# all copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -# THE SOFTWARE. -# -# Version = 2024-7-11 - -import numpy as np - -from cereal import messaging -from openpilot.common.numpy_fast import interp -from openpilot.common.params import Params -from openpilot.common.realtime import DT_MDL - -# d-e2e, from modeldata.h -TRAJECTORY_SIZE = 33 - -LEAD_WINDOW_SIZE = 4 -LEAD_PROB = 0.6 - -SLOW_DOWN_WINDOW_SIZE = 4 -SLOW_DOWN_PROB = 0.6 - -SLOW_DOWN_BP = [0., 10., 20., 30., 40., 50., 55., 60.] -SLOW_DOWN_DIST = [25., 38., 55., 75., 95., 115., 130., 150.] - -SLOWNESS_WINDOW_SIZE = 12 -SLOWNESS_PROB = 0.5 -SLOWNESS_CRUISE_OFFSET = 1.05 - -DANGEROUS_TTC_WINDOW_SIZE = 3 -DANGEROUS_TTC = 2.3 - -HIGHWAY_CRUISE_KPH = 70 - -STOP_AND_GO_FRAME = 60 - -SET_MODE_TIMEOUT = 10 - -MPC_FCW_WINDOW_SIZE = 10 -MPC_FCW_PROB = 0.5 - -V_ACC_MIN = 9.72 - - -class SNG_State: - off = 0 - stopped = 1 - going = 2 - - -class GenericMovingAverageCalculator: - def __init__(self, window_size): - self.window_size = window_size - self.data = [] - self.total = 0 - - def add_data(self, value: float) -> None: - if len(self.data) == self.window_size: - self.total -= self.data.pop(0) - self.data.append(value) - self.total += value - - def get_moving_average(self) -> float | None: - return None if len(self.data) == 0 else self.total / len(self.data) - - def reset_data(self) -> None: - self.data = [] - self.total = 0 - - -class WeightedMovingAverageCalculator: - def __init__(self, window_size): - self.window_size = window_size - self.data = [] - self.weights = np.linspace(1, 3, window_size) # Linear weights, adjust as needed - - def add_data(self, value: float) -> None: - if len(self.data) == self.window_size: - self.data.pop(0) - self.data.append(value) - - def get_weighted_average(self) -> float | None: - if len(self.data) == 0: - return None - weighted_sum: float = float(np.dot(self.data, self.weights[-len(self.data):])) - weight_total: float = float(np.sum(self.weights[-len(self.data):])) - return weighted_sum / weight_total - - def reset_data(self) -> None: - self.data = [] - - -class DynamicExperimentalController: - def __init__(self, params=None): - self._params = params or Params() - self._is_enabled: bool = self._params.get_bool("DynamicExperimentalControl") - self._mode: str = 'acc' - self._mode_prev: str = 'acc' - self._mode_changed: bool = False - self._frame: int = 0 - - # Use weighted moving average for filtering leads - self._lead_gmac = WeightedMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE) - self._has_lead_filtered = False - self._has_lead_filtered_prev = False - - self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=SLOW_DOWN_WINDOW_SIZE) - self._has_slow_down = False - - self._has_blinkers = False - - self._slowness_gmac = WeightedMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE) - self._has_slowness = False - - self._has_nav_instruction = False - - self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE) - self._has_dangerous_ttc = False - - self._v_ego_kph = 0. - self._v_cruise_kph = 0. - - self._has_lead = False - - self._has_standstill = False - self._has_standstill_prev = False - - self._sng_transit_frame = 0 - self._sng_state = SNG_State.off - - self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE) - self._has_mpc_fcw = False - self._mpc_fcw_crash_cnt = 0 - - self._set_mode_timeout = 0 - - @staticmethod - def _anomaly_detection(recent_data: list[float], threshold: float = 2.0, context_check: bool = True) -> bool: - """ - Basic anomaly detection using standard deviation. - """ - if len(recent_data) < 5: - return False - mean: float = float(np.mean(recent_data)) - std_dev: float = float(np.std(recent_data)) - anomaly: bool = bool(recent_data[-1] > mean + threshold * std_dev) - - # Context check to ensure repeated anomaly - if context_check: - return bool(np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1) - return anomaly - - def _adaptive_slowdown_threshold(self) -> float: - """ - Adapts the slow-down threshold based on vehicle speed and recent behavior. - """ - adaptive_threshold: float = float( - interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.05 * np.log(1 + len(self._slow_down_gmac.data))) - ) - return adaptive_threshold - - def _smoothed_lead_detection(self, lead_prob: float, smoothing_factor: float = 0.2) -> bool: - """ - Smoothing the lead detection to avoid erratic behavior. - """ - self._has_lead_filtered = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob - return bool(self._has_lead_filtered > LEAD_PROB) - - def _adaptive_lead_prob_threshold(self) -> float: - """ - Adapts lead probability threshold based on driving conditions. - """ - if self._v_ego_kph > HIGHWAY_CRUISE_KPH: - return float(LEAD_PROB + 0.1) # Increase the threshold on highways - return float(LEAD_PROB) - - def _update(self, sm: messaging.SubMaster) -> None: - car_state = sm['carState'] - lead_one = sm['radarState'].leadOne - md = sm['modelV2'] - - self._v_ego_kph = car_state.vEgo * 3.6 - self._v_cruise_kph = car_state.vCruise - self._has_lead = lead_one.status - self._has_standstill = car_state.standstill - - # fcw detection - self._mpc_fcw_gmac.add_data(self._mpc_fcw_crash_cnt > 0) - self._has_mpc_fcw = self._mpc_fcw_gmac.get_weighted_average() > MPC_FCW_PROB - - # nav enable detection - # self._has_nav_instruction = md.navEnabledDEPRECATED and maneuver_distance / max(car_state.vEgo, 1) < 13 - - # lead detection with smoothing - self._lead_gmac.add_data(lead_one.status) - #self._has_lead_filtered = self._lead_gmac.get_weighted_average() > LEAD_PROB - lead_prob = self._lead_gmac.get_weighted_average() or 0 - self._has_lead_filtered = self._smoothed_lead_detection(lead_prob) - - # adaptive slow down detection - adaptive_threshold = self._adaptive_slowdown_threshold() - slow_down_trigger = len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE and md.position.x[TRAJECTORY_SIZE - 1] < adaptive_threshold - self._slow_down_gmac.add_data(slow_down_trigger) - self._has_slow_down = self._slow_down_gmac.get_weighted_average() > SLOW_DOWN_PROB - - # anomaly detection for slow down events - if self._anomaly_detection(self._slow_down_gmac.data): - # Handle anomaly: potentially log it, adjust behavior, or issue a warning - self._has_slow_down = False # Reset slow down if anomaly detected - - # blinker detection - self._has_blinkers = car_state.leftBlinker or car_state.rightBlinker - - # sng detection - if self._has_standstill: - self._sng_state = SNG_State.stopped - self._sng_transit_frame = 0 - else: - if self._sng_transit_frame == 0: - if self._sng_state == SNG_State.stopped: - self._sng_state = SNG_State.going - self._sng_transit_frame = STOP_AND_GO_FRAME - elif self._sng_state == SNG_State.going: - self._sng_state = SNG_State.off - elif self._sng_transit_frame > 0: - self._sng_transit_frame -= 1 - - # slowness detection - if not self._has_standstill: - self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph * SLOWNESS_CRUISE_OFFSET)) - self._has_slowness = self._slowness_gmac.get_weighted_average() > SLOWNESS_PROB - - # dangerous TTC detection - if not self._has_lead_filtered and self._has_lead_filtered_prev: - self._dangerous_ttc_gmac.reset_data() - self._has_dangerous_ttc = False - - if self._has_lead and car_state.vEgo >= 0.01: - self._dangerous_ttc_gmac.add_data(lead_one.dRel / car_state.vEgo) - - self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_weighted_average() is not None and self._dangerous_ttc_gmac.get_weighted_average() <= DANGEROUS_TTC - - # keep prev values - self._has_standstill_prev = self._has_standstill - self._has_lead_filtered_prev = self._has_lead_filtered - - def _radarless_mode(self) -> None: - # when mpc fcw crash prob is high - # use blended to slow down quickly - if self._has_mpc_fcw: - self._set_mode('blended') - return - - # Nav enabled and distance to upcoming turning is 300 or below - # if self._has_nav_instruction: - # self._set_mode('blended') - # return - - # when blinker is on and speed is driving below V_ACC_MIN: blended - # we don't want it to switch mode at higher speed, blended may trigger hard brake - # if self._has_blinkers and self._v_ego_kph < V_ACC_MIN: - # self._set_mode('blended') - # return - - # when at highway cruise and SNG: blended - # ensuring blended mode is used because acc is bad at catching SNG lead car - # especially those who accel very fast and then brake very hard. - # if self._sng_state == SNG_State.going and self._v_cruise_kph >= V_ACC_MIN: - # self._set_mode('blended') - # return - - # when standstill: blended - # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light. - if self._has_standstill: - self._set_mode('blended') - return - - # when detecting slow down scenario: blended - # e.g. traffic light, curve, stop sign etc. - if self._has_slow_down: - self._set_mode('blended') - return - - # when detecting lead slow down: blended - # use blended for higher braking capability - if self._has_dangerous_ttc: - self._set_mode('blended') - return - - # car driving at speed lower than set speed: acc - if self._has_slowness: - self._set_mode('acc') - return - - self._set_mode('acc') - - def _radar_mode(self) -> None: - # when mpc fcw crash prob is high - # use blended to slow down quickly - if self._has_mpc_fcw: - self._set_mode('blended') - return - - # If there is a filtered lead, the vehicle is not in standstill, and the lead vehicle's yRel meets the condition, - if self._has_lead_filtered and not self._has_standstill: - self._set_mode('acc') - return - - # when blinker is on and speed is driving below V_ACC_MIN: blended - # we don't want it to switch mode at higher speed, blended may trigger hard brake - # if self._has_blinkers and self._v_ego_kph < V_ACC_MIN: - # self._set_mode('blended') - # return - - # when standstill: blended - # in case of lead car suddenly move away under traffic light, acc mode won't brake at traffic light. - if self._has_standstill: - self._set_mode('blended') - return - - # when detecting slow down scenario: blended - # e.g. traffic light, curve, stop sign etc. - if self._has_slow_down: - self._set_mode('blended') - return - - # car driving at speed lower than set speed: acc - if self._has_slowness: - self._set_mode('acc') - return - - # Nav enabled and distance to upcoming turning is 300 or below - # if self._has_nav_instruction: - # self._set_mode('blended') - # return - - self._set_mode('acc') - - def get_mpc_mode(self) -> str: - return str(self._mode) - - def has_changed(self) -> bool: - return bool(self._mode_changed) - - def set_enabled(self, enabled: bool) -> None: - self._is_enabled = enabled - - def is_enabled(self) -> bool: - return self._is_enabled - - def set_mpc_fcw_crash_cnt(self, crash_cnt: float) -> None: - self._mpc_fcw_crash_cnt = crash_cnt - - def _set_mode(self, mode: str) -> None: - if self._set_mode_timeout == 0: - self._mode = mode - if mode == 'blended': - self._set_mode_timeout = SET_MODE_TIMEOUT - - if self._set_mode_timeout > 0: - self._set_mode_timeout -= 1 - - def _read_params(self) -> None: - if self._frame % int(1. / DT_MDL) == 0: - self._is_enabled = self._params.get_bool("DynamicExperimentalControl") - - def update(self, radar_unavailable: bool, sm: messaging.SubMaster) -> None: - self._read_params() - - if self._is_enabled: - self._update(sm) - - if radar_unavailable: - self._radarless_mode() - else: - self._radar_mode() - - self._mode_changed = self._mode != self._mode_prev - self._mode_prev = self._mode - - self._frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/dec/helpers.py b/sunnypilot/selfdrive/controls/lib/dec/helpers.py deleted file mode 100644 index 602d3b64ba..0000000000 --- a/sunnypilot/selfdrive/controls/lib/dec/helpers.py +++ /dev/null @@ -1,46 +0,0 @@ -""" -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 cereal import messaging, custom -from opendbc.car import structs -from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController - -MpcSource = custom.LongitudinalPlanSP.MpcSource - - -class DecPlanner: - def __init__(self, CP: structs.CarParams, mpc): - self.CP = CP - self.mpc = mpc - - self.is_enabled = False - - self.dynamic_experimental_controller = DynamicExperimentalController() - - def get_mpc_mode(self, sm: messaging.SubMaster): - if not self.is_enabled or not sm['selfdriveState'].experimentalMode: - return None - - return self.dynamic_experimental_controller.get_mpc_mode() - - def update(self, sm: messaging.SubMaster) -> None: - self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) - self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm) - - def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: - plan_sp_send = messaging.new_message('longitudinalPlanSP') - - plan_sp_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) - - longitudinalPlanSP = plan_sp_send.longitudinalPlanSP - - # DEC - longitudinalPlanSP.mpcSource = MpcSource.blended if self.mpc.mode == 'blended' else MpcSource.acc - - longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled() - - pm.send('longitudinalPlanSP', plan_sp_send) diff --git a/sunnypilot/selfdrive/controls/lib/dec/tests/__init__.py b/sunnypilot/selfdrive/controls/lib/dec/tests/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py deleted file mode 100644 index af77639424..0000000000 --- a/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py +++ /dev/null @@ -1,257 +0,0 @@ -from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import ( - DynamicExperimentalController, - TRAJECTORY_SIZE, - LEAD_WINDOW_SIZE, - SLOW_DOWN_WINDOW_SIZE, - DANGEROUS_TTC_WINDOW_SIZE, - MPC_FCW_WINDOW_SIZE, - SNG_State, - STOP_AND_GO_FRAME -) - -import pytest -import numpy as np -from openpilot.common.params import Params - -class MockInterp: - def __call__(self, x, xp, fp): - return np.interp(x, xp, fp) - -class MockCarState: - def __init__(self, v_ego=0., standstill=False, left_blinker=False, right_blinker=False): - self.vEgo = v_ego - self.standstill = standstill - self.leftBlinker = left_blinker - self.rightBlinker = right_blinker - -class MockLeadOne: - def __init__(self, status=False, d_rel=0): - self.status = status - self.dRel = d_rel - -class MockModelData: - def __init__(self, x_vals=None, positions=None): - self.orientation = type('Orientation', (), {'x': x_vals})() - self.position = type('Position', (), {'x': positions})() - -class MockControlState: - def __init__(self, v_cruise=0): - self.vCruise = v_cruise - -@pytest.fixture -def interp(monkeypatch): - mock_interp = MockInterp() - monkeypatch.setattr('openpilot.common.numpy_fast.interp', mock_interp) - return mock_interp - -@pytest.fixture -def controller(interp): - params = Params() - params.put_bool("DynamicExperimentalControl", True) - controller = DynamicExperimentalController() - return controller - -def test_initial_state(controller): - """Test initial state of the controller""" - assert controller._mode == 'acc' - assert not controller._has_lead - assert not controller._has_standstill - assert controller._sng_state == SNG_State.off - assert not controller._has_lead_filtered - assert not controller._has_slow_down - assert not controller._has_dangerous_ttc - assert not controller._has_mpc_fcw - -@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"]) -def test_standstill_detection(controller, has_radar): - """Test standstill detection and state transitions""" - car_state = MockCarState(standstill=True) - lead_one = MockLeadOne() - md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE) - controls_state = MockControlState() - - # Test transition to standstill - controller.update(not has_radar, car_state, lead_one, md, controls_state) - assert controller._sng_state == SNG_State.stopped - assert controller.get_mpc_mode() == 'blended' - - # Test transition from standstill to moving - car_state.standstill = False - controller.update(not has_radar, car_state, lead_one, md, controls_state) - assert controller._sng_state == SNG_State.going - - # Test complete transition to normal driving - for _ in range(STOP_AND_GO_FRAME + 1): - controller.update(not has_radar, car_state, lead_one, md, controls_state) - assert controller._sng_state == SNG_State.off - -@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"]) -def test_lead_detection(controller, has_radar): - """Test lead vehicle detection and filtering""" - car_state = MockCarState(v_ego=20) # 72 kph - lead_one = MockLeadOne(status=True, d_rel=50) # Safe distance - md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE) - controls_state = MockControlState(v_cruise=72) - - # Let moving average stabilize - for _ in range(LEAD_WINDOW_SIZE + 1): - controller.update(not has_radar, car_state, lead_one, md, controls_state) - - assert controller._has_lead_filtered - expected_mode = 'acc' if has_radar else 'blended' - assert controller.get_mpc_mode() == expected_mode - - # Test lead loss detection - lead_one.status = False - for _ in range(LEAD_WINDOW_SIZE + 1): - controller.update(not has_radar, car_state, lead_one, md, controls_state) - - assert not controller._has_lead_filtered - -@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"]) -def test_slow_down_detection(controller, has_radar): - """Test slow down detection based on trajectory""" - car_state = MockCarState(v_ego=10/3.6) # 10 kph - lead_one = MockLeadOne() - x_vals = [0] * TRAJECTORY_SIZE - positions = [20] * TRAJECTORY_SIZE # Position within slow down threshold - md = MockModelData(x_vals=x_vals, positions=positions) - controls_state = MockControlState(v_cruise=30) - - # Test slow down detection - for _ in range(SLOW_DOWN_WINDOW_SIZE + 1): - controller.update(not has_radar, car_state, lead_one, md, controls_state) - - assert controller._has_slow_down - assert controller.get_mpc_mode() == 'blended' - - # Test slow down recovery - positions = [200] * TRAJECTORY_SIZE # Position outside slow down threshold - md = MockModelData(x_vals=x_vals, positions=positions) - for _ in range(SLOW_DOWN_WINDOW_SIZE + 1): - controller.update(not has_radar, car_state, lead_one, md, controls_state) - - assert not controller._has_slow_down - -@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"]) -def test_dangerous_ttc_detection(controller, has_radar): - """Test Time-To-Collision detection and handling""" - car_state = MockCarState(v_ego=10) # 36 kph - lead_one = MockLeadOne(status=True) - md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE) - controls_state = MockControlState(v_cruise=36) - - # First establish normal conditions with lead - lead_one.dRel = 100 # Safe distance - for _ in range(LEAD_WINDOW_SIZE + 1): # First establish lead detection - controller.update(not has_radar, car_state, lead_one, md, controls_state) - - assert controller._has_lead_filtered # Verify lead is detected - - # Now test dangerous TTC detection - lead_one.dRel = 10 # 10m distance - should trigger dangerous TTC - # TTC = dRel/vEgo = 10/10 = 1s (which is less than DANGEROUS_TTC = 2.3s) - - # Need to update multiple times to allow the weighted average to stabilize - for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): - controller.update(not has_radar, car_state, lead_one, md, controls_state) - - assert controller._has_dangerous_ttc, "TTC of 1s should be considered dangerous" - expected_mode = 'acc' if has_radar else 'blended' - assert controller.get_mpc_mode() == expected_mode, f"Should be in [{expected_mode}] mode with dangerous TTC" - -@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"]) -def test_mode_transitions(controller, has_radar): - """Test comprehensive mode transitions under different conditions""" - # Initialize with normal driving conditions - car_state = MockCarState(v_ego=25) # 90 kph - lead_one = MockLeadOne(status=False) - md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[200] * TRAJECTORY_SIZE) - controls_state = MockControlState(v_cruise=100) - - def stabilize_filters(): - """Helper to let all moving averages stabilize""" - for _ in range(max(LEAD_WINDOW_SIZE, SLOW_DOWN_WINDOW_SIZE, - DANGEROUS_TTC_WINDOW_SIZE, MPC_FCW_WINDOW_SIZE) + 1): - controller.update(not has_radar, car_state, lead_one, md, controls_state) - - # Test 1: Normal driving -> ACC mode - stabilize_filters() - assert controller.get_mpc_mode() == 'acc', "Should be in ACC mode under normal driving conditions" - - # Test 2: Standstill -> Blended mode - car_state.standstill = True - controller.update(not has_radar, car_state, lead_one, md, controls_state) - assert controller.get_mpc_mode() == 'blended', "Should be in blended mode during standstill" - - # Test 3: Lead car appears -> ACC mode - car_state = MockCarState(v_ego=20) # Reset car state - lead_one.status = True - lead_one.dRel = 50 # Safe distance - stabilize_filters() - assert not controller._has_dangerous_ttc, "Should not have dangerous TTC" - assert controller.get_mpc_mode() == 'acc', "Should be in ACC mode with safe lead distance" - - # Test 4: Dangerous TTC -> Blended mode - car_state = MockCarState(v_ego=20) # 72 kph - lead_one.status = True - lead_one.dRel = 50 # First establish normal lead detection - - # First establish lead detection - for _ in range(LEAD_WINDOW_SIZE + 1): - controller.update(not has_radar, car_state, lead_one, md, controls_state) - - assert controller._has_lead_filtered # Verify lead is detected - - # Now create dangerous TTC condition - lead_one.dRel = 20 # This creates a TTC of 1s, well below DANGEROUS_TTC - - for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): - controller.update(not has_radar, car_state, lead_one, md, controls_state) - - assert controller._has_dangerous_ttc, "Should detect dangerous TTC condition" - expected_mode = 'acc' if has_radar else 'blended' - assert controller.get_mpc_mode() == expected_mode, f"Should be in [{expected_mode}] mode with dangerous TTC" - -@pytest.mark.parametrize("has_radar", [True, False], ids=["with_radar", "without_radar"]) -def test_mpc_fcw_handling(controller, has_radar): - """Test MPC FCW crash count handling and mode transitions""" - car_state = MockCarState(v_ego=20) - lead_one = MockLeadOne() - md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE) - controls_state = MockControlState(v_cruise=72) - - # Test FCW activation - controller.set_mpc_fcw_crash_cnt(5) - for _ in range(MPC_FCW_WINDOW_SIZE + 1): - controller.update(not has_radar, car_state, lead_one, md, controls_state) - - assert controller._has_mpc_fcw - assert controller.get_mpc_mode() == 'blended' - - # Test FCW recovery - controller.set_mpc_fcw_crash_cnt(0) - for _ in range(MPC_FCW_WINDOW_SIZE + 1): - controller.update(not has_radar, car_state, lead_one, md, controls_state) - - assert not controller._has_mpc_fcw - -def test_radar_unavailable_handling(controller): - """Test behavior transitions between radar available and unavailable states""" - car_state = MockCarState(v_ego=27.78) # 100 kph - lead_one = MockLeadOne(status=True, d_rel=50) - md = MockModelData(x_vals=[0] * TRAJECTORY_SIZE, positions=[150] * TRAJECTORY_SIZE) - controls_state = MockControlState(v_cruise=100) - - # Test with radar available - for _ in range(LEAD_WINDOW_SIZE + 1): - controller.update(False, car_state, lead_one, md, controls_state) - radar_mode = controller.get_mpc_mode() - - # Test with radar unavailable - for _ in range(LEAD_WINDOW_SIZE + 1): - controller.update(True, car_state, lead_one, md, controls_state) - radarless_mode = controller.get_mpc_mode() - - assert radar_mode is not None - assert radarless_mode is not None diff --git a/system/manager/manager.py b/system/manager/manager.py index 499ed6474b..7a8ec9fc86 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -43,7 +43,6 @@ def manager_init() -> None: ] sunnypilot_default_params: list[tuple[str, str | bytes]] = [ - ("DynamicExperimentalControl", "0"), ("Mads", "1"), ("MadsMainCruiseAllowed", "1"), ("MadsPauseLateralOnBrake", "0"),