From 386f32ef6e62f03f306982a4c3612440ab36c1dc Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Thu, 16 May 2024 02:59:37 -0700 Subject: [PATCH] Controls - Driving Personalities - Customize Personalities Customize the driving personality profiles to your driving style. --- .../lib/longitudinal_mpc_lib/long_mpc.py | 53 +++++++++++++------ .../frogpilot/controls/frogpilot_planner.py | 9 +++- .../ui/qt/offroad/personalities_info.txt | 5 ++ 3 files changed, 49 insertions(+), 18 deletions(-) mode change 100755 => 100644 selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py create mode 100644 selfdrive/frogpilot/ui/qt/offroad/personalities_info.txt diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py old mode 100755 new mode 100644 index 3be6268..4065eaf --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -58,26 +58,47 @@ T_DIFFS = np.diff(T_IDXS, prepend=[0.]) COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 -def get_jerk_factor(personality=log.LongitudinalPersonality.standard): - if personality==log.LongitudinalPersonality.relaxed: - return 1.0, 1.0 - elif personality==log.LongitudinalPersonality.standard: - return 1.0, 1.0 - elif personality==log.LongitudinalPersonality.aggressive: - return 0.5, 0.5 +def get_jerk_factor(custom_personalities=False, aggressive_jerk_acceleration=0.5, aggressive_jerk_speed=0.5, standard_jerk_acceleration=1.0, standard_jerk_speed=1.0, + relaxed_jerk_acceleration=1.0, relaxed_jerk_speed=1.0, personality=log.LongitudinalPersonality.standard): + if custom_personalities: + if personality==log.LongitudinalPersonality.relaxed: + return relaxed_jerk_acceleration, relaxed_jerk_speed + elif personality==log.LongitudinalPersonality.standard: + return standard_jerk_acceleration, standard_jerk_speed + elif personality==log.LongitudinalPersonality.aggressive: + return aggressive_jerk_acceleration, aggressive_jerk_speed + else: + raise NotImplementedError("Longitudinal personality not supported") else: - raise NotImplementedError("Longitudinal personality not supported") + if personality==log.LongitudinalPersonality.relaxed: + return 1.0, 1.0 + elif personality==log.LongitudinalPersonality.standard: + return 1.0, 1.0 + elif personality==log.LongitudinalPersonality.aggressive: + return 0.5, 0.5 + else: + raise NotImplementedError("Longitudinal personality not supported") -def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard): - if personality==log.LongitudinalPersonality.relaxed: - return 1.75 - elif personality==log.LongitudinalPersonality.standard: - return 1.45 - elif personality==log.LongitudinalPersonality.aggressive: - return 1.25 +def get_T_FOLLOW(custom_personalities=False, aggressive_follow=1.25, standard_follow=1.45, relaxed_follow=1.75, personality=log.LongitudinalPersonality.standard): + if custom_personalities: + if personality==log.LongitudinalPersonality.relaxed: + return relaxed_follow + elif personality==log.LongitudinalPersonality.standard: + return standard_follow + elif personality==log.LongitudinalPersonality.aggressive: + return aggressive_follow + else: + raise NotImplementedError("Longitudinal personality not supported") else: - raise NotImplementedError("Longitudinal personality not supported") + if personality==log.LongitudinalPersonality.relaxed: + return 1.75 + elif personality==log.LongitudinalPersonality.standard: + return 1.45 + elif personality==log.LongitudinalPersonality.aggressive: + return 1.25 + else: + raise NotImplementedError("Longitudinal personality not supported") def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index a7be82f..f4f6bc8 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -44,8 +44,13 @@ class FrogPilotPlanner: lead_distance = self.lead_one.dRel - self.base_acceleration_jerk, self.base_speed_jerk = get_jerk_factor(controlsState.personality) - self.t_follow = get_T_FOLLOW(controlsState.personality) + self.base_acceleration_jerk, self.base_speed_jerk = get_jerk_factor(frogpilot_toggles.custom_personalities, + frogpilot_toggles.aggressive_jerk_acceleration, frogpilot_toggles.aggressive_jerk_speed, + frogpilot_toggles.standard_jerk_acceleration, frogpilot_toggles.standard_jerk_speed, + frogpilot_toggles.relaxed_jerk_acceleration, frogpilot_toggles.relaxed_jerk_speed, + controlsState.personality) + self.t_follow = get_T_FOLLOW(frogpilot_toggles.custom_personalities, frogpilot_toggles.aggressive_follow, + frogpilot_toggles.standard_follow, frogpilot_toggles.relaxed_follow, controlsState.personality) if self.lead_one.status: self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles) diff --git a/selfdrive/frogpilot/ui/qt/offroad/personalities_info.txt b/selfdrive/frogpilot/ui/qt/offroad/personalities_info.txt new file mode 100644 index 0000000..b510593 --- /dev/null +++ b/selfdrive/frogpilot/ui/qt/offroad/personalities_info.txt @@ -0,0 +1,5 @@ +Following Distance - Represents openpilot's desired distance from the lead in seconds. Not an exact value in real world terms, but more of a representative value. + +Acceleration/Deceleration Response - Increases/decreases the "cost" to accelerate. Increasing the value makes it more "costly" to accelerate/decelerate, so openpilot is more conservative when accelerating/decelerating. + +Speed Control Response - Increases/decreases the "cost" to increase/decrease your speed. Increasing the value makes it more "costly" to change speed, so openpilot is more conservative when using the gas/brake pedal. \ No newline at end of file