From 95bdb9026f8a0ef188f7f34dfb70d61f6e58d77e Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sun, 12 Jan 2025 15:34:56 +0100 Subject: [PATCH] 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 ++----------------- .../dynamic_experimental_controller.py | 13 +++-- sunnypilot/selfdrive/controls/dec/helpers.py | 42 ++++++++++++++++ .../tests/pytest_dynamic_controller.py | 7 ++- 4 files changed, 60 insertions(+), 50 deletions(-) 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/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):