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 1/5] 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"), From 4bf4f627bd3b12598a861d5e29d5ec279a4747d7 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 1 Jul 2024 15:05:50 -0400 Subject: [PATCH 2/5] use cereal --- cereal/custom.capnp | 1 + selfdrive/controls/controlsd.py | 4 ++++ selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 6 ++++-- selfdrive/controls/lib/longitudinal_planner.py | 5 +---- selfdrive/ui/qt/offroad/settings.cc | 4 ++-- 5 files changed, 12 insertions(+), 8 deletions(-) diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 399679d03c..1e74981450 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -21,6 +21,7 @@ enum LongitudinalPersonalitySP { struct ControlsStateSP @0x81c2f05a394cf4af { lateralState @0 :Text; personality @8 :LongitudinalPersonalitySP; + dynamicPersonality @9 :Bool; lateralControlState :union { indiState @1 :LateralINDIState; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 21bf73cf9a..bf1875b000 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -187,6 +187,8 @@ class Controls: model_capabilities = ModelCapabilities.get_by_gen(self.model_gen) self.model_use_lateral_planner = self.custom_model and model_capabilities & ModelCapabilities.LateralPlannerSolution + self.dynamic_personality = self.params.get_bool("DynamicPersonality") + self.can_log_mono_time = 0 self.startup_event = get_startup_event(car_recognized, not self.CP.passive, len(self.CP.carFw) > 0) @@ -858,6 +860,7 @@ class Controls: controlsStateSP.lateralState = lat_tuning controlsStateSP.personality = self.personality + controlsStateSP.dynamicPersonality = self.dynamic_personality if self.enable_nnff and lat_tuning == 'torque': controlsStateSP.lateralControlState.torqueState = self.LaC.pid_long_sp @@ -911,6 +914,7 @@ class Controls: self.is_metric = self.params.get_bool("IsMetric") self.experimental_mode = self.params.get_bool("ExperimentalMode") and self.CP.openpilotLongitudinalControl self.personality = self.read_personality_param() + self.dynamic_personality = self.params.get_bool("DynamicPersonality") if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 97a69b2a2a..3a76350ccf 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -82,6 +82,7 @@ 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] @@ -96,7 +97,8 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP. 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") + raise NotImplementedError("Dynamic personality not supported") + return np.interp(v_ego, x_vel, y_dist) @@ -358,7 +360,7 @@ class LongitudinalMpc: 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) + t_follow = get_dynamic_personality(v_ego, personality) if dynamic_personality else get_T_FOLLOW(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 f91e71797c..8902b29d85 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -101,14 +101,11 @@ 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): @@ -181,7 +178,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, dynamic_personality=self.dynamic_personality) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality, dynamic_personality=sm['controlsState'].dynamicPersonality) 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 bdf4d9aa13..211646de47 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -58,7 +58,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { { "DynamicPersonality", tr("Enable Dynamic Personality"), - tr("Enable toggle to allow sunnypilot will dynamically adjust following distance based on your \"Driving Personality\" setting."), + tr("Enable this to allow sunnypilot to dynamically adjust following distance and reaction based on your \"Driving Personality\" setting."), "../assets/offroad/icon_blank.png", }, { @@ -116,7 +116,7 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { 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. " + tr("Standard is recommended. In aggressive/moderate 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 " "your steering wheel distance button."), "../assets/offroad/icon_blank.png", From 4de6fac0d462fc0598b560b399758f06a89bfff6 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 1 Jul 2024 15:35:27 -0400 Subject: [PATCH 3/5] in plannerd --- selfdrive/controls/lib/longitudinal_planner.py | 2 +- selfdrive/controls/plannerd.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 8902b29d85..4f948f195c 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -178,7 +178,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, dynamic_personality=sm['controlsState'].dynamicPersonality) + self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality, dynamic_personality=sm['controlsStateSP'].dynamicPersonality) 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/controls/plannerd.py b/selfdrive/controls/plannerd.py index 11192b8791..640251f163 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -35,7 +35,7 @@ def plannerd_thread(): pm = messaging.PubMaster(['longitudinalPlan', 'longitudinalPlanSP'] + lateral_planner_svs) sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'radarState', 'modelV2', 'longitudinalPlan', 'navInstruction', 'longitudinalPlanSP', - 'liveMapDataSP', 'e2eLongStateSP'] + lateral_planner_svs, + 'liveMapDataSP', 'e2eLongStateSP', 'controlsStateSP'] + lateral_planner_svs, poll='modelV2', ignore_avg_freq=['radarState']) while True: From 36c89ce66698cc9d74ee95fdfa3bd8c18885d488 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 1 Jul 2024 15:47:18 -0400 Subject: [PATCH 4/5] Fix --- system/manager/manager.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/manager/manager.py b/system/manager/manager.py index 2b0920fd8e..6fb415fe5c 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -64,7 +64,7 @@ def manager_init() -> None: ("DisengageLateralOnBrake", "0"), ("DrivingModelGeneration", "0"), ("DynamicLaneProfile", "1"), - ("DynamicFollow", "0"), + ("DynamicPersonality", "0"), ("EnableMads", "1"), ("EnhancedScc", "0"), ("FeatureStatus", "1"), From 9ee63438ef310e2f1ddbbf9aaf86b3a390e2c532 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 1 Jul 2024 16:23:06 -0400 Subject: [PATCH 5/5] update tuning --- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 3a76350ccf..a2df527a55 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.75 + return 0.8 elif personality==custom.LongitudinalPersonalitySP.aggressive: - return 0.5 + return 0.6 else: raise NotImplementedError("Longitudinal personality not supported") @@ -95,7 +95,7 @@ def get_dynamic_personality(v_ego, personality=custom.LongitudinalPersonalitySP. 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] + y_dist = [1.12, 1.12, 1.12, 1.12, 1.12, 1.105, 1.105, 1.15, 1.15, 1.18, 1.20, 1.23] else: raise NotImplementedError("Dynamic personality not supported")