From c5524c6d429a7bb84563613b82480ac814576e0e Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sat, 4 Jan 2025 13:17:37 -0700 Subject: [PATCH 01/65] init dec --- cereal/custom.capnp | 13 +- cereal/log.capnp | 2 +- cereal/services.py | 1 + common/params.cc | 3 + selfdrive/car/card.py | 3 + selfdrive/controls/controlsd.py | 2 +- .../controls/lib/longitudinal_planner.py | 46 +++ selfdrive/controls/plannerd.py | 2 +- selfdrive/ui/qt/offroad/settings.cc | 6 + .../lib/dynamic_experimental_controller.py | 379 ++++++++++++++++++ system/manager/manager.py | 1 + 11 files changed, 454 insertions(+), 4 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 3ddeed75f6..729966b10b 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -8,6 +8,13 @@ $Cxx.namespace("cereal"); # cereal, so use these if you want custom events in your fork. # you can rename the struct, but don't change the identifier + +enum MpcSource { + acc @0; + blended @1; +} + +struct CustomReserved0 @0x81c2f05a394cf4af { struct SelfdriveStateSP @0x81c2f05a394cf4af { mads @0 :ModularAssistiveDrivingSystem; @@ -82,7 +89,11 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { } } -struct CustomReserved2 @0xf35cc4560bbf6ec2 { +struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { + e2eBlended @0 :Text; + e2eStatus @1 :Bool; + mpcSource @2 :MpcSource; + dynamicExperimentalControl @3 :Bool; } struct CustomReserved3 @0xda96579883444c35 { diff --git a/cereal/log.capnp b/cereal/log.capnp index d5fbad6fe8..80a5034392 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; - customReserved2 @109 :Custom.CustomReserved2; + longitudinalPlanSP @109 :Custom.LongitudinalPlanSP; customReserved3 @110 :Custom.CustomReserved3; customReserved4 @111 :Custom.CustomReserved4; customReserved5 @112 :Custom.CustomReserved5; diff --git a/cereal/services.py b/cereal/services.py index 346bf81a09..0528f5e862 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -77,6 +77,7 @@ _services: dict[str, tuple] = { # sunnypilot "modelManagerSP": (False, 1., 1), "selfdriveStateSP": (True, 100., 10), + "longitudinalPlanSP": (True, 20., 5), # debug "uiDebug": (True, 0., 1), diff --git a/common/params.cc b/common/params.cc index 7c1b21c00c..a2f0aeaa91 100644 --- a/common/params.cc +++ b/common/params.cc @@ -223,6 +223,9 @@ std::unordered_map keys = { {"SunnylinkDongleId", PERSISTENT}, {"SunnylinkdPid", PERSISTENT}, {"SunnylinkEnabled", PERSISTENT}, + {"EnableGithubRunner", PERSISTENT}, + + {"DynamicExperimentalControl", PERSISTENT}, }; } // namespace diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 1852dccf74..7341939144 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -76,6 +76,8 @@ class Car: self.CC_prev = car.CarControl.new_message() self.initialized_prev = False + self.dynamic_experimental_control = False + self.last_actuators_output = structs.CarControl.Actuators() self.params = Params() @@ -157,6 +159,7 @@ class Car: self.is_metric = self.params.get_bool("IsMetric") self.experimental_mode = self.params.get_bool("ExperimentalMode") + self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl") # card is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 9a230e4d83..2d76865369 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -38,7 +38,7 @@ class Controls: self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', - 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') + 'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'longitudinalPlanSP'], poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState']) self.steer_limited = False diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index eba8019117..71394f9b93 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -2,6 +2,8 @@ import math import numpy as np from openpilot.common.numpy_fast import clip, interp +from openpilot.common.params import Params +from cereal import custom import cereal.messaging as messaging from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX @@ -16,6 +18,9 @@ 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.dynamic_experimental_controller import DynamicExperimentalController + + 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] @@ -28,6 +33,7 @@ MIN_ALLOW_THROTTLE_SPEED = 2.5 _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] +MpcSource = custom.MpcSource def get_max_accel(v_ego): return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) @@ -84,6 +90,18 @@ class LongitudinalPlanner: self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 + self.params = Params() + self.param_read_counter = 0 + self.read_param() + + self.dynamic_experimental_controller = DynamicExperimentalController() + + def read_param(self): + try: + self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl")) + except AttributeError: + self.dynamic_experimental_controller = DynamicExperimentalController() + @staticmethod def parse_model(model_msg, model_error): if (len(model_msg.position.x) == ModelConstants.IDX_N and @@ -104,6 +122,17 @@ class LongitudinalPlanner: throttle_prob = 1.0 return x, v, a, j, throttle_prob + def update(self, sm): + if self.param_read_counter % 50 == 0: + self.read_param() + self.param_read_counter += 1 + if self.dynamic_experimental_controller.is_enabled() and sm['controlsState'].experimentalMode: + self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) + self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState']) #, sm['navInstruction'].maneuverDistance) + self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode() + else: + self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' + def update(self, sm): self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' @@ -206,3 +235,20 @@ class LongitudinalPlanner: longitudinalPlan.allowThrottle = self.allow_throttle pm.send('longitudinalPlan', plan_send) + + 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 + print(f"mpcSource: {longitudinalPlanSP.mpcSource}") + + longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled() + print(f"dynamicExperimentalControl: {longitudinalPlanSP.dynamicExperimentalControl}") + + + pm.send('longitudinalPlanSP', plan_sp_send) + diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index bcfc4d0c14..16ac80db60 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']) + pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP']) 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 4911dc71e9..58e5dffd5c 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -40,6 +40,12 @@ 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/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py new file mode 100644 index 0000000000..233fa498bb --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py @@ -0,0 +1,379 @@ +#!/usr/bin/env python3 +# 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 +from common.numpy_fast import interp +import numpy as np + +# 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): + 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): + if len(self.data) == 0: + return None + return self.total / len(self.data) + + def reset_data(self): + 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): + if len(self.data) == self.window_size: + self.data.pop(0) + self.data.append(value) + + def get_weighted_average(self): + if len(self.data) == 0: + return None + weighted_sum = np.dot(self.data, self.weights[-len(self.data):]) + weight_total = np.sum(self.weights[-len(self.data):]) + return weighted_sum / weight_total + + def reset_data(self): + self.data = [] + +class DynamicExperimentalController: + def __init__(self): + self._is_enabled = False + self._mode = 'acc' + self._mode_prev = 'acc' + self._mode_changed = False + self._frame = 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 + pass + + + def _adaptive_slowdown_threshold(self): + """ + Adapts the slow down threshold based on vehicle speed and recent behavior. + """ + return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.03 * np.log(1 + len(self._slow_down_gmac.data))) + + def _anomaly_detection(self, recent_data, threshold=2.0, context_check=True): + """ + Basic anomaly detection using standard deviation. + """ + if len(recent_data) < 5: + return False + mean = np.mean(recent_data) + std_dev = np.std(recent_data) + anomaly = recent_data[-1] > mean + threshold * std_dev + + # Context check to ensure repeated anomaly + if context_check: + return np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1 + return anomaly + + def _smoothed_lead_detection(self, lead_prob, smoothing_factor=0.2): + """ + Smoothing the lead detection to avoid erratic behavior. + """ + self._has_lead_filtered = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob + return self._has_lead_filtered > LEAD_PROB + + def _adaptive_lead_prob_threshold(self): + """ + Adapts lead probability threshold based on driving conditions. + """ + if self._v_ego_kph > HIGHWAY_CRUISE_KPH: + return LEAD_PROB + 0.1 # Increase the threshold on highways + return LEAD_PROB + + def _update(self, car_state, lead_one, md, controls_state): #, maneuver_distance): + self._v_ego_kph = car_state.vEgo * 3.6 + self._v_cruise_kph = controls_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 + self._frame += 1 + + def _radarless_mode(self): + # 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 dont 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): + # 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 dont 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 update(self, radar_unavailable, car_state, lead_one, md, controls_state): #, maneuver_distance): + if self._is_enabled: + self._update(car_state, lead_one, md, controls_state) #, maneuver_distance) + if radar_unavailable: + self._radarless_mode() + else: + self._radar_mode() + self._mode_changed = self._mode != self._mode_prev + self._mode_prev = self._mode + + def get_mpc_mode(self): + return self._mode + + def has_changed(self): + return self._mode_changed + + def set_enabled(self, enabled): + self._is_enabled = enabled + + def is_enabled(self): + return self._is_enabled + + def set_mpc_fcw_crash_cnt(self, crash_cnt): + self._mpc_fcw_crash_cnt = crash_cnt + + def _set_mode(self, mode): + 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 \ No newline at end of file diff --git a/system/manager/manager.py b/system/manager/manager.py index 7a8ec9fc86..96e8f6e858 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -40,6 +40,7 @@ def manager_init() -> None: ("LanguageSetting", "main_en"), ("OpenpilotEnabledToggle", "1"), ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)), + ("DynamicExperimentalControl", "0"), ] sunnypilot_default_params: list[tuple[str, str | bytes]] = [ From 75f6f737988ea8a9a8365525ba54d1bcc30ca7ad Mon Sep 17 00:00:00 2001 From: Kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sat, 4 Jan 2025 13:23:48 -0700 Subject: [PATCH 02/65] Update sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py Co-authored-by: sourcery-ai[bot] <58596630+sourcery-ai[bot]@users.noreply.github.com> --- .../selfdrive/controls/lib/dynamic_experimental_controller.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py index 233fa498bb..4c4cddd3de 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py @@ -75,9 +75,7 @@ class GenericMovingAverageCalculator: self.total += value def get_moving_average(self): - if len(self.data) == 0: - return None - return self.total / len(self.data) + return None if len(self.data) == 0 else self.total / len(self.data) def reset_data(self): self.data = [] From 3c434e78e223f24dba77623d0882abf3d0db055e Mon Sep 17 00:00:00 2001 From: Kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sat, 4 Jan 2025 13:24:02 -0700 Subject: [PATCH 03/65] Update sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py Co-authored-by: sourcery-ai[bot] <58596630+sourcery-ai[bot]@users.noreply.github.com> --- .../selfdrive/controls/lib/dynamic_experimental_controller.py | 1 - 1 file changed, 1 deletion(-) diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py index 4c4cddd3de..67ad16a83d 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py @@ -144,7 +144,6 @@ class DynamicExperimentalController: self._mpc_fcw_crash_cnt = 0 self._set_mode_timeout = 0 - pass def _adaptive_slowdown_threshold(self): From 90435f0a92bfcf7008db33b6be306ceff6ac67c3 Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sat, 4 Jan 2025 15:34:45 -0700 Subject: [PATCH 04/65] fix static test --- selfdrive/controls/lib/longitudinal_planner.py | 6 ++---- .../controls/lib/dynamic_experimental_controller.py | 7 +++---- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 71394f9b93..4f691f9278 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -128,14 +128,12 @@ class LongitudinalPlanner: self.param_read_counter += 1 if self.dynamic_experimental_controller.is_enabled() and sm['controlsState'].experimentalMode: self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) - self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState']) #, sm['navInstruction'].maneuverDistance) + self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState']) + #, sm['navInstruction'].maneuverDistance) self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode() else: self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' - def update(self, sm): - self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' - if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) else: diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py index 67ad16a83d..7249cdfad9 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py @@ -1,4 +1,3 @@ -#!/usr/bin/env python3 # The MIT License # # Copyright (c) 2019-, Rick Lan, dragonpilot community, and a number of other of contributors. @@ -22,7 +21,7 @@ # THE SOFTWARE. # # Version = 2024-7-11 -from common.numpy_fast import interp +from openpilot.common.numpy_fast import interp import numpy as np # d-e2e, from modeldata.h @@ -262,7 +261,7 @@ class DynamicExperimentalController: # return # when blinker is on and speed is driving below V_ACC_MIN: blended - # we dont want it to switch mode at higher speed, blended may trigger hard brake + # 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 @@ -312,7 +311,7 @@ class DynamicExperimentalController: return # when blinker is on and speed is driving below V_ACC_MIN: blended - # we dont want it to switch mode at higher speed, blended may trigger hard brake + # 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 From c4ed5a4617860ef3b338f4f62c2ba29aeb7bb343 Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sat, 4 Jan 2025 15:46:12 -0700 Subject: [PATCH 05/65] ff --- 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 ++++++++ .../controls/lib/dynamic_experimental_controller.py | 2 +- 12 files changed, 89 insertions(+), 1 deletion(-) diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts index 5dbd71ff19..55a0b04ea5 100644 --- a/selfdrive/ui/translations/main_ar.ts +++ b/selfdrive/ui/translations/main_ar.ts @@ -1372,6 +1372,14 @@ 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 9d6d626552..e9b9a802a5 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -1356,6 +1356,14 @@ 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 773704061a..c53d8c22f4 100644 --- a/selfdrive/ui/translations/main_es.ts +++ b/selfdrive/ui/translations/main_es.ts @@ -1356,6 +1356,14 @@ 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 4173a6218a..e08f82125a 100644 --- a/selfdrive/ui/translations/main_fr.ts +++ b/selfdrive/ui/translations/main_fr.ts @@ -1356,6 +1356,14 @@ 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 5e0ac26d29..e004d7867c 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -1350,6 +1350,14 @@ 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 899f92c033..8f30f2c995 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -1352,6 +1352,14 @@ 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 997e202cc2..ca4e2b73c4 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -1356,6 +1356,14 @@ 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 54221383b4..ed471800fe 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -1352,6 +1352,14 @@ 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 9deae8ba4f..689168dd4c 100644 --- a/selfdrive/ui/translations/main_tr.ts +++ b/selfdrive/ui/translations/main_tr.ts @@ -1350,6 +1350,14 @@ 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 30550519a5..bcdcc0dc15 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -1352,6 +1352,14 @@ 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 f5e89aa4db..9a1fea1138 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -1352,6 +1352,14 @@ 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/controls/lib/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py index 7249cdfad9..63fe4b7922 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py @@ -372,4 +372,4 @@ class DynamicExperimentalController: self._set_mode_timeout = SET_MODE_TIMEOUT if self._set_mode_timeout > 0: - self._set_mode_timeout -= 1 \ No newline at end of file + self._set_mode_timeout -= 1 From 91922fb9b668514923c2f93ac385e516fbebca89 Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sat, 4 Jan 2025 15:49:27 -0700 Subject: [PATCH 06/65] fix static test --- selfdrive/controls/lib/longitudinal_planner.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4f691f9278..b803bd51d1 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -126,13 +126,13 @@ class LongitudinalPlanner: if self.param_read_counter % 50 == 0: self.read_param() self.param_read_counter += 1 - if self.dynamic_experimental_controller.is_enabled() and sm['controlsState'].experimentalMode: + if self.dynamic_experimental_controller.is_enabled() and sm['selfdriveState'].experimentalMode: self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState']) #, sm['navInstruction'].maneuverDistance) self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode() else: - self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' + self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) From 1dfaf347f84cfc53e17d1d8a9f31ec323336fe4c Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sat, 11 Jan 2025 13:11:40 -0700 Subject: [PATCH 07/65] unitee testt --- .../lib/tests/test_dynamic_controller.py | 253 ++++++++++++++++++ 1 file changed, 253 insertions(+) create mode 100644 sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py new file mode 100644 index 0000000000..4ea956dcc7 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py @@ -0,0 +1,253 @@ +from sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import ( + DynamicExperimentalController, + TRAJECTORY_SIZE, + LEAD_WINDOW_SIZE, + SLOW_DOWN_WINDOW_SIZE, + DANGEROUS_TTC_WINDOW_SIZE, + MPC_FCW_WINDOW_SIZE, + SNG_State, + SLOWNESS_CRUISE_OFFSET, + SLOW_DOWN_BP, + SLOW_DOWN_DIST +) + +import unittest +import numpy as np +from unittest.mock import MagicMock, patch + +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 = MagicMock() + self.position = MagicMock() + if x_vals is not None: + self.orientation.x = x_vals + if positions is not None: + self.position.x = positions + +class MockControlState: + def __init__(self, v_cruise=0): + self.vCruise = v_cruise + +class TestDynamicExperimentalController(unittest.TestCase): + def setUp(self): + """Set up test environment before each test case""" + patcher = patch('openpilot.common.numpy_fast.interp', new_callable=MockInterp) + self.addCleanup(patcher.stop) + patcher.start() + + self.controller = DynamicExperimentalController() + self.controller.set_enabled(True) + + def test_initial_state(self): + """Test initial state of the controller""" + self.assertEqual(self.controller._mode, 'acc') + self.assertFalse(self.controller._has_lead) + self.assertFalse(self.controller._has_standstill) + self.assertEqual(self.controller._sng_state, SNG_State.off) + self.assertFalse(self.controller._has_lead_filtered) + self.assertFalse(self.controller._has_slow_down) + self.assertFalse(self.controller._has_dangerous_ttc) + self.assertFalse(self.controller._has_mpc_fcw) + + def test_standstill_detection(self): + """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 + self.controller.update(False, car_state, lead_one, md, controls_state) + self.assertEqual(self.controller._sng_state, SNG_State.stopped) + self.assertEqual(self.controller.get_mpc_mode(), 'blended') + + # Test transition from standstill to moving + car_state.standstill = False + self.controller.update(False, car_state, lead_one, md, controls_state) + self.assertEqual(self.controller._sng_state, SNG_State.going) + + # Test complete transition to normal driving + for _ in range(STOP_AND_GO_FRAME + 1): + self.controller.update(False, car_state, lead_one, md, controls_state) + self.assertEqual(self.controller._sng_state, SNG_State.off) + + def test_lead_detection(self): + """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): + self.controller.update(False, car_state, lead_one, md, controls_state) + + self.assertTrue(self.controller._has_lead_filtered) + self.assertEqual(self.controller.get_mpc_mode(), 'acc') + + # Test lead loss detection + lead_one.status = False + for _ in range(LEAD_WINDOW_SIZE + 1): + self.controller.update(False, car_state, lead_one, md, controls_state) + + self.assertFalse(self.controller._has_lead_filtered) + + def test_slow_down_detection(self): + """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): + self.controller.update(False, car_state, lead_one, md, controls_state) + + self.assertTrue(self.controller._has_slow_down) + self.assertEqual(self.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): + self.controller.update(False, car_state, lead_one, md, controls_state) + + self.assertFalse(self.controller._has_slow_down) + + def test_dangerous_ttc_detection(self): + """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 + lead_one.dRel = 100 # Safe distance + for _ in range(DANGEROUS_TTC_WINDOW_SIZE + 1): + self.controller.update(False, car_state, lead_one, md, controls_state) + + self.assertFalse(self.controller._has_dangerous_ttc) + + # 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): + self.controller.update(False, car_state, lead_one, md, controls_state) + + self.assertTrue(self.controller._has_dangerous_ttc, + f"TTC of 1s should be considered dangerous (threshold: {DANGEROUS_TTC}s)") + self.assertEqual(self.controller.get_mpc_mode(), 'blended', + "Should be in blended mode with dangerous TTC") + + def test_mode_transitions(self): + """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): + self.controller.update(False, car_state, lead_one, md, controls_state) + + # Test 1: Normal driving -> ACC mode + stabilize_filters() + self.assertEqual(self.controller.get_mpc_mode(), 'acc', + "Should be in ACC mode under normal driving conditions") + + # Test 2: Standstill -> Blended mode + car_state.standstill = True + self.controller.update(False, car_state, lead_one, md, controls_state) + self.assertEqual(self.controller.get_mpc_mode(), 'blended', + "Should be in blended mode during standstill") + + # Test 3: Lead car appears -> ACC mode + car_state = MockCarState(v_ego=25) # Reset car state + lead_one.status = True + lead_one.dRel = 50 + stabilize_filters() + self.assertEqual(self.controller.get_mpc_mode(), 'acc', + "Should be in ACC mode with safe lead distance") + + # Test 4: Dangerous TTC -> Blended mode + # Set up conditions that will definitely trigger dangerous TTC + car_state = MockCarState(v_ego=20) # 72 kph + lead_one.status = True + lead_one.dRel = 20 # This creates a TTC of 1s, well below DANGEROUS_TTC + + # Need more updates to allow the weighted average to stabilize + for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): + self.controller.update(False, car_state, lead_one, md, controls_state) + + self.assertTrue(self.controller._has_dangerous_ttc, + "Should detect dangerous TTC condition") + self.assertEqual(self.controller.get_mpc_mode(), 'blended', + "Should be in blended mode with dangerous TTC") + + def test_mpc_fcw_handling(self): + """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 + self.controller.set_mpc_fcw_crash_cnt(5) + for _ in range(MPC_FCW_WINDOW_SIZE + 1): + self.controller.update(False, car_state, lead_one, md, controls_state) + + self.assertTrue(self.controller._has_mpc_fcw) + self.assertEqual(self.controller.get_mpc_mode(), 'blended') + + # Test FCW recovery + self.controller.set_mpc_fcw_crash_cnt(0) + for _ in range(MPC_FCW_WINDOW_SIZE + 1): + self.controller.update(False, car_state, lead_one, md, controls_state) + + self.assertFalse(self.controller._has_mpc_fcw) + + def test_radar_unavailable_handling(self): + """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): + self.controller.update(False, car_state, lead_one, md, controls_state) + radar_mode = self.controller.get_mpc_mode() + + # Test with radar unavailable + for _ in range(LEAD_WINDOW_SIZE + 1): + self.controller.update(True, car_state, lead_one, md, controls_state) + radarless_mode = self.controller.get_mpc_mode() + + self.assertIsNotNone(radar_mode) + self.assertIsNotNone(radarless_mode) + +if __name__ == '__main__': + unittest.main() \ No newline at end of file From ba9fbd52d725bc3f756bc82cdcb0d789a3e35d53 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sat, 11 Jan 2025 21:42:06 +0100 Subject: [PATCH 08/65] Refactor test_dynamic_controller and fix formatting issues Added a new import for STOP_AND_GO_FRAME and corrected a float initialization for v_ego in MockCarState. Also fixed indentation in the test_standstill_detection method for consistency. --- .../selfdrive/controls/lib/tests/test_dynamic_controller.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py index 4ea956dcc7..077071f82f 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py @@ -8,7 +8,7 @@ from sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import ( SNG_State, SLOWNESS_CRUISE_OFFSET, SLOW_DOWN_BP, - SLOW_DOWN_DIST + SLOW_DOWN_DIST, STOP_AND_GO_FRAME ) import unittest @@ -20,7 +20,7 @@ class MockInterp: return np.interp(x, xp, fp) class MockCarState: - def __init__(self, v_ego=0, standstill=False, left_blinker=False, right_blinker=False): + 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 @@ -65,7 +65,7 @@ class TestDynamicExperimentalController(unittest.TestCase): self.assertFalse(self.controller._has_dangerous_ttc) self.assertFalse(self.controller._has_mpc_fcw) - def test_standstill_detection(self): + def test_standstill_detection(self): """Test standstill detection and state transitions""" car_state = MockCarState(standstill=True) lead_one = MockLeadOne() From 684431e3f69cb228dc8957d42d7db4aec73056e0 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 13:06:15 +0100 Subject: [PATCH 09/65] Refactor test indentation for dynamic controller tests Adjust indentation and formatting in test_dynamic_controller.py to ensure consistency and readability. This change does not alter functionality but improves the maintainability of the test code. --- .../lib/tests/test_dynamic_controller.py | 134 +++++++++--------- 1 file changed, 67 insertions(+), 67 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py index 077071f82f..0eeabbc72d 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py @@ -132,80 +132,80 @@ class TestDynamicExperimentalController(unittest.TestCase): self.assertFalse(self.controller._has_slow_down) - def test_dangerous_ttc_detection(self): - """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) + def test_dangerous_ttc_detection(self): + """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 - lead_one.dRel = 100 # Safe distance - for _ in range(DANGEROUS_TTC_WINDOW_SIZE + 1): - self.controller.update(False, car_state, lead_one, md, controls_state) - - self.assertFalse(self.controller._has_dangerous_ttc) - - # 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): - self.controller.update(False, car_state, lead_one, md, controls_state) - - self.assertTrue(self.controller._has_dangerous_ttc, - f"TTC of 1s should be considered dangerous (threshold: {DANGEROUS_TTC}s)") - self.assertEqual(self.controller.get_mpc_mode(), 'blended', - "Should be in blended mode with dangerous TTC") - - def test_mode_transitions(self): - """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): - self.controller.update(False, car_state, lead_one, md, controls_state) - - # Test 1: Normal driving -> ACC mode - stabilize_filters() - self.assertEqual(self.controller.get_mpc_mode(), 'acc', - "Should be in ACC mode under normal driving conditions") - - # Test 2: Standstill -> Blended mode - car_state.standstill = True + # First establish normal conditions + lead_one.dRel = 100 # Safe distance + for _ in range(DANGEROUS_TTC_WINDOW_SIZE + 1): self.controller.update(False, car_state, lead_one, md, controls_state) - self.assertEqual(self.controller.get_mpc_mode(), 'blended', - "Should be in blended mode during standstill") - # Test 3: Lead car appears -> ACC mode - car_state = MockCarState(v_ego=25) # Reset car state - lead_one.status = True - lead_one.dRel = 50 - stabilize_filters() - self.assertEqual(self.controller.get_mpc_mode(), 'acc', - "Should be in ACC mode with safe lead distance") + self.assertFalse(self.controller._has_dangerous_ttc) - # Test 4: Dangerous TTC -> Blended mode - # Set up conditions that will definitely trigger dangerous TTC - car_state = MockCarState(v_ego=20) # 72 kph - lead_one.status = True - lead_one.dRel = 20 # This creates a TTC of 1s, well below DANGEROUS_TTC + # 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 more updates to allow the weighted average to stabilize - for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): + # Need to update multiple times to allow the weighted average to stabilize + for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): + self.controller.update(False, car_state, lead_one, md, controls_state) + + self.assertTrue(self.controller._has_dangerous_ttc, + f"TTC of 1s should be considered dangerous (threshold: {DANGEROUS_TTC}s)") + self.assertEqual(self.controller.get_mpc_mode(), 'blended', + "Should be in blended mode with dangerous TTC") + + def test_mode_transitions(self): + """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): self.controller.update(False, car_state, lead_one, md, controls_state) - self.assertTrue(self.controller._has_dangerous_ttc, - "Should detect dangerous TTC condition") - self.assertEqual(self.controller.get_mpc_mode(), 'blended', - "Should be in blended mode with dangerous TTC") + # Test 1: Normal driving -> ACC mode + stabilize_filters() + self.assertEqual(self.controller.get_mpc_mode(), 'acc', + "Should be in ACC mode under normal driving conditions") + + # Test 2: Standstill -> Blended mode + car_state.standstill = True + self.controller.update(False, car_state, lead_one, md, controls_state) + self.assertEqual(self.controller.get_mpc_mode(), 'blended', + "Should be in blended mode during standstill") + + # Test 3: Lead car appears -> ACC mode + car_state = MockCarState(v_ego=25) # Reset car state + lead_one.status = True + lead_one.dRel = 50 + stabilize_filters() + self.assertEqual(self.controller.get_mpc_mode(), 'acc', + "Should be in ACC mode with safe lead distance") + + # Test 4: Dangerous TTC -> Blended mode + # Set up conditions that will definitely trigger dangerous TTC + car_state = MockCarState(v_ego=20) # 72 kph + lead_one.status = True + lead_one.dRel = 20 # This creates a TTC of 1s, well below DANGEROUS_TTC + + # Need more updates to allow the weighted average to stabilize + for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): + self.controller.update(False, car_state, lead_one, md, controls_state) + + self.assertTrue(self.controller._has_dangerous_ttc, + "Should detect dangerous TTC condition") + self.assertEqual(self.controller.get_mpc_mode(), 'blended', + "Should be in blended mode with dangerous TTC") def test_mpc_fcw_handling(self): """Test MPC FCW crash count handling and mode transitions""" From 2fb0545344134a1dcad3c0eeabaad121ef885de7 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 13:09:48 +0100 Subject: [PATCH 10/65] Migrated to pytest using claude --- .../lib/tests/pytest_dynamic_controller.py | 249 ++++++++++++++++++ 1 file changed, 249 insertions(+) create mode 100644 sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py diff --git a/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py new file mode 100644 index 0000000000..cc405aa2bd --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py @@ -0,0 +1,249 @@ +from sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller 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 unittest.mock import MagicMock, patch + +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 = MagicMock() + self.position = MagicMock() + if x_vals is not None: + self.orientation.x = x_vals + if positions is not None: + self.position.x = positions + +class MockControlState: + def __init__(self, v_cruise=0): + self.vCruise = v_cruise + +@pytest.fixture +def interp(): + with patch('openpilot.common.numpy_fast.interp', new_callable=MockInterp) as mock: + yield mock + +@pytest.fixture +def controller(interp): + controller = DynamicExperimentalController() + controller.set_enabled(True) + 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 + +def test_standstill_detection(controller): + """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(False, 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(False, 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(False, car_state, lead_one, md, controls_state) + assert controller._sng_state == SNG_State.off + +def test_lead_detection(controller): + """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(False, car_state, lead_one, md, controls_state) + + assert controller._has_lead_filtered + assert controller.get_mpc_mode() == 'acc' + + # Test lead loss detection + lead_one.status = False + for _ in range(LEAD_WINDOW_SIZE + 1): + controller.update(False, car_state, lead_one, md, controls_state) + + assert not controller._has_lead_filtered + +def test_slow_down_detection(controller): + """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(False, 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(False, car_state, lead_one, md, controls_state) + + assert not controller._has_slow_down + +def test_dangerous_ttc_detection(controller): + """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(False, 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(False, car_state, lead_one, md, controls_state) + + assert controller._has_dangerous_ttc, f"TTC of 1s should be considered dangerous" + assert controller.get_mpc_mode() == 'blended', "Should be in blended mode with dangerous TTC" + +def test_mode_transitions(controller): + """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(False, 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(False, 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=25) # Reset car state + lead_one.status = True + lead_one.dRel = 50 + stabilize_filters() + 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(False, 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(False, car_state, lead_one, md, controls_state) + + assert controller._has_dangerous_ttc, "Should detect dangerous TTC condition" + assert controller.get_mpc_mode() == 'blended', "Should be in blended mode with dangerous TTC" + +def test_mpc_fcw_handling(controller): + """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(False, 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(False, 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 \ No newline at end of file From 5e62ccad1210b74585e8a116548ea8262ed4aa1c Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 13:28:59 +0100 Subject: [PATCH 11/65] Integrate radar parameter into dynamic controller's pytest tests Added a `has_radar` parameter to the test functions in the dynamic controller's pytest file. This allows each function to run both with and without radar inputs, thus enhancing the coverage of our test cases. --- .../lib/tests/pytest_dynamic_controller.py | 64 +++++++++++-------- 1 file changed, 37 insertions(+), 27 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py index cc405aa2bd..85319f7a1c 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py @@ -64,7 +64,8 @@ def test_initial_state(controller): assert not controller._has_dangerous_ttc assert not controller._has_mpc_fcw -def test_standstill_detection(controller): +@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() @@ -72,21 +73,22 @@ def test_standstill_detection(controller): controls_state = MockControlState() # Test transition to standstill - controller.update(False, car_state, lead_one, md, controls_state) + 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(False, car_state, lead_one, md, controls_state) + 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(False, car_state, lead_one, md, controls_state) + controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._sng_state == SNG_State.off -def test_lead_detection(controller): +@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 @@ -95,19 +97,21 @@ def test_lead_detection(controller): # Let moving average stabilize for _ in range(LEAD_WINDOW_SIZE + 1): - controller.update(False, car_state, lead_one, md, controls_state) + controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_lead_filtered - assert controller.get_mpc_mode() == 'acc' + 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(False, car_state, lead_one, md, controls_state) + controller.update(not has_radar, car_state, lead_one, md, controls_state) assert not controller._has_lead_filtered -def test_slow_down_detection(controller): +@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() @@ -118,7 +122,7 @@ def test_slow_down_detection(controller): # Test slow down detection for _ in range(SLOW_DOWN_WINDOW_SIZE + 1): - controller.update(False, car_state, lead_one, md, controls_state) + controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_slow_down assert controller.get_mpc_mode() == 'blended' @@ -127,11 +131,12 @@ def test_slow_down_detection(controller): 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(False, car_state, lead_one, md, controls_state) + controller.update(not has_radar, car_state, lead_one, md, controls_state) assert not controller._has_slow_down -def test_dangerous_ttc_detection(controller): +@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) @@ -141,7 +146,7 @@ def test_dangerous_ttc_detection(controller): # 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(False, car_state, lead_one, md, controls_state) + controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_lead_filtered # Verify lead is detected @@ -151,15 +156,17 @@ def test_dangerous_ttc_detection(controller): # Need to update multiple times to allow the weighted average to stabilize for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): - controller.update(False, car_state, lead_one, md, controls_state) + controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_dangerous_ttc, f"TTC of 1s should be considered dangerous" - assert controller.get_mpc_mode() == 'blended', "Should be in blended mode with dangerous TTC" + 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" -def test_mode_transitions(controller): +@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 + 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) @@ -168,7 +175,7 @@ def test_mode_transitions(controller): """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(False, car_state, lead_one, md, controls_state) + controller.update(not has_radar, car_state, lead_one, md, controls_state) # Test 1: Normal driving -> ACC mode stabilize_filters() @@ -176,14 +183,15 @@ def test_mode_transitions(controller): # Test 2: Standstill -> Blended mode car_state.standstill = True - controller.update(False, car_state, lead_one, md, controls_state) + 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=25) # Reset car state + car_state = MockCarState(v_ego=20) # Reset car state lead_one.status = True - lead_one.dRel = 50 + lead_one.dRel = 50 # Safe distance stabilize_filters() + assert controller._has_dangerous_ttc == False, "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 @@ -193,7 +201,7 @@ def test_mode_transitions(controller): # First establish lead detection for _ in range(LEAD_WINDOW_SIZE + 1): - controller.update(False, car_state, lead_one, md, controls_state) + controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_lead_filtered # Verify lead is detected @@ -201,12 +209,14 @@ def test_mode_transitions(controller): lead_one.dRel = 20 # This creates a TTC of 1s, well below DANGEROUS_TTC for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): - controller.update(False, car_state, lead_one, md, controls_state) + controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_dangerous_ttc, "Should detect dangerous TTC condition" - assert controller.get_mpc_mode() == 'blended', "Should be in blended mode with dangerous TTC" + 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" -def test_mpc_fcw_handling(controller): +@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() @@ -216,7 +226,7 @@ def test_mpc_fcw_handling(controller): # Test FCW activation controller.set_mpc_fcw_crash_cnt(5) for _ in range(MPC_FCW_WINDOW_SIZE + 1): - controller.update(False, car_state, lead_one, md, controls_state) + controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_mpc_fcw assert controller.get_mpc_mode() == 'blended' @@ -224,7 +234,7 @@ def test_mpc_fcw_handling(controller): # Test FCW recovery controller.set_mpc_fcw_crash_cnt(0) for _ in range(MPC_FCW_WINDOW_SIZE + 1): - controller.update(False, car_state, lead_one, md, controls_state) + controller.update(not has_radar, car_state, lead_one, md, controls_state) assert not controller._has_mpc_fcw From 8dd750fd83b45a24b9d4ce77e564f6685b8e7273 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 13:48:52 +0100 Subject: [PATCH 12/65] Disabling unittest file to allow checks on the pipeline to succeed. Pending to remove this, but leaving it to validate the move to pytest is okay before merging --- .../lib/tests/test_dynamic_controller.py | 506 +++++++++--------- 1 file changed, 253 insertions(+), 253 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py index 0eeabbc72d..0ce0454d4e 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py @@ -1,253 +1,253 @@ -from sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import ( - DynamicExperimentalController, - TRAJECTORY_SIZE, - LEAD_WINDOW_SIZE, - SLOW_DOWN_WINDOW_SIZE, - DANGEROUS_TTC_WINDOW_SIZE, - MPC_FCW_WINDOW_SIZE, - SNG_State, - SLOWNESS_CRUISE_OFFSET, - SLOW_DOWN_BP, - SLOW_DOWN_DIST, STOP_AND_GO_FRAME -) - -import unittest -import numpy as np -from unittest.mock import MagicMock, patch - -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 = MagicMock() - self.position = MagicMock() - if x_vals is not None: - self.orientation.x = x_vals - if positions is not None: - self.position.x = positions - -class MockControlState: - def __init__(self, v_cruise=0): - self.vCruise = v_cruise - -class TestDynamicExperimentalController(unittest.TestCase): - def setUp(self): - """Set up test environment before each test case""" - patcher = patch('openpilot.common.numpy_fast.interp', new_callable=MockInterp) - self.addCleanup(patcher.stop) - patcher.start() - - self.controller = DynamicExperimentalController() - self.controller.set_enabled(True) - - def test_initial_state(self): - """Test initial state of the controller""" - self.assertEqual(self.controller._mode, 'acc') - self.assertFalse(self.controller._has_lead) - self.assertFalse(self.controller._has_standstill) - self.assertEqual(self.controller._sng_state, SNG_State.off) - self.assertFalse(self.controller._has_lead_filtered) - self.assertFalse(self.controller._has_slow_down) - self.assertFalse(self.controller._has_dangerous_ttc) - self.assertFalse(self.controller._has_mpc_fcw) - - def test_standstill_detection(self): - """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 - self.controller.update(False, car_state, lead_one, md, controls_state) - self.assertEqual(self.controller._sng_state, SNG_State.stopped) - self.assertEqual(self.controller.get_mpc_mode(), 'blended') - - # Test transition from standstill to moving - car_state.standstill = False - self.controller.update(False, car_state, lead_one, md, controls_state) - self.assertEqual(self.controller._sng_state, SNG_State.going) - - # Test complete transition to normal driving - for _ in range(STOP_AND_GO_FRAME + 1): - self.controller.update(False, car_state, lead_one, md, controls_state) - self.assertEqual(self.controller._sng_state, SNG_State.off) - - def test_lead_detection(self): - """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): - self.controller.update(False, car_state, lead_one, md, controls_state) - - self.assertTrue(self.controller._has_lead_filtered) - self.assertEqual(self.controller.get_mpc_mode(), 'acc') - - # Test lead loss detection - lead_one.status = False - for _ in range(LEAD_WINDOW_SIZE + 1): - self.controller.update(False, car_state, lead_one, md, controls_state) - - self.assertFalse(self.controller._has_lead_filtered) - - def test_slow_down_detection(self): - """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): - self.controller.update(False, car_state, lead_one, md, controls_state) - - self.assertTrue(self.controller._has_slow_down) - self.assertEqual(self.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): - self.controller.update(False, car_state, lead_one, md, controls_state) - - self.assertFalse(self.controller._has_slow_down) - - def test_dangerous_ttc_detection(self): - """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 - lead_one.dRel = 100 # Safe distance - for _ in range(DANGEROUS_TTC_WINDOW_SIZE + 1): - self.controller.update(False, car_state, lead_one, md, controls_state) - - self.assertFalse(self.controller._has_dangerous_ttc) - - # 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): - self.controller.update(False, car_state, lead_one, md, controls_state) - - self.assertTrue(self.controller._has_dangerous_ttc, - f"TTC of 1s should be considered dangerous (threshold: {DANGEROUS_TTC}s)") - self.assertEqual(self.controller.get_mpc_mode(), 'blended', - "Should be in blended mode with dangerous TTC") - - def test_mode_transitions(self): - """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): - self.controller.update(False, car_state, lead_one, md, controls_state) - - # Test 1: Normal driving -> ACC mode - stabilize_filters() - self.assertEqual(self.controller.get_mpc_mode(), 'acc', - "Should be in ACC mode under normal driving conditions") - - # Test 2: Standstill -> Blended mode - car_state.standstill = True - self.controller.update(False, car_state, lead_one, md, controls_state) - self.assertEqual(self.controller.get_mpc_mode(), 'blended', - "Should be in blended mode during standstill") - - # Test 3: Lead car appears -> ACC mode - car_state = MockCarState(v_ego=25) # Reset car state - lead_one.status = True - lead_one.dRel = 50 - stabilize_filters() - self.assertEqual(self.controller.get_mpc_mode(), 'acc', - "Should be in ACC mode with safe lead distance") - - # Test 4: Dangerous TTC -> Blended mode - # Set up conditions that will definitely trigger dangerous TTC - car_state = MockCarState(v_ego=20) # 72 kph - lead_one.status = True - lead_one.dRel = 20 # This creates a TTC of 1s, well below DANGEROUS_TTC - - # Need more updates to allow the weighted average to stabilize - for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): - self.controller.update(False, car_state, lead_one, md, controls_state) - - self.assertTrue(self.controller._has_dangerous_ttc, - "Should detect dangerous TTC condition") - self.assertEqual(self.controller.get_mpc_mode(), 'blended', - "Should be in blended mode with dangerous TTC") - - def test_mpc_fcw_handling(self): - """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 - self.controller.set_mpc_fcw_crash_cnt(5) - for _ in range(MPC_FCW_WINDOW_SIZE + 1): - self.controller.update(False, car_state, lead_one, md, controls_state) - - self.assertTrue(self.controller._has_mpc_fcw) - self.assertEqual(self.controller.get_mpc_mode(), 'blended') - - # Test FCW recovery - self.controller.set_mpc_fcw_crash_cnt(0) - for _ in range(MPC_FCW_WINDOW_SIZE + 1): - self.controller.update(False, car_state, lead_one, md, controls_state) - - self.assertFalse(self.controller._has_mpc_fcw) - - def test_radar_unavailable_handling(self): - """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): - self.controller.update(False, car_state, lead_one, md, controls_state) - radar_mode = self.controller.get_mpc_mode() - - # Test with radar unavailable - for _ in range(LEAD_WINDOW_SIZE + 1): - self.controller.update(True, car_state, lead_one, md, controls_state) - radarless_mode = self.controller.get_mpc_mode() - - self.assertIsNotNone(radar_mode) - self.assertIsNotNone(radarless_mode) - -if __name__ == '__main__': - unittest.main() \ No newline at end of file +# from sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import ( +# DynamicExperimentalController, +# TRAJECTORY_SIZE, +# LEAD_WINDOW_SIZE, +# SLOW_DOWN_WINDOW_SIZE, +# DANGEROUS_TTC_WINDOW_SIZE, +# MPC_FCW_WINDOW_SIZE, +# SNG_State, +# SLOWNESS_CRUISE_OFFSET, +# SLOW_DOWN_BP, +# SLOW_DOWN_DIST, STOP_AND_GO_FRAME +# ) +# +# import unittest +# import numpy as np +# from unittest.mock import MagicMock, patch +# +# 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 = MagicMock() +# self.position = MagicMock() +# if x_vals is not None: +# self.orientation.x = x_vals +# if positions is not None: +# self.position.x = positions +# +# class MockControlState: +# def __init__(self, v_cruise=0): +# self.vCruise = v_cruise +# +# class TestDynamicExperimentalController(unittest.TestCase): +# def setUp(self): +# """Set up test environment before each test case""" +# patcher = patch('openpilot.common.numpy_fast.interp', new_callable=MockInterp) +# self.addCleanup(patcher.stop) +# patcher.start() +# +# self.controller = DynamicExperimentalController() +# self.controller.set_enabled(True) +# +# def test_initial_state(self): +# """Test initial state of the controller""" +# self.assertEqual(self.controller._mode, 'acc') +# self.assertFalse(self.controller._has_lead) +# self.assertFalse(self.controller._has_standstill) +# self.assertEqual(self.controller._sng_state, SNG_State.off) +# self.assertFalse(self.controller._has_lead_filtered) +# self.assertFalse(self.controller._has_slow_down) +# self.assertFalse(self.controller._has_dangerous_ttc) +# self.assertFalse(self.controller._has_mpc_fcw) +# +# def test_standstill_detection(self): +# """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 +# self.controller.update(False, car_state, lead_one, md, controls_state) +# self.assertEqual(self.controller._sng_state, SNG_State.stopped) +# self.assertEqual(self.controller.get_mpc_mode(), 'blended') +# +# # Test transition from standstill to moving +# car_state.standstill = False +# self.controller.update(False, car_state, lead_one, md, controls_state) +# self.assertEqual(self.controller._sng_state, SNG_State.going) +# +# # Test complete transition to normal driving +# for _ in range(STOP_AND_GO_FRAME + 1): +# self.controller.update(False, car_state, lead_one, md, controls_state) +# self.assertEqual(self.controller._sng_state, SNG_State.off) +# +# def test_lead_detection(self): +# """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): +# self.controller.update(False, car_state, lead_one, md, controls_state) +# +# self.assertTrue(self.controller._has_lead_filtered) +# self.assertEqual(self.controller.get_mpc_mode(), 'acc') +# +# # Test lead loss detection +# lead_one.status = False +# for _ in range(LEAD_WINDOW_SIZE + 1): +# self.controller.update(False, car_state, lead_one, md, controls_state) +# +# self.assertFalse(self.controller._has_lead_filtered) +# +# def test_slow_down_detection(self): +# """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): +# self.controller.update(False, car_state, lead_one, md, controls_state) +# +# self.assertTrue(self.controller._has_slow_down) +# self.assertEqual(self.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): +# self.controller.update(False, car_state, lead_one, md, controls_state) +# +# self.assertFalse(self.controller._has_slow_down) +# +# def test_dangerous_ttc_detection(self): +# """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 +# lead_one.dRel = 100 # Safe distance +# for _ in range(DANGEROUS_TTC_WINDOW_SIZE + 1): +# self.controller.update(False, car_state, lead_one, md, controls_state) +# +# self.assertFalse(self.controller._has_dangerous_ttc) +# +# # 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): +# self.controller.update(False, car_state, lead_one, md, controls_state) +# +# self.assertTrue(self.controller._has_dangerous_ttc, +# f"TTC of 1s should be considered dangerous (threshold: {DANGEROUS_TTC}s)") +# self.assertEqual(self.controller.get_mpc_mode(), 'blended', +# "Should be in blended mode with dangerous TTC") +# +# def test_mode_transitions(self): +# """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): +# self.controller.update(False, car_state, lead_one, md, controls_state) +# +# # Test 1: Normal driving -> ACC mode +# stabilize_filters() +# self.assertEqual(self.controller.get_mpc_mode(), 'acc', +# "Should be in ACC mode under normal driving conditions") +# +# # Test 2: Standstill -> Blended mode +# car_state.standstill = True +# self.controller.update(False, car_state, lead_one, md, controls_state) +# self.assertEqual(self.controller.get_mpc_mode(), 'blended', +# "Should be in blended mode during standstill") +# +# # Test 3: Lead car appears -> ACC mode +# car_state = MockCarState(v_ego=25) # Reset car state +# lead_one.status = True +# lead_one.dRel = 50 +# stabilize_filters() +# self.assertEqual(self.controller.get_mpc_mode(), 'acc', +# "Should be in ACC mode with safe lead distance") +# +# # Test 4: Dangerous TTC -> Blended mode +# # Set up conditions that will definitely trigger dangerous TTC +# car_state = MockCarState(v_ego=20) # 72 kph +# lead_one.status = True +# lead_one.dRel = 20 # This creates a TTC of 1s, well below DANGEROUS_TTC +# +# # Need more updates to allow the weighted average to stabilize +# for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): +# self.controller.update(False, car_state, lead_one, md, controls_state) +# +# self.assertTrue(self.controller._has_dangerous_ttc, +# "Should detect dangerous TTC condition") +# self.assertEqual(self.controller.get_mpc_mode(), 'blended', +# "Should be in blended mode with dangerous TTC") +# +# def test_mpc_fcw_handling(self): +# """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 +# self.controller.set_mpc_fcw_crash_cnt(5) +# for _ in range(MPC_FCW_WINDOW_SIZE + 1): +# self.controller.update(False, car_state, lead_one, md, controls_state) +# +# self.assertTrue(self.controller._has_mpc_fcw) +# self.assertEqual(self.controller.get_mpc_mode(), 'blended') +# +# # Test FCW recovery +# self.controller.set_mpc_fcw_crash_cnt(0) +# for _ in range(MPC_FCW_WINDOW_SIZE + 1): +# self.controller.update(False, car_state, lead_one, md, controls_state) +# +# self.assertFalse(self.controller._has_mpc_fcw) +# +# def test_radar_unavailable_handling(self): +# """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): +# self.controller.update(False, car_state, lead_one, md, controls_state) +# radar_mode = self.controller.get_mpc_mode() +# +# # Test with radar unavailable +# for _ in range(LEAD_WINDOW_SIZE + 1): +# self.controller.update(True, car_state, lead_one, md, controls_state) +# radarless_mode = self.controller.get_mpc_mode() +# +# self.assertIsNotNone(radar_mode) +# self.assertIsNotNone(radarless_mode) +# +# if __name__ == '__main__': +# unittest.main() \ No newline at end of file From 4b64f85f85211de051cb4441d43ada39ed2ccb56 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 16:02:55 +0100 Subject: [PATCH 13/65] Replace unittest with pytest for dynamic controller tests Migrated dynamic controller tests from unittest to pytest for improved readability and maintainability. Refactored mock setup using pytest fixtures and monkeypatching while preserving test coverage. --- .../lib/tests/pytest_dynamic_controller.py | 24 +- .../lib/tests/test_dynamic_controller.py | 253 ------------------ 2 files changed, 10 insertions(+), 267 deletions(-) delete mode 100644 sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py diff --git a/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py index 85319f7a1c..6e5a642b00 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py @@ -11,7 +11,6 @@ from sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import ( import pytest import numpy as np -from unittest.mock import MagicMock, patch class MockInterp: def __call__(self, x, xp, fp): @@ -31,21 +30,18 @@ class MockLeadOne: class MockModelData: def __init__(self, x_vals=None, positions=None): - self.orientation = MagicMock() - self.position = MagicMock() - if x_vals is not None: - self.orientation.x = x_vals - if positions is not None: - self.position.x = positions + 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(): - with patch('openpilot.common.numpy_fast.interp', new_callable=MockInterp) as mock: - yield mock +def interp(monkeypatch): + mock_interp = MockInterp() + monkeypatch.setattr('openpilot.common.numpy_fast.interp', mock_interp) + return mock_interp @pytest.fixture def controller(interp): @@ -158,7 +154,7 @@ def test_dangerous_ttc_detection(controller, has_radar): 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, f"TTC of 1s should be considered dangerous" + 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" @@ -166,7 +162,7 @@ def test_dangerous_ttc_detection(controller, has_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 + 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) @@ -189,9 +185,9 @@ def test_mode_transitions(controller, has_radar): # 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 + lead_one.dRel = 50 # Safe distance stabilize_filters() - assert controller._has_dangerous_ttc == False, "Should not have dangerous TTC" + 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 diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py deleted file mode 100644 index 0ce0454d4e..0000000000 --- a/sunnypilot/selfdrive/controls/lib/tests/test_dynamic_controller.py +++ /dev/null @@ -1,253 +0,0 @@ -# from sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import ( -# DynamicExperimentalController, -# TRAJECTORY_SIZE, -# LEAD_WINDOW_SIZE, -# SLOW_DOWN_WINDOW_SIZE, -# DANGEROUS_TTC_WINDOW_SIZE, -# MPC_FCW_WINDOW_SIZE, -# SNG_State, -# SLOWNESS_CRUISE_OFFSET, -# SLOW_DOWN_BP, -# SLOW_DOWN_DIST, STOP_AND_GO_FRAME -# ) -# -# import unittest -# import numpy as np -# from unittest.mock import MagicMock, patch -# -# 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 = MagicMock() -# self.position = MagicMock() -# if x_vals is not None: -# self.orientation.x = x_vals -# if positions is not None: -# self.position.x = positions -# -# class MockControlState: -# def __init__(self, v_cruise=0): -# self.vCruise = v_cruise -# -# class TestDynamicExperimentalController(unittest.TestCase): -# def setUp(self): -# """Set up test environment before each test case""" -# patcher = patch('openpilot.common.numpy_fast.interp', new_callable=MockInterp) -# self.addCleanup(patcher.stop) -# patcher.start() -# -# self.controller = DynamicExperimentalController() -# self.controller.set_enabled(True) -# -# def test_initial_state(self): -# """Test initial state of the controller""" -# self.assertEqual(self.controller._mode, 'acc') -# self.assertFalse(self.controller._has_lead) -# self.assertFalse(self.controller._has_standstill) -# self.assertEqual(self.controller._sng_state, SNG_State.off) -# self.assertFalse(self.controller._has_lead_filtered) -# self.assertFalse(self.controller._has_slow_down) -# self.assertFalse(self.controller._has_dangerous_ttc) -# self.assertFalse(self.controller._has_mpc_fcw) -# -# def test_standstill_detection(self): -# """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 -# self.controller.update(False, car_state, lead_one, md, controls_state) -# self.assertEqual(self.controller._sng_state, SNG_State.stopped) -# self.assertEqual(self.controller.get_mpc_mode(), 'blended') -# -# # Test transition from standstill to moving -# car_state.standstill = False -# self.controller.update(False, car_state, lead_one, md, controls_state) -# self.assertEqual(self.controller._sng_state, SNG_State.going) -# -# # Test complete transition to normal driving -# for _ in range(STOP_AND_GO_FRAME + 1): -# self.controller.update(False, car_state, lead_one, md, controls_state) -# self.assertEqual(self.controller._sng_state, SNG_State.off) -# -# def test_lead_detection(self): -# """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): -# self.controller.update(False, car_state, lead_one, md, controls_state) -# -# self.assertTrue(self.controller._has_lead_filtered) -# self.assertEqual(self.controller.get_mpc_mode(), 'acc') -# -# # Test lead loss detection -# lead_one.status = False -# for _ in range(LEAD_WINDOW_SIZE + 1): -# self.controller.update(False, car_state, lead_one, md, controls_state) -# -# self.assertFalse(self.controller._has_lead_filtered) -# -# def test_slow_down_detection(self): -# """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): -# self.controller.update(False, car_state, lead_one, md, controls_state) -# -# self.assertTrue(self.controller._has_slow_down) -# self.assertEqual(self.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): -# self.controller.update(False, car_state, lead_one, md, controls_state) -# -# self.assertFalse(self.controller._has_slow_down) -# -# def test_dangerous_ttc_detection(self): -# """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 -# lead_one.dRel = 100 # Safe distance -# for _ in range(DANGEROUS_TTC_WINDOW_SIZE + 1): -# self.controller.update(False, car_state, lead_one, md, controls_state) -# -# self.assertFalse(self.controller._has_dangerous_ttc) -# -# # 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): -# self.controller.update(False, car_state, lead_one, md, controls_state) -# -# self.assertTrue(self.controller._has_dangerous_ttc, -# f"TTC of 1s should be considered dangerous (threshold: {DANGEROUS_TTC}s)") -# self.assertEqual(self.controller.get_mpc_mode(), 'blended', -# "Should be in blended mode with dangerous TTC") -# -# def test_mode_transitions(self): -# """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): -# self.controller.update(False, car_state, lead_one, md, controls_state) -# -# # Test 1: Normal driving -> ACC mode -# stabilize_filters() -# self.assertEqual(self.controller.get_mpc_mode(), 'acc', -# "Should be in ACC mode under normal driving conditions") -# -# # Test 2: Standstill -> Blended mode -# car_state.standstill = True -# self.controller.update(False, car_state, lead_one, md, controls_state) -# self.assertEqual(self.controller.get_mpc_mode(), 'blended', -# "Should be in blended mode during standstill") -# -# # Test 3: Lead car appears -> ACC mode -# car_state = MockCarState(v_ego=25) # Reset car state -# lead_one.status = True -# lead_one.dRel = 50 -# stabilize_filters() -# self.assertEqual(self.controller.get_mpc_mode(), 'acc', -# "Should be in ACC mode with safe lead distance") -# -# # Test 4: Dangerous TTC -> Blended mode -# # Set up conditions that will definitely trigger dangerous TTC -# car_state = MockCarState(v_ego=20) # 72 kph -# lead_one.status = True -# lead_one.dRel = 20 # This creates a TTC of 1s, well below DANGEROUS_TTC -# -# # Need more updates to allow the weighted average to stabilize -# for _ in range(DANGEROUS_TTC_WINDOW_SIZE * 2): -# self.controller.update(False, car_state, lead_one, md, controls_state) -# -# self.assertTrue(self.controller._has_dangerous_ttc, -# "Should detect dangerous TTC condition") -# self.assertEqual(self.controller.get_mpc_mode(), 'blended', -# "Should be in blended mode with dangerous TTC") -# -# def test_mpc_fcw_handling(self): -# """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 -# self.controller.set_mpc_fcw_crash_cnt(5) -# for _ in range(MPC_FCW_WINDOW_SIZE + 1): -# self.controller.update(False, car_state, lead_one, md, controls_state) -# -# self.assertTrue(self.controller._has_mpc_fcw) -# self.assertEqual(self.controller.get_mpc_mode(), 'blended') -# -# # Test FCW recovery -# self.controller.set_mpc_fcw_crash_cnt(0) -# for _ in range(MPC_FCW_WINDOW_SIZE + 1): -# self.controller.update(False, car_state, lead_one, md, controls_state) -# -# self.assertFalse(self.controller._has_mpc_fcw) -# -# def test_radar_unavailable_handling(self): -# """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): -# self.controller.update(False, car_state, lead_one, md, controls_state) -# radar_mode = self.controller.get_mpc_mode() -# -# # Test with radar unavailable -# for _ in range(LEAD_WINDOW_SIZE + 1): -# self.controller.update(True, car_state, lead_one, md, controls_state) -# radarless_mode = self.controller.get_mpc_mode() -# -# self.assertIsNotNone(radar_mode) -# self.assertIsNotNone(radarless_mode) -# -# if __name__ == '__main__': -# unittest.main() \ No newline at end of file From 760e7e847a8d4f03ad823bde387c6d95fc50b8bf Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 16:05:55 +0100 Subject: [PATCH 14/65] new line... --- .../selfdrive/controls/lib/tests/pytest_dynamic_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py index 6e5a642b00..0abe70eb64 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py @@ -252,4 +252,4 @@ def test_radar_unavailable_handling(controller): radarless_mode = controller.get_mpc_mode() assert radar_mode is not None - assert radarless_mode is not None \ No newline at end of file + assert radarless_mode is not None From 55b6eae92e6219eb0d6550f43007dbd56d4bdd87 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 15:34:56 +0100 Subject: [PATCH 15/65] Refactor and modularize DynamicExperimentalController logic Moved DynamicExperimentalController logic and helper functions to a dedicated module for better readability and maintainability. Simplified longitudinal planner logic by introducing reusable methods to manage MPC mode and longitudinal plan publishing. Adjusted file structure for dynamic controller-related components and updated relevant imports. --- .../controls/lib/longitudinal_planner.py | 48 ++----------------- .../controls/{lib => dec}/drive_helpers.py | 0 .../dynamic_experimental_controller.py | 13 +++-- sunnypilot/selfdrive/controls/dec/helpers.py | 42 ++++++++++++++++ .../tests/pytest_dynamic_controller.py | 7 ++- 5 files changed, 60 insertions(+), 50 deletions(-) rename sunnypilot/selfdrive/controls/{lib => dec}/drive_helpers.py (100%) rename sunnypilot/selfdrive/controls/{lib => dec}/dynamic_experimental_controller.py (97%) create mode 100644 sunnypilot/selfdrive/controls/dec/helpers.py rename sunnypilot/selfdrive/controls/{lib => dec}/tests/pytest_dynamic_controller.py (98%) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index b803bd51d1..54dab29d3e 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -2,8 +2,6 @@ import math import numpy as np from openpilot.common.numpy_fast import clip, interp -from openpilot.common.params import Params -from cereal import custom import cereal.messaging as messaging from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX @@ -17,9 +15,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDX from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog - -from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import DynamicExperimentalController - +from sunnypilot.selfdrive.controls.dec.dynamic_experimental_controller import DynamicExperimentalController +from sunnypilot.selfdrive.controls.dec.helpers import get_mpc_mode, publish_longitudinal_plan_sp LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 @@ -33,7 +30,6 @@ MIN_ALLOW_THROTTLE_SPEED = 2.5 _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] -MpcSource = custom.MpcSource def get_max_accel(v_ego): return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS) @@ -89,19 +85,8 @@ class LongitudinalPlanner: self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 - - self.params = Params() - self.param_read_counter = 0 - self.read_param() - self.dynamic_experimental_controller = DynamicExperimentalController() - def read_param(self): - try: - self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl")) - except AttributeError: - self.dynamic_experimental_controller = DynamicExperimentalController() - @staticmethod def parse_model(model_msg, model_error): if (len(model_msg.position.x) == ModelConstants.IDX_N and @@ -123,16 +108,7 @@ class LongitudinalPlanner: return x, v, a, j, throttle_prob def update(self, sm): - if self.param_read_counter % 50 == 0: - self.read_param() - self.param_read_counter += 1 - if self.dynamic_experimental_controller.is_enabled() and sm['selfdriveState'].experimentalMode: - self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) - self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState']) - #, sm['navInstruction'].maneuverDistance) - self.mpc.mode = self.dynamic_experimental_controller.get_mpc_mode() - else: - self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' + self.mpc.mode = get_mpc_mode(sm, self.dynamic_experimental_controller, self.mpc, self.CP) if len(sm['carControl'].orientationNED) == 3: accel_coast = get_coast_accel(sm['carControl'].orientationNED[1]) @@ -233,20 +209,4 @@ class LongitudinalPlanner: longitudinalPlan.allowThrottle = self.allow_throttle pm.send('longitudinalPlan', plan_send) - - 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 - print(f"mpcSource: {longitudinalPlanSP.mpcSource}") - - longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled() - print(f"dynamicExperimentalControl: {longitudinalPlanSP.dynamicExperimentalControl}") - - - pm.send('longitudinalPlanSP', plan_sp_send) - + publish_longitudinal_plan_sp(sm, pm, self.mpc, self.dynamic_experimental_controller) diff --git a/sunnypilot/selfdrive/controls/lib/drive_helpers.py b/sunnypilot/selfdrive/controls/dec/drive_helpers.py similarity index 100% rename from sunnypilot/selfdrive/controls/lib/drive_helpers.py rename to sunnypilot/selfdrive/controls/dec/drive_helpers.py diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py similarity index 97% rename from sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py rename to sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py index 63fe4b7922..3eaf6096f6 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py +++ b/sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py @@ -22,6 +22,7 @@ # # Version = 2024-7-11 from openpilot.common.numpy_fast import interp +from openpilot.common.params import Params import numpy as np # d-e2e, from modeldata.h @@ -102,8 +103,9 @@ class WeightedMovingAverageCalculator: self.data = [] class DynamicExperimentalController: - def __init__(self): - self._is_enabled = False + def __init__(self, params = None): + self._params = params or Params() + self._is_enabled = self._params.get_bool("DynamicExperimentalControl") self._mode = 'acc' self._mode_prev = 'acc' self._mode_changed = False @@ -181,7 +183,7 @@ class DynamicExperimentalController: return LEAD_PROB + 0.1 # Increase the threshold on highways return LEAD_PROB - def _update(self, car_state, lead_one, md, controls_state): #, maneuver_distance): + def _update(self, car_state, lead_one, md, controls_state): #, maneuver_distance): self._v_ego_kph = car_state.vEgo * 3.6 self._v_cruise_kph = controls_state.vCruise self._has_lead = lead_one.status @@ -246,7 +248,6 @@ class DynamicExperimentalController: # keep prev values self._has_standstill_prev = self._has_standstill self._has_lead_filtered_prev = self._has_lead_filtered - self._frame += 1 def _radarless_mode(self): # when mpc fcw crash prob is high @@ -341,6 +342,9 @@ class DynamicExperimentalController: self._set_mode('acc') def update(self, radar_unavailable, car_state, lead_one, md, controls_state): #, maneuver_distance): + if self._frame % 50 == 0: + self._is_enabled = self._params.get_bool("DynamicExperimentalControl") + if self._is_enabled: self._update(car_state, lead_one, md, controls_state) #, maneuver_distance) if radar_unavailable: @@ -349,6 +353,7 @@ class DynamicExperimentalController: self._radar_mode() self._mode_changed = self._mode != self._mode_prev self._mode_prev = self._mode + self._frame += 1 def get_mpc_mode(self): return self._mode diff --git a/sunnypilot/selfdrive/controls/dec/helpers.py b/sunnypilot/selfdrive/controls/dec/helpers.py new file mode 100644 index 0000000000..3ad5e8a5b4 --- /dev/null +++ b/sunnypilot/selfdrive/controls/dec/helpers.py @@ -0,0 +1,42 @@ +from cereal import custom +MpcSource = custom.MpcSource + +def get_mpc_mode(sm, dynamic_experimental_controller, mpc, CP): + """ + Determines the appropriate MPC mode based on the experimental state and system + configurations. It either returns a default mode or updates the dynamic + experimental controller and retrieves the updated mode. + + :param is_experimental: A flag indicating whether to use the experimental mode. + If False, defaults to 'acc' mode. + :type is_experimental: bool + + :return: The calculated or retrieved MPC mode. + :rtype: str + """ + is_experimental = sm['selfdriveState'].experimentalMode + if not dynamic_experimental_controller.is_enabled() or not is_experimental: + return 'blended' if is_experimental else 'acc' + + dynamic_experimental_controller.set_mpc_fcw_crash_cnt(mpc.crash_cnt) + dynamic_experimental_controller.update(CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState']) + #, sm['navInstruction'].maneuverDistance) + return dynamic_experimental_controller.get_mpc_mode() + + +def publish_longitudinal_plan_sp(sm, pm, mpc, dynamic_experimental_controller): + 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 mpc.mode == 'blended' else MpcSource.acc + print(f"mpcSource: {longitudinalPlanSP.mpcSource}") + + longitudinalPlanSP.dynamicExperimentalControl = dynamic_experimental_controller.is_enabled() + print(f"dynamicExperimentalControl: {longitudinalPlanSP.dynamicExperimentalControl}") + + + pm.send('longitudinalPlanSP', plan_sp_send) \ No newline at end of file diff --git a/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py b/sunnypilot/selfdrive/controls/dec/tests/pytest_dynamic_controller.py similarity index 98% rename from sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py rename to sunnypilot/selfdrive/controls/dec/tests/pytest_dynamic_controller.py index 0abe70eb64..69d7c92e9a 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/dec/tests/pytest_dynamic_controller.py @@ -1,4 +1,6 @@ -from sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import ( +from openpilot.common.params import Params + +from sunnypilot.selfdrive.controls.dec.dynamic_experimental_controller import ( DynamicExperimentalController, TRAJECTORY_SIZE, LEAD_WINDOW_SIZE, @@ -45,8 +47,9 @@ def interp(monkeypatch): @pytest.fixture def controller(interp): + params = Params() + params.put_bool("DynamicExperimentalControl", True) controller = DynamicExperimentalController() - controller.set_enabled(True) return controller def test_initial_state(controller): From 84b6af094f93071b14fb7a2babcaed479f84c3a0 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 16:04:08 +0100 Subject: [PATCH 16/65] Add missing import for messaging in helpers.py The `messaging` module was added to resolve potential issues with undefined references. This change ensures all required imports are present, improving the reliability and maintainability of the code. --- sunnypilot/selfdrive/controls/dec/helpers.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sunnypilot/selfdrive/controls/dec/helpers.py b/sunnypilot/selfdrive/controls/dec/helpers.py index 3ad5e8a5b4..c35d55e5b0 100644 --- a/sunnypilot/selfdrive/controls/dec/helpers.py +++ b/sunnypilot/selfdrive/controls/dec/helpers.py @@ -1,4 +1,4 @@ -from cereal import custom +from cereal import custom, messaging MpcSource = custom.MpcSource def get_mpc_mode(sm, dynamic_experimental_controller, mpc, CP): @@ -39,4 +39,4 @@ def publish_longitudinal_plan_sp(sm, pm, mpc, dynamic_experimental_controller): print(f"dynamicExperimentalControl: {longitudinalPlanSP.dynamicExperimentalControl}") - pm.send('longitudinalPlanSP', plan_sp_send) \ No newline at end of file + pm.send('longitudinalPlanSP', plan_sp_send) From ec25ca070a1c569c30fca2471dbf3f35eceb5443 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 16:05:14 +0100 Subject: [PATCH 17/65] Format --- .../selfdrive/controls/dec/tests/pytest_dynamic_controller.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/sunnypilot/selfdrive/controls/dec/tests/pytest_dynamic_controller.py b/sunnypilot/selfdrive/controls/dec/tests/pytest_dynamic_controller.py index 69d7c92e9a..938b7bf9ac 100644 --- a/sunnypilot/selfdrive/controls/dec/tests/pytest_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/dec/tests/pytest_dynamic_controller.py @@ -1,5 +1,3 @@ -from openpilot.common.params import Params - from sunnypilot.selfdrive.controls.dec.dynamic_experimental_controller import ( DynamicExperimentalController, TRAJECTORY_SIZE, @@ -13,6 +11,7 @@ from sunnypilot.selfdrive.controls.dec.dynamic_experimental_controller import ( import pytest import numpy as np +from openpilot.common.params import Params class MockInterp: def __call__(self, x, xp, fp): From 73e658bf8c22a84a696aee964a5c3da6a53f89cf Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 16:10:19 +0100 Subject: [PATCH 18/65] Formatting --- .../controls/dec/dynamic_experimental_controller.py | 4 ++-- sunnypilot/selfdrive/controls/dec/helpers.py | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py index 3eaf6096f6..5135a405fa 100644 --- a/sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py +++ b/sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py @@ -183,7 +183,7 @@ class DynamicExperimentalController: return LEAD_PROB + 0.1 # Increase the threshold on highways return LEAD_PROB - def _update(self, car_state, lead_one, md, controls_state): #, maneuver_distance): + def _update(self, car_state, lead_one, md, controls_state): #, maneuver_distance): self._v_ego_kph = car_state.vEgo * 3.6 self._v_cruise_kph = controls_state.vCruise self._has_lead = lead_one.status @@ -344,7 +344,7 @@ class DynamicExperimentalController: def update(self, radar_unavailable, car_state, lead_one, md, controls_state): #, maneuver_distance): if self._frame % 50 == 0: self._is_enabled = self._params.get_bool("DynamicExperimentalControl") - + if self._is_enabled: self._update(car_state, lead_one, md, controls_state) #, maneuver_distance) if radar_unavailable: diff --git a/sunnypilot/selfdrive/controls/dec/helpers.py b/sunnypilot/selfdrive/controls/dec/helpers.py index c35d55e5b0..22c48b6218 100644 --- a/sunnypilot/selfdrive/controls/dec/helpers.py +++ b/sunnypilot/selfdrive/controls/dec/helpers.py @@ -3,11 +3,11 @@ MpcSource = custom.MpcSource def get_mpc_mode(sm, dynamic_experimental_controller, mpc, CP): """ - Determines the appropriate MPC mode based on the experimental state and system - configurations. It either returns a default mode or updates the dynamic + Determines the appropriate MPC mode based on the experimental state and system + configurations. It either returns a default mode or updates the dynamic experimental controller and retrieves the updated mode. - :param is_experimental: A flag indicating whether to use the experimental mode. + :param is_experimental: A flag indicating whether to use the experimental mode. If False, defaults to 'acc' mode. :type is_experimental: bool From 4a46bccc20461c01e4dc506b0bb244c8cd5b9e5b Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 16:28:40 +0100 Subject: [PATCH 19/65] rebase fix --- cereal/custom.capnp | 1 - 1 file changed, 1 deletion(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 729966b10b..678ba2ea09 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -14,7 +14,6 @@ enum MpcSource { blended @1; } -struct CustomReserved0 @0x81c2f05a394cf4af { struct SelfdriveStateSP @0x81c2f05a394cf4af { mads @0 :ModularAssistiveDrivingSystem; From fd6ec85e20770fd521b7c8aa13a506baf9f8223b Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 16:40:20 +0100 Subject: [PATCH 20/65] Refactor MpcSource definition and update references. Moved MpcSource enum into LongitudinalPlanSP for better encapsulation. Updated references in helpers.py to use the new path. This change improves code organization and maintains functionality. --- cereal/custom.capnp | 10 +++++----- sunnypilot/selfdrive/controls/dec/helpers.py | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 678ba2ea09..253d582f31 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -9,11 +9,6 @@ $Cxx.namespace("cereal"); # you can rename the struct, but don't change the identifier -enum MpcSource { - acc @0; - blended @1; -} - struct SelfdriveStateSP @0x81c2f05a394cf4af { mads @0 :ModularAssistiveDrivingSystem; @@ -93,6 +88,11 @@ struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { e2eStatus @1 :Bool; mpcSource @2 :MpcSource; dynamicExperimentalControl @3 :Bool; + + enum MpcSource { + acc @0; + blended @1; + } } struct CustomReserved3 @0xda96579883444c35 { diff --git a/sunnypilot/selfdrive/controls/dec/helpers.py b/sunnypilot/selfdrive/controls/dec/helpers.py index 22c48b6218..0ce8359f0c 100644 --- a/sunnypilot/selfdrive/controls/dec/helpers.py +++ b/sunnypilot/selfdrive/controls/dec/helpers.py @@ -1,5 +1,5 @@ from cereal import custom, messaging -MpcSource = custom.MpcSource +MpcSource = custom.LongitudinalPlanSP.MpcSource def get_mpc_mode(sm, dynamic_experimental_controller, mpc, CP): """ From f861e8d6780ac8d8fccc4f4bb9944c9a1cae533d Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 16:40:43 +0100 Subject: [PATCH 21/65] Format --- cereal/custom.capnp | 1 - 1 file changed, 1 deletion(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 253d582f31..eef2b7938e 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -8,7 +8,6 @@ $Cxx.namespace("cereal"); # cereal, so use these if you want custom events in your fork. # you can rename the struct, but don't change the identifier - struct SelfdriveStateSP @0x81c2f05a394cf4af { mads @0 :ModularAssistiveDrivingSystem; From 898f5b89ed918abe21349e311eace9122052dec6 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 20:51:35 +0100 Subject: [PATCH 22/65] Refactor DEC into a dedicated longitudinal planner class Move Dynamic Experimental Control (DEC) logic to a new `DecLongitudinalPlanner` class for better modularity and maintainability. This simplifies the `LongitudinalPlanner` by delegating DEC-specific behavior and consolidates related methods into a single file. Additionally, redundant code was removed to improve readability and reduce complexity. --- .../controls/lib/longitudinal_planner.py | 15 ++++--- .../controls/dec/dec_longitudinal_planner.py | 38 +++++++++++++++++ .../dec/dynamic_experimental_controller.py | 16 ++++--- sunnypilot/selfdrive/controls/dec/helpers.py | 42 ------------------- 4 files changed, 58 insertions(+), 53 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/dec/dec_longitudinal_planner.py delete mode 100644 sunnypilot/selfdrive/controls/dec/helpers.py diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 54dab29d3e..724cf11349 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -15,8 +15,8 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDX from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog -from sunnypilot.selfdrive.controls.dec.dynamic_experimental_controller import DynamicExperimentalController -from sunnypilot.selfdrive.controls.dec.helpers import get_mpc_mode, publish_longitudinal_plan_sp + +from sunnypilot.selfdrive.controls.dec.dec_longitudinal_planner import DecLongitudinalPlanner LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 @@ -69,10 +69,11 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05): return a_target, should_stop -class LongitudinalPlanner: +class LongitudinalPlanner(DecLongitudinalPlanner): def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP self.mpc = LongitudinalMpc(dt=dt) + DecLongitudinalPlanner.__init__(self, self.CP, self.mpc) self.fcw = False self.dt = dt self.allow_throttle = True @@ -85,7 +86,6 @@ class LongitudinalPlanner: self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N) self.solverExecutionTime = 0.0 - self.dynamic_experimental_controller = DynamicExperimentalController() @staticmethod def parse_model(model_msg, model_error): @@ -108,7 +108,10 @@ class LongitudinalPlanner: return x, v, a, j, throttle_prob def update(self, sm): - self.mpc.mode = get_mpc_mode(sm, self.dynamic_experimental_controller, self.mpc, self.CP) + DecLongitudinalPlanner.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]) @@ -209,4 +212,4 @@ class LongitudinalPlanner: longitudinalPlan.allowThrottle = self.allow_throttle pm.send('longitudinalPlan', plan_send) - publish_longitudinal_plan_sp(sm, pm, self.mpc, self.dynamic_experimental_controller) + self.publish_longitudinal_plan_sp(sm, pm) diff --git a/sunnypilot/selfdrive/controls/dec/dec_longitudinal_planner.py b/sunnypilot/selfdrive/controls/dec/dec_longitudinal_planner.py new file mode 100644 index 0000000000..bbe14c6b71 --- /dev/null +++ b/sunnypilot/selfdrive/controls/dec/dec_longitudinal_planner.py @@ -0,0 +1,38 @@ +from cereal import messaging, custom +from sunnypilot.selfdrive.controls.dec.dynamic_experimental_controller import DynamicExperimentalController +MpcSource = custom.LongitudinalPlanSP.MpcSource + + +class DecLongitudinalPlanner: + def __init__(self, CP, mpc): + self.CP = CP + self.mpc = mpc + self.dynamic_experimental_controller = DynamicExperimentalController() + self.is_enabled = False + + def update(self, sm): + self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) + self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm) + + def get_mpc_mode(self, sm) -> str | None: + if not self.is_enabled or not sm['selfdriveState'].experimentalMode: + return None + + return self.dynamic_experimental_controller.get_mpc_mode() + + def publish_longitudinal_plan_sp(self, sm, pm): + 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 + print(f"mpcSource: {longitudinalPlanSP.mpcSource}") + + longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled() + print(f"dynamicExperimentalControl: {longitudinalPlanSP.dynamicExperimentalControl}") + + + pm.send('longitudinalPlanSP', plan_sp_send) diff --git a/sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py index 5135a405fa..7c524bbec1 100644 --- a/sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py +++ b/sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py @@ -109,7 +109,7 @@ class DynamicExperimentalController: self._mode = 'acc' self._mode_prev = 'acc' self._mode_changed = False - self._frame = 0 + # self._frame = 0 # Use weighted moving average for filtering leads self._lead_gmac = WeightedMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE) @@ -341,9 +341,15 @@ class DynamicExperimentalController: self._set_mode('acc') - def update(self, radar_unavailable, car_state, lead_one, md, controls_state): #, maneuver_distance): - if self._frame % 50 == 0: - self._is_enabled = self._params.get_bool("DynamicExperimentalControl") + def update(self, radar_unavailable, sm): #, maneuver_distance): + # if self._frame % 50 == 0: + # self._is_enabled = self._params.get_bool("DynamicExperimentalControl") + + car_state = sm['carState'] + lead_one = sm['radarState'].leadOne + md = sm['modelV2'] + controls_state = sm['controlsState'] + if self._is_enabled: self._update(car_state, lead_one, md, controls_state) #, maneuver_distance) @@ -353,7 +359,7 @@ class DynamicExperimentalController: self._radar_mode() self._mode_changed = self._mode != self._mode_prev self._mode_prev = self._mode - self._frame += 1 + # self._frame += 1 def get_mpc_mode(self): return self._mode diff --git a/sunnypilot/selfdrive/controls/dec/helpers.py b/sunnypilot/selfdrive/controls/dec/helpers.py deleted file mode 100644 index 0ce8359f0c..0000000000 --- a/sunnypilot/selfdrive/controls/dec/helpers.py +++ /dev/null @@ -1,42 +0,0 @@ -from cereal import custom, messaging -MpcSource = custom.LongitudinalPlanSP.MpcSource - -def get_mpc_mode(sm, dynamic_experimental_controller, mpc, CP): - """ - Determines the appropriate MPC mode based on the experimental state and system - configurations. It either returns a default mode or updates the dynamic - experimental controller and retrieves the updated mode. - - :param is_experimental: A flag indicating whether to use the experimental mode. - If False, defaults to 'acc' mode. - :type is_experimental: bool - - :return: The calculated or retrieved MPC mode. - :rtype: str - """ - is_experimental = sm['selfdriveState'].experimentalMode - if not dynamic_experimental_controller.is_enabled() or not is_experimental: - return 'blended' if is_experimental else 'acc' - - dynamic_experimental_controller.set_mpc_fcw_crash_cnt(mpc.crash_cnt) - dynamic_experimental_controller.update(CP.radarUnavailable, sm['carState'], sm['radarState'].leadOne, sm['modelV2'], sm['controlsState']) - #, sm['navInstruction'].maneuverDistance) - return dynamic_experimental_controller.get_mpc_mode() - - -def publish_longitudinal_plan_sp(sm, pm, mpc, dynamic_experimental_controller): - 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 mpc.mode == 'blended' else MpcSource.acc - print(f"mpcSource: {longitudinalPlanSP.mpcSource}") - - longitudinalPlanSP.dynamicExperimentalControl = dynamic_experimental_controller.is_enabled() - print(f"dynamicExperimentalControl: {longitudinalPlanSP.dynamicExperimentalControl}") - - - pm.send('longitudinalPlanSP', plan_sp_send) From c0c74e3761d48374c8945047c401fab03824ac27 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 21:03:38 +0100 Subject: [PATCH 23/65] **Refactor DEC module structure for better organization** Moved DEC-related files from `dec` to `lib` for improved clarity and consistency within the project structure. Updated all relevant import paths to reflect the new locations. Ensured functionality remains unaffected with these changes. --- selfdrive/controls/lib/longitudinal_planner.py | 2 +- .../selfdrive/controls/{dec => lib}/dec_longitudinal_planner.py | 2 +- sunnypilot/selfdrive/controls/{dec => lib}/drive_helpers.py | 0 .../controls/{dec => lib}/dynamic_experimental_controller.py | 0 .../controls/{dec => lib}/tests/pytest_dynamic_controller.py | 2 +- 5 files changed, 3 insertions(+), 3 deletions(-) rename sunnypilot/selfdrive/controls/{dec => lib}/dec_longitudinal_planner.py (92%) rename sunnypilot/selfdrive/controls/{dec => lib}/drive_helpers.py (100%) rename sunnypilot/selfdrive/controls/{dec => lib}/dynamic_experimental_controller.py (100%) rename sunnypilot/selfdrive/controls/{dec => lib}/tests/pytest_dynamic_controller.py (99%) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 724cf11349..275f4354fa 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,7 +16,7 @@ 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 sunnypilot.selfdrive.controls.dec.dec_longitudinal_planner import DecLongitudinalPlanner +from openpilot.sunnypilot.selfdrive.controls.lib.dec_longitudinal_planner import DecLongitudinalPlanner LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 diff --git a/sunnypilot/selfdrive/controls/dec/dec_longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py similarity index 92% rename from sunnypilot/selfdrive/controls/dec/dec_longitudinal_planner.py rename to sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py index bbe14c6b71..a206cf74fe 100644 --- a/sunnypilot/selfdrive/controls/dec/dec_longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py @@ -1,5 +1,5 @@ from cereal import messaging, custom -from sunnypilot.selfdrive.controls.dec.dynamic_experimental_controller import DynamicExperimentalController +from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import DynamicExperimentalController MpcSource = custom.LongitudinalPlanSP.MpcSource diff --git a/sunnypilot/selfdrive/controls/dec/drive_helpers.py b/sunnypilot/selfdrive/controls/lib/drive_helpers.py similarity index 100% rename from sunnypilot/selfdrive/controls/dec/drive_helpers.py rename to sunnypilot/selfdrive/controls/lib/drive_helpers.py diff --git a/sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py similarity index 100% rename from sunnypilot/selfdrive/controls/dec/dynamic_experimental_controller.py rename to sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py diff --git a/sunnypilot/selfdrive/controls/dec/tests/pytest_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py similarity index 99% rename from sunnypilot/selfdrive/controls/dec/tests/pytest_dynamic_controller.py rename to sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py index 938b7bf9ac..cbc161d8a4 100644 --- a/sunnypilot/selfdrive/controls/dec/tests/pytest_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py @@ -1,4 +1,4 @@ -from sunnypilot.selfdrive.controls.dec.dynamic_experimental_controller import ( +from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import ( DynamicExperimentalController, TRAJECTORY_SIZE, LEAD_WINDOW_SIZE, From 7d6c9d1a8cbcc103f39edee2f3ff8f276828c90a Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sun, 12 Jan 2025 13:48:08 -0700 Subject: [PATCH 24/65] static test --- .../controls/lib/dec_longitudinal_planner.py | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py index a206cf74fe..d16ddbd090 100644 --- a/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py @@ -13,26 +13,25 @@ class DecLongitudinalPlanner: def update(self, sm): self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm) - + def get_mpc_mode(self, sm) -> str | None: if not self.is_enabled or not sm['selfdriveState'].experimentalMode: return None - + return self.dynamic_experimental_controller.get_mpc_mode() def publish_longitudinal_plan_sp(self, sm, pm): 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 print(f"mpcSource: {longitudinalPlanSP.mpcSource}") - + longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled() print(f"dynamicExperimentalControl: {longitudinalPlanSP.dynamicExperimentalControl}") - - + pm.send('longitudinalPlanSP', plan_sp_send) From 1d422ce5cf9c901d5b18e5dbfdb5ae1c95161a5a Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sun, 12 Jan 2025 14:06:50 -0700 Subject: [PATCH 25/65] static --- sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py index d16ddbd090..3e1e1b97cf 100644 --- a/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py @@ -14,7 +14,7 @@ class DecLongitudinalPlanner: self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm) - def get_mpc_mode(self, sm) -> str | None: + def get_mpc_mode(self, sm): if not self.is_enabled or not sm['selfdriveState'].experimentalMode: return None From 87ca1513f4f099ddb7e613f56bbccb67555ecda0 Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sun, 12 Jan 2025 14:29:22 -0700 Subject: [PATCH 26/65] had moved to car_state --- .../selfdrive/controls/lib/dynamic_experimental_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py index 7c524bbec1..67b570373a 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py @@ -185,7 +185,7 @@ class DynamicExperimentalController: def _update(self, car_state, lead_one, md, controls_state): #, maneuver_distance): self._v_ego_kph = car_state.vEgo * 3.6 - self._v_cruise_kph = controls_state.vCruise + self._v_cruise_kph = car_state.vCruise self._has_lead = lead_one.status self._has_standstill = car_state.standstill From cc507a5cd9ed4c5fd491c76cb912f4da11f4e40e Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 16:40:07 -0500 Subject: [PATCH 27/65] cleanup --- cereal/custom.capnp | 6 ++---- common/params.cc | 1 - selfdrive/car/card.py | 3 --- selfdrive/controls/controlsd.py | 2 +- system/manager/manager.py | 2 +- 5 files changed, 4 insertions(+), 10 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index eef2b7938e..bffc1e4d3b 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -83,10 +83,8 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { } struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { - e2eBlended @0 :Text; - e2eStatus @1 :Bool; - mpcSource @2 :MpcSource; - dynamicExperimentalControl @3 :Bool; + mpcSource @0 :MpcSource; + dynamicExperimentalControl @1 :Bool; enum MpcSource { acc @0; diff --git a/common/params.cc b/common/params.cc index a2f0aeaa91..3c11e034cb 100644 --- a/common/params.cc +++ b/common/params.cc @@ -223,7 +223,6 @@ std::unordered_map keys = { {"SunnylinkDongleId", PERSISTENT}, {"SunnylinkdPid", PERSISTENT}, {"SunnylinkEnabled", PERSISTENT}, - {"EnableGithubRunner", PERSISTENT}, {"DynamicExperimentalControl", PERSISTENT}, }; diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 7341939144..1852dccf74 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -76,8 +76,6 @@ class Car: self.CC_prev = car.CarControl.new_message() self.initialized_prev = False - self.dynamic_experimental_control = False - self.last_actuators_output = structs.CarControl.Actuators() self.params = Params() @@ -159,7 +157,6 @@ class Car: self.is_metric = self.params.get_bool("IsMetric") self.experimental_mode = self.params.get_bool("ExperimentalMode") - self.dynamic_experimental_control = self.params.get_bool("DynamicExperimentalControl") # card is driven by can recv, expected at 100Hz self.rk = Ratekeeper(100, print_delay_threshold=None) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2d76865369..9a230e4d83 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -38,7 +38,7 @@ class Controls: self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', - 'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'longitudinalPlanSP'], poll='selfdriveState') + 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState']) self.steer_limited = False diff --git a/system/manager/manager.py b/system/manager/manager.py index 96e8f6e858..499ed6474b 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -40,10 +40,10 @@ def manager_init() -> None: ("LanguageSetting", "main_en"), ("OpenpilotEnabledToggle", "1"), ("LongitudinalPersonality", str(log.LongitudinalPersonality.standard)), - ("DynamicExperimentalControl", "0"), ] sunnypilot_default_params: list[tuple[str, str | bytes]] = [ + ("DynamicExperimentalControl", "0"), ("Mads", "1"), ("MadsMainCruiseAllowed", "1"), ("MadsPauseLateralOnBrake", "0"), From e1cf216c89c7f0edf58802929a9ba860885eed7e Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 16:44:54 -0500 Subject: [PATCH 28/65] some more --- .../lib/dynamic_experimental_controller.py | 46 +++++++++---------- 1 file changed, 21 insertions(+), 25 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py index 67b570373a..01eeb2e425 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py @@ -21,9 +21,10 @@ # THE SOFTWARE. # # Version = 2024-7-11 + +import numpy as np from openpilot.common.numpy_fast import interp from openpilot.common.params import Params -import numpy as np # d-e2e, from modeldata.h TRAJECTORY_SIZE = 33 @@ -81,6 +82,7 @@ class GenericMovingAverageCalculator: self.data = [] self.total = 0 + class WeightedMovingAverageCalculator: def __init__(self, window_size): self.window_size = window_size @@ -102,14 +104,14 @@ class WeightedMovingAverageCalculator: def reset_data(self): self.data = [] + class DynamicExperimentalController: - def __init__(self, params = None): + def __init__(self, params=None): self._params = params or Params() self._is_enabled = self._params.get_bool("DynamicExperimentalControl") self._mode = 'acc' self._mode_prev = 'acc' self._mode_changed = False - # self._frame = 0 # Use weighted moving average for filtering leads self._lead_gmac = WeightedMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE) @@ -146,10 +148,9 @@ class DynamicExperimentalController: self._set_mode_timeout = 0 - def _adaptive_slowdown_threshold(self): """ - Adapts the slow down threshold based on vehicle speed and recent behavior. + Adapts the slow-down threshold based on vehicle speed and recent behavior. """ return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.03 * np.log(1 + len(self._slow_down_gmac.data))) @@ -183,7 +184,7 @@ class DynamicExperimentalController: return LEAD_PROB + 0.1 # Increase the threshold on highways return LEAD_PROB - def _update(self, car_state, lead_one, md, controls_state): #, maneuver_distance): + def _update(self, car_state, lead_one, md, controls_state): # , maneuver_distance): self._v_ego_kph = car_state.vEgo * 3.6 self._v_cruise_kph = car_state.vCruise self._has_lead = lead_one.status @@ -194,13 +195,13 @@ class DynamicExperimentalController: 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 + # 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) + # 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() @@ -232,7 +233,7 @@ class DynamicExperimentalController: # slowness detection if not self._has_standstill: - self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph*SLOWNESS_CRUISE_OFFSET)) + 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 @@ -241,7 +242,7 @@ class DynamicExperimentalController: 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._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 @@ -257,20 +258,20 @@ class DynamicExperimentalController: return # Nav enabled and distance to upcoming turning is 300 or below - #if self._has_nav_instruction: + # 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: + # 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: + # if self._sng_state == SNG_State.going and self._v_cruise_kph >= V_ACC_MIN: # self._set_mode('blended') # return @@ -313,7 +314,7 @@ class DynamicExperimentalController: # 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: + # if self._has_blinkers and self._v_ego_kph < V_ACC_MIN: # self._set_mode('blended') # return @@ -335,31 +336,26 @@ class DynamicExperimentalController: return # Nav enabled and distance to upcoming turning is 300 or below - #if self._has_nav_instruction: + # if self._has_nav_instruction: # self._set_mode('blended') # return self._set_mode('acc') - def update(self, radar_unavailable, sm): #, maneuver_distance): - # if self._frame % 50 == 0: - # self._is_enabled = self._params.get_bool("DynamicExperimentalControl") - - car_state = sm['carState'] + def update(self, radar_unavailable, sm): + car_state = sm['carState'] lead_one = sm['radarState'].leadOne - md = sm['modelV2'] + md = sm['modelV2'] controls_state = sm['controlsState'] - if self._is_enabled: - self._update(car_state, lead_one, md, controls_state) #, maneuver_distance) + self._update(car_state, lead_one, md, controls_state) 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 def get_mpc_mode(self): return self._mode From 12981e02f7548aecb9236bfa5d6c2dd26dd055ae Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 16:46:15 -0500 Subject: [PATCH 29/65] static method --- .../lib/dynamic_experimental_controller.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py index 01eeb2e425..5f3b323574 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py @@ -148,13 +148,8 @@ class DynamicExperimentalController: self._set_mode_timeout = 0 - def _adaptive_slowdown_threshold(self): - """ - Adapts the slow-down threshold based on vehicle speed and recent behavior. - """ - return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.03 * np.log(1 + len(self._slow_down_gmac.data))) - - def _anomaly_detection(self, recent_data, threshold=2.0, context_check=True): + @staticmethod + def _anomaly_detection(recent_data, threshold=2.0, context_check=True): """ Basic anomaly detection using standard deviation. """ @@ -169,6 +164,12 @@ class DynamicExperimentalController: return np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1 return anomaly + def _adaptive_slowdown_threshold(self): + """ + Adapts the slow-down threshold based on vehicle speed and recent behavior. + """ + return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.03 * np.log(1 + len(self._slow_down_gmac.data))) + def _smoothed_lead_detection(self, lead_prob, smoothing_factor=0.2): """ Smoothing the lead detection to avoid erratic behavior. From e7d27f0bb96704d64f2e8007652c2fba466a6b8c Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 16:46:43 -0500 Subject: [PATCH 30/65] move around --- .../lib/dynamic_experimental_controller.py | 30 +++++++++---------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py index 5f3b323574..e166c7c65d 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py @@ -343,21 +343,6 @@ class DynamicExperimentalController: self._set_mode('acc') - def update(self, radar_unavailable, sm): - car_state = sm['carState'] - lead_one = sm['radarState'].leadOne - md = sm['modelV2'] - controls_state = sm['controlsState'] - - if self._is_enabled: - self._update(car_state, lead_one, md, controls_state) - if radar_unavailable: - self._radarless_mode() - else: - self._radar_mode() - self._mode_changed = self._mode != self._mode_prev - self._mode_prev = self._mode - def get_mpc_mode(self): return self._mode @@ -381,3 +366,18 @@ class DynamicExperimentalController: if self._set_mode_timeout > 0: self._set_mode_timeout -= 1 + + def update(self, radar_unavailable, sm): + car_state = sm['carState'] + lead_one = sm['radarState'].leadOne + md = sm['modelV2'] + controls_state = sm['controlsState'] + + if self._is_enabled: + self._update(car_state, lead_one, md, controls_state) + if radar_unavailable: + self._radarless_mode() + else: + self._radar_mode() + self._mode_changed = self._mode != self._mode_prev + self._mode_prev = self._mode From c3fde450005dac7831a85f7b1499bc035b6239ac Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 16:48:44 -0500 Subject: [PATCH 31/65] more cleanup --- .../selfdrive/controls/lib/dec_longitudinal_planner.py | 3 +-- .../controls/lib/dynamic_experimental_controller.py | 7 ++++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py index 3e1e1b97cf..4901ab7c01 100644 --- a/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py @@ -1,5 +1,6 @@ from cereal import messaging, custom from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import DynamicExperimentalController + MpcSource = custom.LongitudinalPlanSP.MpcSource @@ -29,9 +30,7 @@ class DecLongitudinalPlanner: # DEC longitudinalPlanSP.mpcSource = MpcSource.blended if self.mpc.mode == 'blended' else MpcSource.acc - print(f"mpcSource: {longitudinalPlanSP.mpcSource}") longitudinalPlanSP.dynamicExperimentalControl = self.dynamic_experimental_controller.is_enabled() - print(f"dynamicExperimentalControl: {longitudinalPlanSP.dynamicExperimentalControl}") pm.send('longitudinalPlanSP', plan_sp_send) diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py index e166c7c65d..eff8fc804a 100644 --- a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py @@ -185,7 +185,7 @@ class DynamicExperimentalController: return LEAD_PROB + 0.1 # Increase the threshold on highways return LEAD_PROB - def _update(self, car_state, lead_one, md, controls_state): # , maneuver_distance): + def _update(self, car_state, lead_one, md): self._v_ego_kph = car_state.vEgo * 3.6 self._v_cruise_kph = car_state.vCruise self._has_lead = lead_one.status @@ -371,13 +371,14 @@ class DynamicExperimentalController: car_state = sm['carState'] lead_one = sm['radarState'].leadOne md = sm['modelV2'] - controls_state = sm['controlsState'] if self._is_enabled: - self._update(car_state, lead_one, md, controls_state) + self._update(car_state, lead_one, md) + if radar_unavailable: self._radarless_mode() else: self._radar_mode() + self._mode_changed = self._mode != self._mode_prev self._mode_prev = self._mode From e4c29a2c58de430d4615894c4512814031916782 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 16:50:16 -0500 Subject: [PATCH 32/65] stuff --- .../selfdrive/controls/lib/dec_longitudinal_planner.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py index 4901ab7c01..101da609a5 100644 --- a/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py @@ -1,3 +1,10 @@ +""" +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 openpilot.sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import DynamicExperimentalController From de305a81d56ce1e5fd9d3fb752cb759e1eb0d817 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 16:51:44 -0500 Subject: [PATCH 33/65] into their own --- selfdrive/controls/lib/longitudinal_planner.py | 2 +- sunnypilot/selfdrive/__init__.py | 0 sunnypilot/selfdrive/controls/__init__.py | 0 sunnypilot/selfdrive/controls/lib/__init__.py | 0 sunnypilot/selfdrive/controls/lib/dec/__init__.py | 0 .../lib/{dynamic_experimental_controller.py => dec/dec.py} | 0 .../controls/lib/{ => dec}/dec_longitudinal_planner.py | 2 +- sunnypilot/selfdrive/controls/lib/dec/tests/__init__.py | 0 .../controls/lib/{ => dec}/tests/pytest_dynamic_controller.py | 2 +- 9 files changed, 3 insertions(+), 3 deletions(-) create mode 100644 sunnypilot/selfdrive/__init__.py create mode 100644 sunnypilot/selfdrive/controls/__init__.py create mode 100644 sunnypilot/selfdrive/controls/lib/__init__.py create mode 100644 sunnypilot/selfdrive/controls/lib/dec/__init__.py rename sunnypilot/selfdrive/controls/lib/{dynamic_experimental_controller.py => dec/dec.py} (100%) rename sunnypilot/selfdrive/controls/lib/{ => dec}/dec_longitudinal_planner.py (92%) create mode 100644 sunnypilot/selfdrive/controls/lib/dec/tests/__init__.py rename sunnypilot/selfdrive/controls/lib/{ => dec}/tests/pytest_dynamic_controller.py (99%) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 275f4354fa..3b094cf4a2 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,7 +16,7 @@ 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_longitudinal_planner import DecLongitudinalPlanner +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec_longitudinal_planner import DecLongitudinalPlanner LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 diff --git a/sunnypilot/selfdrive/__init__.py b/sunnypilot/selfdrive/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/__init__.py b/sunnypilot/selfdrive/controls/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/__init__.py b/sunnypilot/selfdrive/controls/lib/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/dec/__init__.py b/sunnypilot/selfdrive/controls/lib/dec/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py similarity index 100% rename from sunnypilot/selfdrive/controls/lib/dynamic_experimental_controller.py rename to sunnypilot/selfdrive/controls/lib/dec/dec.py diff --git a/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/dec/dec_longitudinal_planner.py similarity index 92% rename from sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py rename to sunnypilot/selfdrive/controls/lib/dec/dec_longitudinal_planner.py index 101da609a5..eb7a237422 100644 --- a/sunnypilot/selfdrive/controls/lib/dec_longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec_longitudinal_planner.py @@ -6,7 +6,7 @@ See the LICENSE.md file in the root directory for more details. """ from cereal import messaging, custom -from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import DynamicExperimentalController +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController MpcSource = custom.LongitudinalPlanSP.MpcSource diff --git a/sunnypilot/selfdrive/controls/lib/dec/tests/__init__.py b/sunnypilot/selfdrive/controls/lib/dec/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py similarity index 99% rename from sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py rename to sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py index cbc161d8a4..af77639424 100644 --- a/sunnypilot/selfdrive/controls/lib/tests/pytest_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py @@ -1,4 +1,4 @@ -from openpilot.sunnypilot.selfdrive.controls.lib.dynamic_experimental_controller import ( +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import ( DynamicExperimentalController, TRAJECTORY_SIZE, LEAD_WINDOW_SIZE, From 426d41a0a7aaab1b468d3079d8f8acfc06066097 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 16:56:31 -0500 Subject: [PATCH 34/65] rename --- selfdrive/controls/lib/longitudinal_planner.py | 8 ++++---- .../dec/{dec_longitudinal_planner.py => helpers.py} | 12 +++++++----- 2 files changed, 11 insertions(+), 9 deletions(-) rename sunnypilot/selfdrive/controls/lib/dec/{dec_longitudinal_planner.py => helpers.py} (98%) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 3b094cf4a2..2272ebfeb8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,7 +16,7 @@ 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.dec_longitudinal_planner import DecLongitudinalPlanner +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 @@ -69,11 +69,11 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05): return a_target, should_stop -class LongitudinalPlanner(DecLongitudinalPlanner): +class LongitudinalPlanner(DecPlanner): def __init__(self, CP, init_v=0.0, init_a=0.0, dt=DT_MDL): self.CP = CP self.mpc = LongitudinalMpc(dt=dt) - DecLongitudinalPlanner.__init__(self, self.CP, self.mpc) + DecPlanner.__init__(self, self.CP, self.mpc) self.fcw = False self.dt = dt self.allow_throttle = True @@ -108,7 +108,7 @@ class LongitudinalPlanner(DecLongitudinalPlanner): return x, v, a, j, throttle_prob def update(self, sm): - DecLongitudinalPlanner.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 diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec_longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/dec/helpers.py similarity index 98% rename from sunnypilot/selfdrive/controls/lib/dec/dec_longitudinal_planner.py rename to sunnypilot/selfdrive/controls/lib/dec/helpers.py index eb7a237422..1f0a22b58c 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec_longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/dec/helpers.py @@ -11,16 +11,14 @@ from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimen MpcSource = custom.LongitudinalPlanSP.MpcSource -class DecLongitudinalPlanner: +class DecPlanner: def __init__(self, CP, mpc): self.CP = CP self.mpc = mpc - self.dynamic_experimental_controller = DynamicExperimentalController() + self.is_enabled = False - def update(self, sm): - self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) - self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm) + self.dynamic_experimental_controller = DynamicExperimentalController() def get_mpc_mode(self, sm): if not self.is_enabled or not sm['selfdriveState'].experimentalMode: @@ -28,6 +26,10 @@ class DecLongitudinalPlanner: return self.dynamic_experimental_controller.get_mpc_mode() + def update(self, sm): + 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, pm): plan_sp_send = messaging.new_message('longitudinalPlanSP') From f645e2cdb001bf8c93833285d4792cac1647a864 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 17:12:31 -0500 Subject: [PATCH 35/65] check live param --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index eff8fc804a..3760d56860 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -25,6 +25,7 @@ import numpy as np 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 @@ -367,11 +368,17 @@ class DynamicExperimentalController: if self._set_mode_timeout > 0: self._set_mode_timeout -= 1 + def _read_params(self, sm): + if sm.frame % int(1. / DT_MDL) == 0: + self._is_enabled = self._params.get_bool("DynamicExperimentalControl") + def update(self, radar_unavailable, sm): car_state = sm['carState'] lead_one = sm['radarState'].leadOne md = sm['modelV2'] + self._read_params(sm) + if self._is_enabled: self._update(car_state, lead_one, md) From c64b6797045db67b5961e5b26f27e10c6f354475 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 17:16:34 -0500 Subject: [PATCH 36/65] sync with stock --- cereal/services.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cereal/services.py b/cereal/services.py index 0528f5e862..ae4a35b3e4 100755 --- a/cereal/services.py +++ b/cereal/services.py @@ -77,7 +77,7 @@ _services: dict[str, tuple] = { # sunnypilot "modelManagerSP": (False, 1., 1), "selfdriveStateSP": (True, 100., 10), - "longitudinalPlanSP": (True, 20., 5), + "longitudinalPlanSP": (True, 20., 10), # debug "uiDebug": (True, 0., 1), From bf3350b7f2e3ec005766e94718907d841a657772 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 17:38:49 -0500 Subject: [PATCH 37/65] type hint --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 57 ++++++++++--------- .../selfdrive/controls/lib/dec/helpers.py | 9 +-- 2 files changed, 35 insertions(+), 31 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 3760d56860..137a284993 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -23,6 +23,9 @@ # Version = 2024-7-11 import numpy as np + +from cereal import messaging +from opendbc.car import structs from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL @@ -70,16 +73,16 @@ class GenericMovingAverageCalculator: self.data = [] self.total = 0 - def add_data(self, value): + 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): + def get_moving_average(self) -> float | None: return None if len(self.data) == 0 else self.total / len(self.data) - def reset_data(self): + def reset_data(self) -> None: self.data = [] self.total = 0 @@ -90,19 +93,19 @@ class WeightedMovingAverageCalculator: self.data = [] self.weights = np.linspace(1, 3, window_size) # Linear weights, adjust as needed - def add_data(self, value): + 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): + def get_weighted_average(self) -> float | None: if len(self.data) == 0: return None weighted_sum = np.dot(self.data, self.weights[-len(self.data):]) weight_total = np.sum(self.weights[-len(self.data):]) return weighted_sum / weight_total - def reset_data(self): + def reset_data(self) -> None: self.data = [] @@ -150,7 +153,7 @@ class DynamicExperimentalController: self._set_mode_timeout = 0 @staticmethod - def _anomaly_detection(recent_data, threshold=2.0, context_check=True): + def _anomaly_detection(recent_data: list[float], threshold: float = 2.0, context_check: bool = True) -> bool: """ Basic anomaly detection using standard deviation. """ @@ -165,20 +168,20 @@ class DynamicExperimentalController: return np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1 return anomaly - def _adaptive_slowdown_threshold(self): + def _adaptive_slowdown_threshold(self) -> float: """ Adapts the slow-down threshold based on vehicle speed and recent behavior. """ return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.03 * np.log(1 + len(self._slow_down_gmac.data))) - def _smoothed_lead_detection(self, lead_prob, smoothing_factor=0.2): + 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 self._has_lead_filtered > LEAD_PROB - def _adaptive_lead_prob_threshold(self): + def _adaptive_lead_prob_threshold(self) -> float: """ Adapts lead probability threshold based on driving conditions. """ @@ -186,7 +189,11 @@ class DynamicExperimentalController: return LEAD_PROB + 0.1 # Increase the threshold on highways return LEAD_PROB - def _update(self, car_state, lead_one, md): + 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 @@ -252,7 +259,7 @@ class DynamicExperimentalController: self._has_standstill_prev = self._has_standstill self._has_lead_filtered_prev = self._has_lead_filtered - def _radarless_mode(self): + def _radarless_mode(self) -> None: # when mpc fcw crash prob is high # use blended to slow down quickly if self._has_mpc_fcw: @@ -302,7 +309,7 @@ class DynamicExperimentalController: self._set_mode('acc') - def _radar_mode(self): + def _radar_mode(self) -> None: # when mpc fcw crash prob is high # use blended to slow down quickly if self._has_mpc_fcw: @@ -344,43 +351,39 @@ class DynamicExperimentalController: self._set_mode('acc') - def get_mpc_mode(self): + def get_mpc_mode(self) -> str: return self._mode - def has_changed(self): + def has_changed(self) -> bool: return self._mode_changed - def set_enabled(self, enabled): + def set_enabled(self, enabled: bool) -> None: self._is_enabled = enabled - def is_enabled(self): + def is_enabled(self) -> bool: return self._is_enabled - def set_mpc_fcw_crash_cnt(self, crash_cnt): + def set_mpc_fcw_crash_cnt(self, crash_cnt: float) -> None: self._mpc_fcw_crash_cnt = crash_cnt - def _set_mode(self, mode): + def _set_mode(self, mode: str) -> None: if self._set_mode_timeout == 0: self._mode = mode - if mode == "blended": + 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, sm): + def _read_params(self, sm: messaging.SubMaster) -> None: if sm.frame % int(1. / DT_MDL) == 0: self._is_enabled = self._params.get_bool("DynamicExperimentalControl") - def update(self, radar_unavailable, sm): - car_state = sm['carState'] - lead_one = sm['radarState'].leadOne - md = sm['modelV2'] - + def update(self, radar_unavailable: bool, sm: messaging.SubMaster) -> None: self._read_params(sm) if self._is_enabled: - self._update(car_state, lead_one, md) + self._update(sm) if radar_unavailable: self._radarless_mode() diff --git a/sunnypilot/selfdrive/controls/lib/dec/helpers.py b/sunnypilot/selfdrive/controls/lib/dec/helpers.py index 1f0a22b58c..602d3b64ba 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/helpers.py +++ b/sunnypilot/selfdrive/controls/lib/dec/helpers.py @@ -6,13 +6,14 @@ 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, mpc): + def __init__(self, CP: structs.CarParams, mpc): self.CP = CP self.mpc = mpc @@ -20,17 +21,17 @@ class DecPlanner: self.dynamic_experimental_controller = DynamicExperimentalController() - def get_mpc_mode(self, sm): + 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): + 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, pm): + 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']) From 5074881d6d444fe761767f86d8febb95fe925bfd Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 17:40:21 -0500 Subject: [PATCH 38/65] unused --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 1 - 1 file changed, 1 deletion(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 137a284993..9ee7fa595b 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -25,7 +25,6 @@ import numpy as np from cereal import messaging -from opendbc.car import structs from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL From 7113d4a76ac7af3a640822ce327f1dd784bd73f6 Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sun, 12 Jan 2025 15:54:50 -0700 Subject: [PATCH 39/65] smoother trans --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 9ee7fa595b..ff57998e34 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -90,7 +90,7 @@ 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 + self.weights = np.linspace(1, 5, window_size) # Linear weights, adjust as needed def add_data(self, value: float) -> None: if len(self.data) == self.window_size: @@ -171,7 +171,7 @@ class DynamicExperimentalController: """ Adapts the slow-down threshold based on vehicle speed and recent behavior. """ - return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.03 * np.log(1 + len(self._slow_down_gmac.data))) + return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.05 * np.log(1 + len(self._slow_down_gmac.data))) def _smoothed_lead_detection(self, lead_prob: float, smoothing_factor: float = 0.2) -> bool: """ @@ -207,9 +207,9 @@ class DynamicExperimentalController: # 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) + #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() From 6e86d242cd28761533a6c37349236e9dc373b9b0 Mon Sep 17 00:00:00 2001 From: rav4kumar Date: Sun, 12 Jan 2025 16:03:17 -0700 Subject: [PATCH 40/65] window time --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index ff57998e34..9b77768dd3 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -90,7 +90,7 @@ class WeightedMovingAverageCalculator: def __init__(self, window_size): self.window_size = window_size self.data = [] - self.weights = np.linspace(1, 5, window_size) # Linear weights, adjust as needed + 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: From c34a21980e8a66f70868f6b36a665ecc46100c0a Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 18:13:52 -0500 Subject: [PATCH 41/65] fix type hint --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 23 +++++++++++--------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 9b77768dd3..f5feb0401f 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -158,35 +158,38 @@ class DynamicExperimentalController: """ if len(recent_data) < 5: return False - mean = np.mean(recent_data) - std_dev = np.std(recent_data) - anomaly = recent_data[-1] > mean + threshold * std_dev + 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 np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1 + 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. """ - return interp(self._v_ego_kph, SLOW_DOWN_BP, SLOW_DOWN_DIST) * (1.0 + 0.05 * np.log(1 + len(self._slow_down_gmac.data))) + 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 self._has_lead_filtered > 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 LEAD_PROB + 0.1 # Increase the threshold on highways - return LEAD_PROB + 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'] @@ -351,10 +354,10 @@ class DynamicExperimentalController: self._set_mode('acc') def get_mpc_mode(self) -> str: - return self._mode + return str(self._mode) def has_changed(self) -> bool: - return self._mode_changed + return bool(self._mode_changed) def set_enabled(self, enabled: bool) -> None: self._is_enabled = enabled From a8deaa69b89f599f86ae517827a570789566e079 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 18:20:06 -0500 Subject: [PATCH 42/65] pass sm.frame from plannerd --- selfdrive/controls/lib/longitudinal_planner.py | 4 ++-- selfdrive/controls/plannerd.py | 2 +- sunnypilot/selfdrive/controls/lib/dec/dec.py | 8 ++++---- sunnypilot/selfdrive/controls/lib/dec/helpers.py | 4 ++-- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2272ebfeb8..dae13c328a 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -107,8 +107,8 @@ class LongitudinalPlanner(DecPlanner): throttle_prob = 1.0 return x, v, a, j, throttle_prob - def update(self, sm): - DecPlanner.update(self, sm) + def update(self, sm, frame): + DecPlanner.update(self, sm, frame) 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 diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 16ac80db60..252540ac19 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -25,7 +25,7 @@ def main(): while True: sm.update() if sm.updated['modelV2']: - longitudinal_planner.update(sm) + longitudinal_planner.update(sm, sm.frame) longitudinal_planner.publish(sm, pm) ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl']) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index f5feb0401f..b114829cb5 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -377,12 +377,12 @@ class DynamicExperimentalController: if self._set_mode_timeout > 0: self._set_mode_timeout -= 1 - def _read_params(self, sm: messaging.SubMaster) -> None: - if sm.frame % int(1. / DT_MDL) == 0: + def _read_params(self, frame: int) -> None: + if 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(sm) + def update(self, radar_unavailable: bool, sm: messaging.SubMaster, frame: int) -> None: + self._read_params(frame) if self._is_enabled: self._update(sm) diff --git a/sunnypilot/selfdrive/controls/lib/dec/helpers.py b/sunnypilot/selfdrive/controls/lib/dec/helpers.py index 602d3b64ba..4c6cfa5c8a 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/helpers.py +++ b/sunnypilot/selfdrive/controls/lib/dec/helpers.py @@ -27,9 +27,9 @@ class DecPlanner: return self.dynamic_experimental_controller.get_mpc_mode() - def update(self, sm: messaging.SubMaster) -> None: + def update(self, sm: messaging.SubMaster, frame: int) -> None: self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) - self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm) + self.dynamic_experimental_controller.update(self.CP.radarUnavailable, sm, frame) def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: plan_sp_send = messaging.new_message('longitudinalPlanSP') From 1b2586914f8ca4acca70164deb3e36760fa94c6d Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 18:24:58 -0500 Subject: [PATCH 43/65] more fixes --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index b114829cb5..02b164275d 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -100,8 +100,8 @@ class WeightedMovingAverageCalculator: def get_weighted_average(self) -> float | None: if len(self.data) == 0: return None - weighted_sum = np.dot(self.data, self.weights[-len(self.data):]) - weight_total = np.sum(self.weights[-len(self.data):]) + 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: @@ -160,11 +160,12 @@ class DynamicExperimentalController: 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) + anomaly: bool = bool(float(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) + count_above_threshold: int = int(np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev)) + return count_above_threshold > 1 return anomaly def _adaptive_slowdown_threshold(self) -> float: From 1c1ef0648909d93c77a6b665d68b5838c218ff50 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 18:28:20 -0500 Subject: [PATCH 44/65] more --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 02b164275d..3eca4ff966 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -165,7 +165,7 @@ class DynamicExperimentalController: # Context check to ensure repeated anomaly if context_check: count_above_threshold: int = int(np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev)) - return count_above_threshold > 1 + return bool(count_above_threshold > 1) return anomaly def _adaptive_slowdown_threshold(self) -> float: From b42c060b2e2a249eface77e10827d8c73670112f Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 18:37:47 -0500 Subject: [PATCH 45/65] more explicit --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 3eca4ff966..3d7c8f7f22 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -111,10 +111,10 @@ class WeightedMovingAverageCalculator: class DynamicExperimentalController: def __init__(self, params=None): self._params = params or Params() - self._is_enabled = self._params.get_bool("DynamicExperimentalControl") - self._mode = 'acc' - self._mode_prev = 'acc' - self._mode_changed = False + self._is_enabled: bool = self._params.get_bool("DynamicExperimentalControl") + self._mode: str = 'acc' + self._mode_prev: str = 'acc' + self._mode_changed: bool = False # Use weighted moving average for filtering leads self._lead_gmac = WeightedMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE) @@ -160,12 +160,11 @@ class DynamicExperimentalController: return False mean: float = float(np.mean(recent_data)) std_dev: float = float(np.std(recent_data)) - anomaly: bool = bool(float(recent_data[-1]) > mean + threshold * std_dev) + anomaly: bool = bool(recent_data[-1] > mean + threshold * std_dev) # Context check to ensure repeated anomaly if context_check: - count_above_threshold: int = int(np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev)) - return bool(count_above_threshold > 1) + return bool(np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1) return anomaly def _adaptive_slowdown_threshold(self) -> float: From 635b15f2bc11a8ddd5c4194546d51df33de324a0 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 18:54:17 -0500 Subject: [PATCH 46/65] fix test --- selfdrive/test/longitudinal_maneuvers/plant.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index c08ac6d369..3bbd18f726 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -45,6 +45,7 @@ class Plant: self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) self.ts = 1. / self.rate + self.frame = 0 time.sleep(0.1) self.sm = messaging.SubMaster(['longitudinalPlan']) @@ -133,7 +134,7 @@ class Plant: 'selfdriveState': ss.selfdriveState, 'liveParameters': lp.liveParameters, 'modelV2': model.modelV2} - self.planner.update(sm) + self.planner.update(sm, self.frame) self.speed = self.planner.v_desired_filter.x self.acceleration = self.planner.a_desired self.speeds = self.planner.v_desired_trajectory.tolist() @@ -160,6 +161,7 @@ class Plant: # 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)) + self.frame += 1 # ******** update prevs ******** self.rk.monitor_time() From ea6cd3429b6b5558c8252f0ecc5703ed21c2d556 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 18:55:33 -0500 Subject: [PATCH 47/65] Revert "fix test" This reverts commit 635b15f2bc11a8ddd5c4194546d51df33de324a0. --- selfdrive/test/longitudinal_maneuvers/plant.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 3bbd18f726..c08ac6d369 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -45,7 +45,6 @@ class Plant: self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0) self.ts = 1. / self.rate - self.frame = 0 time.sleep(0.1) self.sm = messaging.SubMaster(['longitudinalPlan']) @@ -134,7 +133,7 @@ class Plant: 'selfdriveState': ss.selfdriveState, 'liveParameters': lp.liveParameters, 'modelV2': model.modelV2} - self.planner.update(sm, self.frame) + self.planner.update(sm) self.speed = self.planner.v_desired_filter.x self.acceleration = self.planner.a_desired self.speeds = self.planner.v_desired_trajectory.tolist() @@ -161,7 +160,6 @@ class Plant: # 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)) - self.frame += 1 # ******** update prevs ******** self.rk.monitor_time() From 67692babdce63c7ce25a8da1e9d71e88fc61e875 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 18:55:38 -0500 Subject: [PATCH 48/65] Revert "pass sm.frame from plannerd" This reverts commit a8deaa69b89f599f86ae517827a570789566e079. --- selfdrive/controls/lib/longitudinal_planner.py | 4 ++-- selfdrive/controls/plannerd.py | 2 +- sunnypilot/selfdrive/controls/lib/dec/dec.py | 8 ++++---- sunnypilot/selfdrive/controls/lib/dec/helpers.py | 4 ++-- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index dae13c328a..2272ebfeb8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -107,8 +107,8 @@ class LongitudinalPlanner(DecPlanner): throttle_prob = 1.0 return x, v, a, j, throttle_prob - def update(self, sm, frame): - DecPlanner.update(self, sm, frame) + 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 diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 252540ac19..16ac80db60 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -25,7 +25,7 @@ def main(): while True: sm.update() if sm.updated['modelV2']: - longitudinal_planner.update(sm, sm.frame) + longitudinal_planner.update(sm) longitudinal_planner.publish(sm, pm) ldw.update(sm.frame, sm['modelV2'], sm['carState'], sm['carControl']) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 3d7c8f7f22..078de0f59f 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -377,12 +377,12 @@ class DynamicExperimentalController: if self._set_mode_timeout > 0: self._set_mode_timeout -= 1 - def _read_params(self, frame: int) -> None: - if frame % int(1. / DT_MDL) == 0: + def _read_params(self, sm: messaging.SubMaster) -> None: + if sm.frame % int(1. / DT_MDL) == 0: self._is_enabled = self._params.get_bool("DynamicExperimentalControl") - def update(self, radar_unavailable: bool, sm: messaging.SubMaster, frame: int) -> None: - self._read_params(frame) + def update(self, radar_unavailable: bool, sm: messaging.SubMaster) -> None: + self._read_params(sm) if self._is_enabled: self._update(sm) diff --git a/sunnypilot/selfdrive/controls/lib/dec/helpers.py b/sunnypilot/selfdrive/controls/lib/dec/helpers.py index 4c6cfa5c8a..602d3b64ba 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/helpers.py +++ b/sunnypilot/selfdrive/controls/lib/dec/helpers.py @@ -27,9 +27,9 @@ class DecPlanner: return self.dynamic_experimental_controller.get_mpc_mode() - def update(self, sm: messaging.SubMaster, frame: int) -> None: + 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, frame) + 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') From f6ef036158a98f33f6f54b99a2a5acf70aa48782 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 12 Jan 2025 18:57:22 -0500 Subject: [PATCH 49/65] use internal frame --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 078de0f59f..eb7660965e 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -115,6 +115,7 @@ class DynamicExperimentalController: 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) @@ -377,12 +378,12 @@ class DynamicExperimentalController: if self._set_mode_timeout > 0: self._set_mode_timeout -= 1 - def _read_params(self, sm: messaging.SubMaster) -> None: - if sm.frame % int(1. / DT_MDL) == 0: + 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(sm) + self._read_params() if self._is_enabled: self._update(sm) @@ -394,3 +395,5 @@ class DynamicExperimentalController: self._mode_changed = self._mode != self._mode_prev self._mode_prev = self._mode + + self._frame += 1 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 50/65] 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"), From dff2a5796d0bd24c06b3c2e2ead5009c5cada95c Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 13 Jan 2025 11:41:11 -0500 Subject: [PATCH 51/65] update name --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index eb7660965e..de7597207c 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -192,7 +192,7 @@ class DynamicExperimentalController: return float(LEAD_PROB + 0.1) # Increase the threshold on highways return float(LEAD_PROB) - def _update(self, sm: messaging.SubMaster) -> None: + def _update_calculations(self, sm: messaging.SubMaster) -> None: car_state = sm['carState'] lead_one = sm['radarState'].leadOne md = sm['modelV2'] @@ -386,7 +386,7 @@ class DynamicExperimentalController: self._read_params() if self._is_enabled: - self._update(sm) + self._update_calculations(sm) if radar_unavailable: self._radarless_mode() From 01c5dbdc4cc9cda824db217c91977b5c862e675d Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 13 Jan 2025 16:14:00 -0500 Subject: [PATCH 52/65] Hyundai CAN: auto-enable radar tracks on applicable Mando radar (#561) * more * event and checks * comments * missed events * retry 2 times is enough * rename to radar tracks * fix data type * more rename * bump opendbc * drain first * put it behind a toggle lol * re-enable * update comments * revert lead smoothing * Revert "revert lead smoothing" This reverts commit 872267970c3dd1dbfd64a5bdd21d2e1b9ea600bf. * real events and radard engagement * only show up for hyundai with mando * update translations * bump opendbc * fix event name * update description * move above * translations --------- Co-authored-by: rav4kumar --- cereal/log.capnp | 1 + common/params.cc | 6 ++++ opendbc_repo | 2 +- selfdrive/car/card.py | 3 ++ selfdrive/selfdrived/events.py | 3 ++ selfdrive/selfdrived/selfdrived.py | 7 ++++ selfdrive/ui/qt/offroad/developer_panel.cc | 18 ++++++++++ selfdrive/ui/qt/offroad/developer_panel.h | 1 + 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/car/car_specific.py | 34 ++++++++++++++++++ sunnypilot/selfdrive/car/interfaces.py | 41 ++++++++++++++++++++++ 21 files changed, 203 insertions(+), 1 deletion(-) create mode 100644 sunnypilot/selfdrive/car/car_specific.py create mode 100644 sunnypilot/selfdrive/car/interfaces.py diff --git a/cereal/log.capnp b/cereal/log.capnp index d5fbad6fe8..fb38185dfe 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -198,6 +198,7 @@ struct OnroadEvent @0xc4fa6047f024e718 { silentSeatbeltNotLatched @161; silentParkBrake @162; controlsMismatchLateral @163; + hyundaiRadarTracksConfirmed @164; soundsUnavailableDEPRECATED @47; } diff --git a/common/params.cc b/common/params.cc index 7c1b21c00c..d86dd58c9f 100644 --- a/common/params.cc +++ b/common/params.cc @@ -223,6 +223,12 @@ std::unordered_map keys = { {"SunnylinkDongleId", PERSISTENT}, {"SunnylinkdPid", PERSISTENT}, {"SunnylinkEnabled", PERSISTENT}, + + // sunnypilot car specific params + {"HyundaiRadarTracks", PERSISTENT}, + {"HyundaiRadarTracksConfirmed", PERSISTENT}, + {"HyundaiRadarTracksPersistent", PERSISTENT}, + {"HyundaiRadarTracksToggle", PERSISTENT}, }; } // namespace diff --git a/opendbc_repo b/opendbc_repo index 6ddebc9a03..6a2ad131eb 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 6ddebc9a0365d3421bee7cceddb0263c88fb78dd +Subproject commit 6a2ad131ebfe7a15dca5aae391ef5225189c3c2d diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 1852dccf74..e093b5e0d0 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -23,6 +23,7 @@ from openpilot.selfdrive.car.cruise import VCruiseHelper from openpilot.selfdrive.car.car_specific import MockCarState from openpilot.sunnypilot.mads.mads import MadsParams +from openpilot.sunnypilot.selfdrive.car.interfaces import setup_car_interface_sp, initialize_car_interface_sp REPLAY = "REPLAY" in os.environ @@ -100,6 +101,7 @@ class Car: cached_params = _cached_params self.CI = get_car(*self.can_callbacks, obd_callback(self.params), experimental_long_allowed, num_pandas, cached_params) + setup_car_interface_sp(self.CI.CP, self.params) self.RI = get_radar_interface(self.CI.CP) self.CP = self.CI.CP @@ -230,6 +232,7 @@ class Car: # Initialize CarInterface, once controls are ready # TODO: this can make us miss at least a few cycles when doing an ECU knockout self.CI.init(self.CP, *self.can_callbacks) + initialize_car_interface_sp(self.CP, self.params, *self.can_callbacks) # signal pandad to switch to car safety mode self.params.put_bool_nonblocking("ControlsReady", True) diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 672d809dd9..481a33079b 100755 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -1061,6 +1061,9 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { ET.NO_ENTRY: NoEntryAlert("Controls Mismatch: Lateral"), }, + EventName.hyundaiRadarTracksConfirmed: { + ET.PERMANENT: NormalPermanentAlert("Radar tracks available. Restart the car to initialize") + } } diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 8328bf67f0..759682f44f 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -24,6 +24,7 @@ from openpilot.selfdrive.controls.lib.latcontrol import MIN_LATERAL_CONTROL_SPEE from openpilot.system.version import get_build_metadata from openpilot.sunnypilot.mads.mads import ModularAssistiveDrivingSystem +from openpilot.sunnypilot.selfdrive.car.car_specific import CarSpecificEventsSP REPLAY = "REPLAY" in os.environ SIMULATION = "SIMULATION" in os.environ @@ -137,6 +138,8 @@ class SelfdriveD: sock_services = list(self.pm.sock.keys()) + ['selfdriveStateSP'] self.pm = messaging.PubMaster(sock_services) + self.car_events_sp = CarSpecificEventsSP(self.CP, self.params) + def update_events(self, CS): """Compute onroadEvents from carState""" @@ -177,6 +180,9 @@ class SelfdriveD: car_events = self.car_events.update(CS, self.CS_prev, self.sm['carControl']).to_msg() self.events.add_from_msg(car_events) + car_events_sp = self.car_events_sp.update().to_msg() + self.events.add_from_msg(car_events_sp) + if self.CP.notCar: # wait for everything to init first if self.sm.frame > int(5. / DT_CTRL) and self.initialized: @@ -495,6 +501,7 @@ class SelfdriveD: self.personality = self.read_personality_param() self.mads.read_params() + self.car_events_sp.read_params() time.sleep(0.1) def run(self): diff --git a/selfdrive/ui/qt/offroad/developer_panel.cc b/selfdrive/ui/qt/offroad/developer_panel.cc index eee48a6bdb..1a362f62c6 100644 --- a/selfdrive/ui/qt/offroad/developer_panel.cc +++ b/selfdrive/ui/qt/offroad/developer_panel.cc @@ -29,6 +29,18 @@ DeveloperPanel::DeveloperPanel(SettingsWindow *parent) : ListWidget(parent) { }); addItem(longManeuverToggle); + // TODO-SP: Move to Vehicles panel when ported back + hyundaiRadarTracksToggle = new ParamControl( + "HyundaiRadarTracksToggle", + tr("Hyundai: Enable Radar Tracks"), + tr("Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. " + "This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance."), ""); + hyundaiRadarTracksToggle->setConfirmation(true, false); + QObject::connect(hyundaiRadarTracksToggle, &ParamControl::toggleFlipped, [=](bool state) { + updateToggles(offroad); + }); + addItem(hyundaiRadarTracksToggle); + auto enableGithubRunner = new ParamControl("EnableGithubRunner", tr("Enable GitHub runner service"), tr("Enables or disables the github runner service."), ""); addItem(enableGithubRunner); @@ -51,9 +63,15 @@ void DeveloperPanel::updateToggles(bool _offroad) { AlignedBuffer aligned_buf; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size())); cereal::CarParams::Reader CP = cmsg.getRoot(); + + auto hyundai = CP.getCarName() == "hyundai"; + auto hyundai_mando_radar = hyundai && (CP.getFlags() & 4096); + longManeuverToggle->setEnabled(hasLongitudinalControl(CP) && _offroad); + hyundaiRadarTracksToggle->setVisible(hyundai_mando_radar && hasLongitudinalControl(CP)); } else { longManeuverToggle->setEnabled(false); + hyundaiRadarTracksToggle->setVisible(false); } offroad = _offroad; diff --git a/selfdrive/ui/qt/offroad/developer_panel.h b/selfdrive/ui/qt/offroad/developer_panel.h index 0351cd045c..5dc5a5c494 100644 --- a/selfdrive/ui/qt/offroad/developer_panel.h +++ b/selfdrive/ui/qt/offroad/developer_panel.h @@ -16,6 +16,7 @@ private: Params params; ParamControl* joystickToggle; ParamControl* longManeuverToggle; + ParamControl* hyundaiRadarTracksToggle; bool is_release; bool offroad; diff --git a/selfdrive/ui/translations/main_ar.ts b/selfdrive/ui/translations/main_ar.ts index bd1452612c..482893fae5 100644 --- a/selfdrive/ui/translations/main_ar.ts +++ b/selfdrive/ui/translations/main_ar.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_de.ts b/selfdrive/ui/translations/main_de.ts index f015d2ea5f..3129b79f0d 100644 --- a/selfdrive/ui/translations/main_de.ts +++ b/selfdrive/ui/translations/main_de.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_es.ts b/selfdrive/ui/translations/main_es.ts index 658f131a36..dd58f9ddb5 100644 --- a/selfdrive/ui/translations/main_es.ts +++ b/selfdrive/ui/translations/main_es.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_fr.ts b/selfdrive/ui/translations/main_fr.ts index 7e2f6649d6..dac8c0373b 100644 --- a/selfdrive/ui/translations/main_fr.ts +++ b/selfdrive/ui/translations/main_fr.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_ja.ts b/selfdrive/ui/translations/main_ja.ts index 0229a36bd6..28a4db8a8f 100644 --- a/selfdrive/ui/translations/main_ja.ts +++ b/selfdrive/ui/translations/main_ja.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index 0b1c82e92c..af62de8bd5 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_pt-BR.ts b/selfdrive/ui/translations/main_pt-BR.ts index 2a2693961e..004f03affe 100644 --- a/selfdrive/ui/translations/main_pt-BR.ts +++ b/selfdrive/ui/translations/main_pt-BR.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_th.ts b/selfdrive/ui/translations/main_th.ts index c2557a6faa..1c28c0ebd1 100644 --- a/selfdrive/ui/translations/main_th.ts +++ b/selfdrive/ui/translations/main_th.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_tr.ts b/selfdrive/ui/translations/main_tr.ts index ac22574b97..81bc70e154 100644 --- a/selfdrive/ui/translations/main_tr.ts +++ b/selfdrive/ui/translations/main_tr.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index d284bbac6e..81de7f76b8 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 37c9d0c70f..0f8a40ed29 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -131,6 +131,14 @@ Enable GitHub runner service + + Hyundai: Enable Radar Tracks + + + + Enable this to attempt to enable radar tracks for Hyundai, Kia, and Genesis models equipped with the supported Mando SCC radar. This allows sunnypilot to use radar data for improved lead tracking and overall longitudinal performance. + + DevicePanel diff --git a/sunnypilot/selfdrive/car/car_specific.py b/sunnypilot/selfdrive/car/car_specific.py new file mode 100644 index 0000000000..5abb5603d6 --- /dev/null +++ b/sunnypilot/selfdrive/car/car_specific.py @@ -0,0 +1,34 @@ +""" +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 log +from opendbc.car import structs + +from openpilot.selfdrive.selfdrived.events import Events + +EventName = log.OnroadEvent.EventName + + +class CarSpecificEventsSP: + def __init__(self, CP: structs.CarParams, params): + self.CP = CP + self.params = params + + self.hyundai_radar_tracks = self.params.get_bool("HyundaiRadarTracks") + self.hyundai_radar_tracks_confirmed = self.params.get_bool("HyundaiRadarTracksConfirmed") + + def read_params(self): + self.hyundai_radar_tracks = self.params.get_bool("HyundaiRadarTracks") + self.hyundai_radar_tracks_confirmed = self.params.get_bool("HyundaiRadarTracksConfirmed") + + def update(self): + events = Events() + if self.CP.carName == 'hyundai': + if self.hyundai_radar_tracks and not self.hyundai_radar_tracks_confirmed: + events.add(EventName.hyundaiRadarTracksConfirmed) + + return events diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py new file mode 100644 index 0000000000..a92f8f60be --- /dev/null +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -0,0 +1,41 @@ +""" +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 opendbc.car import Bus, structs +from opendbc.car.can_definitions import CanRecvCallable, CanSendCallable +from opendbc.car.car_helpers import can_fingerprint +from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR +from opendbc.car.hyundai.values import HyundaiFlags, DBC as HYUNDAI_DBC +from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP + + +def setup_car_interface_sp(CP: structs.CarParams, params): + if CP.carName == 'hyundai': + if CP.flags & HyundaiFlags.MANDO_RADAR and CP.radarUnavailable: + # Having this automatic without a toggle causes a weird process replay diff because + # somehow it sees fewer logs than intended + if params.get_bool("HyundaiRadarTracksToggle"): + CP.sunnypilotFlags |= HyundaiFlagsSP.ENABLE_RADAR_TRACKS.value + if params.get_bool("HyundaiRadarTracks"): + CP.radarUnavailable = False + + +def initialize_car_interface_sp(CP: structs.CarParams, params, can_recv: CanRecvCallable, can_send: CanSendCallable): + if CP.carName == 'hyundai': + if CP.sunnypilotFlags & HyundaiFlagsSP.ENABLE_RADAR_TRACKS: + can_recv() + _, fingerprint = can_fingerprint(can_recv) + radar_unavailable = RADAR_START_ADDR not in fingerprint[1] or Bus.radar not in HYUNDAI_DBC[CP.carFingerprint] + + radar_tracks = params.get_bool("HyundaiRadarTracks") + radar_tracks_persistent = params.get_bool("HyundaiRadarTracksPersistent") + + params.put_bool_nonblocking("HyundaiRadarTracksConfirmed", radar_tracks) + + if not radar_tracks_persistent: + params.put_bool_nonblocking("HyundaiRadarTracks", not radar_unavailable) + params.put_bool_nonblocking("HyundaiRadarTracksPersistent", True) From 223abdc536c93024ff494ca56fb52fafa056692a Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 13 Jan 2025 23:26:07 -0500 Subject: [PATCH 53/65] never used --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index de7597207c..5e230df372 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -113,8 +113,6 @@ class DynamicExperimentalController: 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 @@ -357,12 +355,6 @@ class DynamicExperimentalController: 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 @@ -393,7 +385,4 @@ class DynamicExperimentalController: else: self._radar_mode() - self._mode_changed = self._mode != self._mode_prev - self._mode_prev = self._mode - self._frame += 1 From 85faddc7afd230e2fac04cd0841d9443877c7d82 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 13 Jan 2025 23:46:38 -0500 Subject: [PATCH 54/65] this is why it was never using DEC --- sunnypilot/selfdrive/controls/lib/dec/helpers.py | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/helpers.py b/sunnypilot/selfdrive/controls/lib/dec/helpers.py index 602d3b64ba..d232ec7bf0 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/helpers.py +++ b/sunnypilot/selfdrive/controls/lib/dec/helpers.py @@ -16,13 +16,10 @@ 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: + if not self.dynamic_experimental_controller.is_enabled() or not sm['selfdriveState'].experimentalMode: return None return self.dynamic_experimental_controller.get_mpc_mode() From e630546250bf141e8152553c1a8d9eded00ea602 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 13 Jan 2025 23:57:21 -0500 Subject: [PATCH 55/65] more logs --- cereal/custom.capnp | 17 +++++++++++------ selfdrive/controls/lib/longitudinal_planner.py | 2 +- sunnypilot/selfdrive/controls/lib/dec/dec.py | 18 ++++++++++++------ .../selfdrive/controls/lib/dec/helpers.py | 15 ++++++++------- 4 files changed, 32 insertions(+), 20 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index bffc1e4d3b..9c83b0f6fc 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -83,12 +83,17 @@ struct ModelManagerSP @0xaedffd8f31e7b55d { } struct LongitudinalPlanSP @0xf35cc4560bbf6ec2 { - mpcSource @0 :MpcSource; - dynamicExperimentalControl @1 :Bool; - - enum MpcSource { - acc @0; - blended @1; + dec @0 :DynamicExperimentalControl; + + struct DynamicExperimentalControl { + state @0 :DynamicExperimentalControlState; + enabled @1 :Bool; + active @2 :Bool; + + enum DynamicExperimentalControlState { + acc @0; + blended @1; + } } } diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2272ebfeb8..87c7832462 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -110,7 +110,7 @@ class LongitudinalPlanner(DecPlanner): 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): + if dec_mpc_mode := self.get_mpc_mode(): self.mpc.mode = dec_mpc_mode if len(sm['carControl'].orientationNED) == 3: diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 5e230df372..d7410d5993 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -111,7 +111,8 @@ class WeightedMovingAverageCalculator: class DynamicExperimentalController: def __init__(self, params=None): self._params = params or Params() - self._is_enabled: bool = self._params.get_bool("DynamicExperimentalControl") + self._enabled: bool = self._params.get_bool("DynamicExperimentalControl") + self._active: bool = False self._mode: str = 'acc' self._frame: int = 0 @@ -352,11 +353,14 @@ class DynamicExperimentalController: self._set_mode('acc') - def get_mpc_mode(self) -> str: + def mode(self) -> str: return str(self._mode) - def is_enabled(self) -> bool: - return self._is_enabled + def enabled(self) -> bool: + return self._enabled + + def active(self) -> bool: + return self._active def set_mpc_fcw_crash_cnt(self, crash_cnt: float) -> None: self._mpc_fcw_crash_cnt = crash_cnt @@ -372,12 +376,12 @@ class DynamicExperimentalController: def _read_params(self) -> None: if self._frame % int(1. / DT_MDL) == 0: - self._is_enabled = self._params.get_bool("DynamicExperimentalControl") + self._enabled = self._params.get_bool("DynamicExperimentalControl") def update(self, radar_unavailable: bool, sm: messaging.SubMaster) -> None: self._read_params() - if self._is_enabled: + if self._enabled: self._update_calculations(sm) if radar_unavailable: @@ -385,4 +389,6 @@ class DynamicExperimentalController: else: self._radar_mode() + self._active = sm['selfdriveState'].experimentalMode and self._enabled + self._frame += 1 diff --git a/sunnypilot/selfdrive/controls/lib/dec/helpers.py b/sunnypilot/selfdrive/controls/lib/dec/helpers.py index d232ec7bf0..ced2d4b306 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/helpers.py +++ b/sunnypilot/selfdrive/controls/lib/dec/helpers.py @@ -9,7 +9,7 @@ from cereal import messaging, custom from opendbc.car import structs from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController -MpcSource = custom.LongitudinalPlanSP.MpcSource +DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState class DecPlanner: @@ -18,11 +18,11 @@ class DecPlanner: self.mpc = mpc self.dynamic_experimental_controller = DynamicExperimentalController() - def get_mpc_mode(self, sm: messaging.SubMaster): - if not self.dynamic_experimental_controller.is_enabled() or not sm['selfdriveState'].experimentalMode: + def get_mpc_mode(self) -> str | None: + if not self.dynamic_experimental_controller.active(): return None - return self.dynamic_experimental_controller.get_mpc_mode() + return self.dynamic_experimental_controller.mode() def update(self, sm: messaging.SubMaster) -> None: self.dynamic_experimental_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) @@ -36,8 +36,9 @@ class DecPlanner: 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() + dec = longitudinalPlanSP.dec + dec.state = DecState.blended if self.dynamic_experimental_controller.mode() == 'blended' else DecState.acc + dec.enabled = self.dynamic_experimental_controller.enabled() + dec.active = self.dynamic_experimental_controller.active() pm.send('longitudinalPlanSP', plan_sp_send) From a56e1e6e69f442e2f95e8a71377199e043dc2509 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 14 Jan 2025 00:01:31 -0500 Subject: [PATCH 56/65] slight cleanup --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 2 +- .../controls/lib/dec/tests/pytest_dynamic_controller.py | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index d7410d5993..91b4865fd2 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -164,7 +164,7 @@ class DynamicExperimentalController: # Context check to ensure repeated anomaly if context_check: - return bool(np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1) + return np.count_nonzero(np.array(recent_data) > mean + threshold * std_dev) > 1 return anomaly def _adaptive_slowdown_threshold(self) -> float: diff --git a/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py index af77639424..6d6c98daf4 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py @@ -48,8 +48,7 @@ def interp(monkeypatch): def controller(interp): params = Params() params.put_bool("DynamicExperimentalControl", True) - controller = DynamicExperimentalController() - return controller + return DynamicExperimentalController() def test_initial_state(controller): """Test initial state of the controller""" From 1f39c4ccfb0305d35f1ae14f0023980866a03ef4 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 14 Jan 2025 00:04:48 -0500 Subject: [PATCH 57/65] remove to fail test --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 91b4865fd2..1801f9946b 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -176,12 +176,12 @@ class DynamicExperimentalController: ) return adaptive_threshold - def _smoothed_lead_detection(self, lead_prob: float, smoothing_factor: float = 0.2) -> bool: + def _smoothed_lead_detection(self, lead_prob: float, smoothing_factor: float = 0.2): """ 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) + return self._has_lead_filtered > LEAD_PROB def _adaptive_lead_prob_threshold(self) -> float: """ From f4af0aa422c42a8aea522711f53cf67d433bd80a Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 14 Jan 2025 16:42:41 -0500 Subject: [PATCH 58/65] update name --- .../selfdrive/controls/lib/dec/helpers.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/helpers.py b/sunnypilot/selfdrive/controls/lib/dec/helpers.py index ced2d4b306..9ef3e8489a 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/helpers.py +++ b/sunnypilot/selfdrive/controls/lib/dec/helpers.py @@ -16,17 +16,17 @@ class DecPlanner: def __init__(self, CP: structs.CarParams, mpc): self.CP = CP self.mpc = mpc - self.dynamic_experimental_controller = DynamicExperimentalController() + self.dec = DynamicExperimentalController() def get_mpc_mode(self) -> str | None: - if not self.dynamic_experimental_controller.active(): + if not self.dec.active(): return None - return self.dynamic_experimental_controller.mode() + return self.dec.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) + self.dec.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) + self.dec.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') @@ -35,10 +35,10 @@ class DecPlanner: longitudinalPlanSP = plan_sp_send.longitudinalPlanSP - # DEC + # Dynamic Experimental Control dec = longitudinalPlanSP.dec - dec.state = DecState.blended if self.dynamic_experimental_controller.mode() == 'blended' else DecState.acc - dec.enabled = self.dynamic_experimental_controller.enabled() - dec.active = self.dynamic_experimental_controller.active() + dec.state = DecState.blended if self.dec.mode() == 'blended' else DecState.acc + dec.enabled = self.dec.enabled() + dec.active = self.dec.active() pm.send('longitudinalPlanSP', plan_sp_send) From d01b02b18535ff6a221b561e9d7fe2e1a70873c8 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 14 Jan 2025 20:50:57 -0500 Subject: [PATCH 59/65] more --- selfdrive/controls/lib/longitudinal_planner.py | 2 +- sunnypilot/selfdrive/controls/lib/dec/helpers.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 87c7832462..237cf0263f 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -110,7 +110,7 @@ class LongitudinalPlanner(DecPlanner): 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(): + if dec_mpc_mode := self.get_dec_mpc_mode(): self.mpc.mode = dec_mpc_mode if len(sm['carControl'].orientationNED) == 3: diff --git a/sunnypilot/selfdrive/controls/lib/dec/helpers.py b/sunnypilot/selfdrive/controls/lib/dec/helpers.py index 9ef3e8489a..4eb7be0afa 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/helpers.py +++ b/sunnypilot/selfdrive/controls/lib/dec/helpers.py @@ -18,7 +18,7 @@ class DecPlanner: self.mpc = mpc self.dec = DynamicExperimentalController() - def get_mpc_mode(self) -> str | None: + def get_dec_mpc_mode(self) -> str | None: if not self.dec.active(): return None From 21793721cc6667250f4e234ed3ceb60d7dc028a1 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 14 Jan 2025 21:29:42 -0500 Subject: [PATCH 60/65] rename --- .../controls/lib/longitudinal_planner.py | 11 ++++--- sunnypilot/selfdrive/controls/lib/dec/dec.py | 32 +++++++++++-------- .../helpers.py => longitudinal_planner.py} | 11 +++---- 3 files changed, 28 insertions(+), 26 deletions(-) rename sunnypilot/selfdrive/controls/lib/{dec/helpers.py => longitudinal_planner.py} (83%) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 237cf0263f..c15a65bf49 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,7 +16,7 @@ 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 +from openpilot.sunnypilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlannerSP LON_MPC_STEP = 0.2 # first step is 0.2s A_CRUISE_MIN = -1.2 @@ -69,11 +69,11 @@ def get_accel_from_plan(speeds, accels, action_t=DT_MDL, vEgoStopping=0.05): return a_target, should_stop -class LongitudinalPlanner(DecPlanner): +class LongitudinalPlanner(LongitudinalPlannerSP): 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) + LongitudinalPlannerSP.__init__(self, self.CP, self.mpc) self.fcw = False self.dt = dt self.allow_throttle = True @@ -108,9 +108,9 @@ class LongitudinalPlanner(DecPlanner): return x, v, a, j, throttle_prob def update(self, sm): - DecPlanner.update(self, sm) + LongitudinalPlannerSP.update(self, sm) self.mpc.mode = 'blended' if sm['selfdriveState'].experimentalMode else 'acc' - if dec_mpc_mode := self.get_dec_mpc_mode(): + if dec_mpc_mode := self.get_mpc_mode(): self.mpc.mode = dec_mpc_mode if len(sm['carControl'].orientationNED) == 3: @@ -212,4 +212,5 @@ class LongitudinalPlanner(DecPlanner): longitudinalPlan.allowThrottle = self.allow_throttle pm.send('longitudinalPlan', plan_send) + self.publish_longitudinal_plan_sp(sm, pm) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 1801f9946b..931d86f09d 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -25,6 +25,7 @@ import numpy as np from cereal import messaging +from opendbc.car import structs from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL @@ -109,7 +110,9 @@ class WeightedMovingAverageCalculator: class DynamicExperimentalController: - def __init__(self, params=None): + def __init__(self, CP: structs.CarParams, mpc, params=None): + self._CP = CP + self._mpc = mpc self._params = params or Params() self._enabled: bool = self._params.get_bool("DynamicExperimentalControl") self._active: bool = False @@ -151,6 +154,10 @@ class DynamicExperimentalController: self._set_mode_timeout = 0 + def _read_params(self) -> None: + if self._frame % int(1. / DT_MDL) == 0: + self._enabled = self._params.get_bool("DynamicExperimentalControl") + @staticmethod def _anomaly_detection(recent_data: list[float], threshold: float = 2.0, context_check: bool = True) -> bool: """ @@ -362,8 +369,8 @@ class DynamicExperimentalController: def active(self) -> bool: return self._active - def set_mpc_fcw_crash_cnt(self, crash_cnt: float) -> None: - self._mpc_fcw_crash_cnt = crash_cnt + def set_mpc_fcw_crash_cnt(self) -> None: + self._mpc_fcw_crash_cnt = self._mpc.crash_cnt def _set_mode(self, mode: str) -> None: if self._set_mode_timeout == 0: @@ -374,20 +381,17 @@ class DynamicExperimentalController: 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._enabled = self._params.get_bool("DynamicExperimentalControl") - - def update(self, radar_unavailable: bool, sm: messaging.SubMaster) -> None: + def update(self, sm: messaging.SubMaster) -> None: self._read_params() - if self._enabled: - self._update_calculations(sm) + self.set_mpc_fcw_crash_cnt() - if radar_unavailable: - self._radarless_mode() - else: - self._radar_mode() + self._update_calculations(sm) + + if self._CP.radarUnavailable: + self._radarless_mode() + else: + self._radar_mode() self._active = sm['selfdriveState'].experimentalMode and self._enabled diff --git a/sunnypilot/selfdrive/controls/lib/dec/helpers.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py similarity index 83% rename from sunnypilot/selfdrive/controls/lib/dec/helpers.py rename to sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 4eb7be0afa..56f32373d5 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/helpers.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -12,21 +12,18 @@ from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimen DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimentalControlState -class DecPlanner: +class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, mpc): - self.CP = CP - self.mpc = mpc - self.dec = DynamicExperimentalController() + self.dec = DynamicExperimentalController(CP, mpc) - def get_dec_mpc_mode(self) -> str | None: + def get_mpc_mode(self) -> str | None: if not self.dec.active(): return None return self.dec.mode() def update(self, sm: messaging.SubMaster) -> None: - self.dec.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt) - self.dec.update(self.CP.radarUnavailable, sm) + self.dec.update(sm) def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: plan_sp_send = messaging.new_message('longitudinalPlanSP') From 45f3c70596addcf29ffe7970157b41158e2b4448 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Tue, 14 Jan 2025 21:55:56 -0500 Subject: [PATCH 61/65] move around --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 931d86f09d..dff5cacdf7 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -158,6 +158,15 @@ class DynamicExperimentalController: if self._frame % int(1. / DT_MDL) == 0: self._enabled = self._params.get_bool("DynamicExperimentalControl") + def mode(self) -> str: + return str(self._mode) + + def enabled(self) -> bool: + return self._enabled + + def active(self) -> bool: + return self._active + @staticmethod def _anomaly_detection(recent_data: list[float], threshold: float = 2.0, context_check: bool = True) -> bool: """ @@ -360,15 +369,6 @@ class DynamicExperimentalController: self._set_mode('acc') - def mode(self) -> str: - return str(self._mode) - - def enabled(self) -> bool: - return self._enabled - - def active(self) -> bool: - return self._active - def set_mpc_fcw_crash_cnt(self) -> None: self._mpc_fcw_crash_cnt = self._mpc.crash_cnt From c205497b15511a83a20cbf4dd0d9d4cf219494dc Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 15 Jan 2025 00:13:28 -0500 Subject: [PATCH 62/65] explicit type hints --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index dff5cacdf7..04a6cfb3e3 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -98,9 +98,9 @@ class WeightedMovingAverageCalculator: self.data.pop(0) self.data.append(value) - def get_weighted_average(self) -> float | None: + def get_weighted_average(self) -> float: if len(self.data) == 0: - return None + return 0.0 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 @@ -227,7 +227,7 @@ class DynamicExperimentalController: # 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 + lead_prob = self._lead_gmac.get_weighted_average() self._has_lead_filtered = self._smoothed_lead_detection(lead_prob) # adaptive slow down detection @@ -271,7 +271,7 @@ class DynamicExperimentalController: 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 + self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_weighted_average() != 0.0 and self._dangerous_ttc_gmac.get_weighted_average() <= DANGEROUS_TTC # keep prev values self._has_standstill_prev = self._has_standstill From c6474fc3ad82753c73ec0bbd4f41f9c6c85c790e Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 15 Jan 2025 01:45:29 -0500 Subject: [PATCH 63/65] move to constants py --- .../selfdrive/controls/lib/dec/constants.py | 25 ++++++++ sunnypilot/selfdrive/controls/lib/dec/dec.py | 57 ++++++------------- 2 files changed, 42 insertions(+), 40 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/dec/constants.py diff --git a/sunnypilot/selfdrive/controls/lib/dec/constants.py b/sunnypilot/selfdrive/controls/lib/dec/constants.py new file mode 100644 index 0000000000..1922ab85db --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/dec/constants.py @@ -0,0 +1,25 @@ +class WMACConstants: + 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 + + MPC_FCW_WINDOW_SIZE = 10 + MPC_FCW_PROB = 0.5 + + +class SNG_State: + off = 0 + stopped = 1 + going = 2 diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 04a6cfb3e3..3c04978eb3 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -29,44 +29,20 @@ from opendbc.car import structs from openpilot.common.numpy_fast import interp from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants, SNG_State # 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 @@ -120,21 +96,21 @@ class DynamicExperimentalController: self._frame: int = 0 # Use weighted moving average for filtering leads - self._lead_gmac = WeightedMovingAverageCalculator(window_size=LEAD_WINDOW_SIZE) + self._lead_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.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._slow_down_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.SLOW_DOWN_WINDOW_SIZE) self._has_slow_down = False self._has_blinkers = False - self._slowness_gmac = WeightedMovingAverageCalculator(window_size=SLOWNESS_WINDOW_SIZE) + self._slowness_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.SLOWNESS_WINDOW_SIZE) self._has_slowness = False self._has_nav_instruction = False - self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=DANGEROUS_TTC_WINDOW_SIZE) + self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.DANGEROUS_TTC_WINDOW_SIZE) self._has_dangerous_ttc = False self._v_ego_kph = 0. @@ -148,7 +124,7 @@ class DynamicExperimentalController: self._sng_transit_frame = 0 self._sng_state = SNG_State.off - self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=MPC_FCW_WINDOW_SIZE) + self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.MPC_FCW_WINDOW_SIZE) self._has_mpc_fcw = False self._mpc_fcw_crash_cnt = 0 @@ -187,8 +163,9 @@ class DynamicExperimentalController: """ Adapts the slow-down threshold based on vehicle speed and recent behavior. """ + slowdown_scaling_factor: float = (1.0 + 0.05 * np.log(1 + len(self._slow_down_gmac.data))) 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))) + interp(self._v_ego_kph, WMACConstants.SLOW_DOWN_BP, WMACConstants.SLOW_DOWN_DIST) * slowdown_scaling_factor ) return adaptive_threshold @@ -197,15 +174,15 @@ class DynamicExperimentalController: Smoothing the lead detection to avoid erratic behavior. """ self._has_lead_filtered = (1 - smoothing_factor) * self._has_lead_filtered + smoothing_factor * lead_prob - return self._has_lead_filtered > LEAD_PROB + return self._has_lead_filtered > WMACConstants.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) + return float(WMACConstants.LEAD_PROB + 0.1) # Increase the threshold on highways + return float(WMACConstants.LEAD_PROB) def _update_calculations(self, sm: messaging.SubMaster) -> None: car_state = sm['carState'] @@ -219,14 +196,14 @@ class DynamicExperimentalController: # 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 + self._has_mpc_fcw = self._mpc_fcw_gmac.get_weighted_average() > WMACConstants.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 + #self._has_lead_filtered = self._lead_gmac.get_weighted_average() > WMACConstants.LEAD_PROB lead_prob = self._lead_gmac.get_weighted_average() self._has_lead_filtered = self._smoothed_lead_detection(lead_prob) @@ -234,7 +211,7 @@ class DynamicExperimentalController: 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 + self._has_slow_down = self._slow_down_gmac.get_weighted_average() > WMACConstants.SLOW_DOWN_PROB # anomaly detection for slow down events if self._anomaly_detection(self._slow_down_gmac.data): @@ -260,8 +237,8 @@ class DynamicExperimentalController: # 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 + self._slowness_gmac.add_data(self._v_ego_kph <= (self._v_cruise_kph * WMACConstants.SLOWNESS_CRUISE_OFFSET)) + self._has_slowness = self._slowness_gmac.get_weighted_average() > WMACConstants.SLOWNESS_PROB # dangerous TTC detection if not self._has_lead_filtered and self._has_lead_filtered_prev: @@ -271,7 +248,7 @@ class DynamicExperimentalController: 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() != 0.0 and self._dangerous_ttc_gmac.get_weighted_average() <= DANGEROUS_TTC + self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_weighted_average() != 0.0 and self._dangerous_ttc_gmac.get_weighted_average() <= WMACConstants.DANGEROUS_TTC # keep prev values self._has_standstill_prev = self._has_standstill From f94eeb9780bfd89f29f55fb2b0e58e41da3f9ba2 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 15 Jan 2025 01:46:31 -0500 Subject: [PATCH 64/65] Revert "explicit type hints" This reverts commit c205497b --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 3c04978eb3..13482d7ddb 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -74,9 +74,9 @@ class WeightedMovingAverageCalculator: self.data.pop(0) self.data.append(value) - def get_weighted_average(self) -> float: + def get_weighted_average(self) -> float | None: if len(self.data) == 0: - return 0.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 @@ -204,7 +204,7 @@ class DynamicExperimentalController: # lead detection with smoothing self._lead_gmac.add_data(lead_one.status) #self._has_lead_filtered = self._lead_gmac.get_weighted_average() > WMACConstants.LEAD_PROB - lead_prob = self._lead_gmac.get_weighted_average() + lead_prob = self._lead_gmac.get_weighted_average() or 0 self._has_lead_filtered = self._smoothed_lead_detection(lead_prob) # adaptive slow down detection @@ -248,7 +248,7 @@ class DynamicExperimentalController: 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() != 0.0 and self._dangerous_ttc_gmac.get_weighted_average() <= WMACConstants.DANGEROUS_TTC + self._has_dangerous_ttc = self._dangerous_ttc_gmac.get_weighted_average() is not None and self._dangerous_ttc_gmac.get_weighted_average() <= WMACConstants.DANGEROUS_TTC # keep prev values self._has_standstill_prev = self._has_standstill From d937062724269cbd1d69d98f86b3bb90f8182413 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Wed, 15 Jan 2025 01:51:22 -0500 Subject: [PATCH 65/65] more --- sunnypilot/selfdrive/controls/lib/dec/dec.py | 8 ++-- .../dec/tests/pytest_dynamic_controller.py | 42 ++++++++----------- 2 files changed, 21 insertions(+), 29 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/dec/dec.py b/sunnypilot/selfdrive/controls/lib/dec/dec.py index 13482d7ddb..f7bc028ee4 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/dec.py +++ b/sunnypilot/selfdrive/controls/lib/dec/dec.py @@ -101,17 +101,17 @@ class DynamicExperimentalController: self._has_lead_filtered_prev = False self._slow_down_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.SLOW_DOWN_WINDOW_SIZE) - self._has_slow_down = False + self._has_slow_down: bool = False self._has_blinkers = False self._slowness_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.SLOWNESS_WINDOW_SIZE) - self._has_slowness = False + self._has_slowness: bool = False self._has_nav_instruction = False self._dangerous_ttc_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.DANGEROUS_TTC_WINDOW_SIZE) - self._has_dangerous_ttc = False + self._has_dangerous_ttc: bool = False self._v_ego_kph = 0. self._v_cruise_kph = 0. @@ -125,7 +125,7 @@ class DynamicExperimentalController: self._sng_state = SNG_State.off self._mpc_fcw_gmac = WeightedMovingAverageCalculator(window_size=WMACConstants.MPC_FCW_WINDOW_SIZE) - self._has_mpc_fcw = False + self._has_mpc_fcw: bool = False self._mpc_fcw_crash_cnt = 0 self._set_mode_timeout = 0 diff --git a/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py b/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py index 6d6c98daf4..741d9e0170 100644 --- a/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py +++ b/sunnypilot/selfdrive/controls/lib/dec/tests/pytest_dynamic_controller.py @@ -1,18 +1,10 @@ -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 +from openpilot.sunnypilot.selfdrive.controls.lib.dec.constants import WMACConstants, SNG_State +from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController, TRAJECTORY_SIZE, STOP_AND_GO_FRAME + class MockInterp: def __call__(self, x, xp, fp): return np.interp(x, xp, fp) @@ -93,7 +85,7 @@ def test_lead_detection(controller, has_radar): controls_state = MockControlState(v_cruise=72) # Let moving average stabilize - for _ in range(LEAD_WINDOW_SIZE + 1): + for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_lead_filtered @@ -102,7 +94,7 @@ def test_lead_detection(controller, has_radar): # Test lead loss detection lead_one.status = False - for _ in range(LEAD_WINDOW_SIZE + 1): + for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert not controller._has_lead_filtered @@ -118,7 +110,7 @@ def test_slow_down_detection(controller, has_radar): controls_state = MockControlState(v_cruise=30) # Test slow down detection - for _ in range(SLOW_DOWN_WINDOW_SIZE + 1): + for _ in range(WMACConstants.SLOW_DOWN_WINDOW_SIZE + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_slow_down @@ -127,7 +119,7 @@ def test_slow_down_detection(controller, has_radar): # 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): + for _ in range(WMACConstants.SLOW_DOWN_WINDOW_SIZE + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert not controller._has_slow_down @@ -142,7 +134,7 @@ def test_dangerous_ttc_detection(controller, has_radar): # First establish normal conditions with lead lead_one.dRel = 100 # Safe distance - for _ in range(LEAD_WINDOW_SIZE + 1): # First establish lead detection + for _ in range(WMACConstants.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 @@ -152,7 +144,7 @@ def test_dangerous_ttc_detection(controller, has_radar): # 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): + for _ in range(WMACConstants.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" @@ -170,8 +162,8 @@ def test_mode_transitions(controller, has_radar): 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): + for _ in range(max(WMACConstants.LEAD_WINDOW_SIZE, WMACConstants.SLOW_DOWN_WINDOW_SIZE, + WMACConstants.DANGEROUS_TTC_WINDOW_SIZE, WMACConstants.MPC_FCW_WINDOW_SIZE) + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) # Test 1: Normal driving -> ACC mode @@ -197,7 +189,7 @@ def test_mode_transitions(controller, has_radar): lead_one.dRel = 50 # First establish normal lead detection # First establish lead detection - for _ in range(LEAD_WINDOW_SIZE + 1): + for _ in range(WMACConstants.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 @@ -205,7 +197,7 @@ def test_mode_transitions(controller, has_radar): # 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): + for _ in range(WMACConstants.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" @@ -222,7 +214,7 @@ def test_mpc_fcw_handling(controller, has_radar): # Test FCW activation controller.set_mpc_fcw_crash_cnt(5) - for _ in range(MPC_FCW_WINDOW_SIZE + 1): + for _ in range(WMACConstants.MPC_FCW_WINDOW_SIZE + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert controller._has_mpc_fcw @@ -230,7 +222,7 @@ def test_mpc_fcw_handling(controller, has_radar): # Test FCW recovery controller.set_mpc_fcw_crash_cnt(0) - for _ in range(MPC_FCW_WINDOW_SIZE + 1): + for _ in range(WMACConstants.MPC_FCW_WINDOW_SIZE + 1): controller.update(not has_radar, car_state, lead_one, md, controls_state) assert not controller._has_mpc_fcw @@ -243,12 +235,12 @@ def test_radar_unavailable_handling(controller): controls_state = MockControlState(v_cruise=100) # Test with radar available - for _ in range(LEAD_WINDOW_SIZE + 1): + for _ in range(WMACConstants.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): + for _ in range(WMACConstants.LEAD_WINDOW_SIZE + 1): controller.update(True, car_state, lead_one, md, controls_state) radarless_mode = controller.get_mpc_mode()