From f809fcf0aa0c73f0ab5907c261a9934dbc42e412 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sun, 9 Jun 2024 11:47:22 -0700 Subject: [PATCH] dynamic personality --- common/params.cc | 1 + .../lib/longitudinal_mpc_lib/long_mpc.py | 26 ++++++++++++++++--- .../controls/lib/longitudinal_planner.py | 5 +++- selfdrive/ui/qt/offroad/settings.cc | 8 +++++- selfdrive/ui/ui.h | 2 +- system/manager/manager.py | 1 + 6 files changed, 36 insertions(+), 7 deletions(-) diff --git a/common/params.cc b/common/params.cc index 73906d35f4..552153fae1 100644 --- a/common/params.cc +++ b/common/params.cc @@ -243,6 +243,7 @@ std::unordered_map keys = { {"DrivingModelUrl", PERSISTENT}, {"DynamicExperimentalControl", PERSISTENT | BACKUP}, {"DynamicLaneProfile", PERSISTENT | BACKUP}, + {"DynamicPersonality", PERSISTENT | BACKUP}, {"EnableAmap", PERSISTENT | BACKUP}, {"EnableGmap", PERSISTENT | BACKUP}, {"EnableMads", PERSISTENT | BACKUP}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index cba398bcca..97a69b2a2a 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -63,9 +63,9 @@ def get_jerk_factor(personality=custom.LongitudinalPersonalitySP.standard): elif personality==custom.LongitudinalPersonalitySP.standard: return 1.0 elif personality==custom.LongitudinalPersonalitySP.moderate: - return 0.5 + return 0.75 elif personality==custom.LongitudinalPersonalitySP.aggressive: - return 0.222 + return 0.5 else: raise NotImplementedError("Longitudinal personality not supported") @@ -82,6 +82,24 @@ def get_T_FOLLOW(personality=custom.LongitudinalPersonalitySP.standard): else: raise NotImplementedError("Longitudinal personality not supported") +def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP.standard): + if personality==custom.LongitudinalPersonalitySP.relaxed: + x_vel = [0, 11, 14.5, 15, 20, 20.01, 25, 25.01, 36, 36.01] + y_dist = [1.5, 1.5, 1.5, 1.6, 1.76, 1.76, 1.78, 1.78, 1.8, 1.8] + elif personality==custom.LongitudinalPersonalitySP.standard: + x_vel = [0, 11, 14.5, 15, 20, 20.01, 25, 25.01, 36, 36.01] + y_dist = [1.40, 1.40, 1.40, 1.50, 1.60, 1.76, 1.76, 1.78, 1.8, 1.8] + elif personality==custom.LongitudinalPersonalitySP.moderate: + x_vel = [0, 11, 14.5, 15, 20, 20.01, 25, 25.01, 36, 36.01] + y_dist = [1.3, 1.3, 1.3, 1.35, 1.35, 1.385, 1.385, 1.4, 1.4, 1.45] + elif personality==custom.LongitudinalPersonalitySP.aggressive: + x_vel = [0, 5, 5.01, 11, 14.5, 15, 20, 20.01, 25, 25.01, 36, 36.01] + y_dist = [1.25, 1.25, 1.12, 1.12, 1.12, 1.105, 1.105, 1.15, 1.15, 1.18, 1.20, 1.23] + else: + raise NotImplementedError("Dynamic Follow personality not supported") + return np.interp(v_ego, x_vel, y_dist) + + def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) @@ -338,9 +356,9 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard): - t_follow = get_T_FOLLOW(personality) + def update(self, radarstate, v_cruise, x, v, a, j, personality=custom.LongitudinalPersonalitySP.standard, dynamic_personality=False): v_ego = self.x0[1] + t_follow = get_T_FOLLOW(personality) if not dynamic_personality else get_dynamic_personality(v_ego, personality) self.status = radarstate.leadOne.status or radarstate.leadTwo.status lead_xv_0 = self.process_lead(radarstate.leadOne) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 9f508fb76b..f91e71797c 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -101,11 +101,14 @@ class LongitudinalPlanner: self.turn_speed_controller = TurnSpeedController() self.dynamic_experimental_controller = DynamicExperimentalController() + self.dynamic_personality = False + def read_param(self): try: self.dynamic_experimental_controller.set_enabled(self.params.get_bool("DynamicExperimentalControl")) except AttributeError: self.dynamic_experimental_controller = DynamicExperimentalController() + self.dynamic_personality = self.params.get_bool("DynamicPersonality") @staticmethod def parse_model(model_msg, model_error): @@ -178,7 +181,7 @@ class LongitudinalPlanner: self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error) - self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality, dynamic_personality=self.dynamic_personality) self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 4c9a2e9085..bdf4d9aa13 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -55,6 +55,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { tr("Enable toggle to allow the model to determine when to use openpilot ACC or openpilot End to End Longitudinal."), "../assets/offroad/icon_blank.png", }, + { + "DynamicPersonality", + tr("Enable Dynamic Personality"), + tr("Enable toggle to allow sunnypilot will dynamically adjust following distance based on your \"Driving Personality\" setting."), + "../assets/offroad/icon_blank.png", + }, { "DisengageOnAccelerator", tr("Disengage on Accelerator Pedal"), @@ -108,7 +114,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { }; - std::vector longi_button_texts{tr("Maniac"), tr("Aggressive"), tr("Stock"), tr("Relaxed")}; + std::vector longi_button_texts{tr("Aggressive"), tr("Moderate"), tr("Standard"), tr("Relaxed")}; long_personality_setting = new ButtonParamControl("LongitudinalPersonality", tr("Driving Personality"), tr("Stock is recommended. In aggressive/maniac mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. " "In relaxed mode openpilot will stay further away from lead cars. On supported cars, you can cycle through these personalities with " diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index ed9996f54d..a43887a100 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -27,7 +27,7 @@ const int UI_ROAD_NAME_MARGIN_X = 14; struct FeatureStatusText { const QStringList dlp_list_text = { "Laneful", "Laneless", "Auto" }; - const QStringList gac_list_text = { "Maniac", "Aggressive", "Standard", "Relaxed" }; + const QStringList gac_list_text = { "Aggressive", "Moderate", "Standard", "Relaxed" }; const QStringList slc_list_text = { "Inactive", "Temp Off", "Adapting", "Active", "Pre Active" }; }; diff --git a/system/manager/manager.py b/system/manager/manager.py index f3a5b1b366..1b8812a619 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -64,6 +64,7 @@ def manager_init() -> None: ("DisengageLateralOnBrake", "0"), ("DrivingModelGeneration", "0"), ("DynamicLaneProfile", "1"), + ("DynamicFollow", "0"), ("EnableMads", "1"), ("EnhancedScc", "0"), ("FeatureStatus", "1"),