diff --git a/common/params.cc b/common/params.cc index c98a24c..1fb1df7 100644 --- a/common/params.cc +++ b/common/params.cc @@ -214,6 +214,8 @@ std::unordered_map keys = { // FrogPilot parameters {"AccelerationProfile", PERSISTENT}, {"AggressiveAcceleration", PERSISTENT}, + {"AggressiveFollow", PERSISTENT}, + {"AggressiveJerk", PERSISTENT}, {"AlwaysOnLateral", PERSISTENT}, {"AlwaysOnLateralMain", PERSISTENT}, {"ApiCache_DriveStats", PERSISTENT}, @@ -230,11 +232,16 @@ std::unordered_map keys = { {"CEStopLights", PERSISTENT}, {"CEStopLightsLead", PERSISTENT}, {"ConditionalExperimental", PERSISTENT}, + {"CustomPersonalities", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"GasRegenCmd", PERSISTENT}, {"LateralTune", PERSISTENT}, {"LongitudinalTune", PERSISTENT}, {"OfflineMode", PERSISTENT}, + {"RelaxedFollow", PERSISTENT}, + {"RelaxedJerk", PERSISTENT}, + {"StandardFollow", PERSISTENT}, + {"StandardJerk", PERSISTENT}, {"Updated", PERSISTENT}, {"UpdateSchedule", PERSISTENT}, {"UpdateTime", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 25e15dd..4a3fc29 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -56,26 +56,46 @@ 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 - elif personality==log.LongitudinalPersonality.standard: - return 1.0 - elif personality==log.LongitudinalPersonality.aggressive: - return 0.5 +def get_jerk_factor(custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, personality=log.LongitudinalPersonality.standard): + if custom_personalities: + if personality==log.LongitudinalPersonality.relaxed: + return relaxed_jerk + elif personality==log.LongitudinalPersonality.standard: + return standard_jerk + elif personality==log.LongitudinalPersonality.aggressive: + return aggressive_jerk + else: + raise NotImplementedError("Longitudinal personality not supported") else: - raise NotImplementedError("Longitudinal personality not supported") + if personality==log.LongitudinalPersonality.relaxed: + return 1.0 + elif personality==log.LongitudinalPersonality.standard: + return 1.0 + elif personality==log.LongitudinalPersonality.aggressive: + return 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) @@ -272,8 +292,8 @@ class LongitudinalMpc: for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): - jerk_factor = get_jerk_factor(personality) + def set_weights(self, custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): + jerk_factor = get_jerk_factor(custom_personalities, aggressive_jerk, standard_jerk, relaxed_jerk, personality) if self.mode == 'acc': a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] @@ -331,8 +351,8 @@ class LongitudinalMpc: self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, personality=log.LongitudinalPersonality.standard): - t_follow = get_T_FOLLOW(personality) + def update(self, radarstate, v_cruise, x, v, a, j, aggressive_acceleration, custom_personalities, aggressive_follow, standard_follow, relaxed_follow, personality=log.LongitudinalPersonality.standard): + t_follow = get_T_FOLLOW(custom_personalities, aggressive_follow, standard_follow, relaxed_follow, personality) self.t_follow = t_follow v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 6d2e594..cda535d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -135,11 +135,12 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.set_weights(prev_accel_constraint, personality=self.personality) + self.mpc.set_weights(frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_jerk, frogpilot_planner.standard_jerk, frogpilot_planner.relaxed_jerk, prev_accel_constraint, personality=self.personality) 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, frogpilot_planner.aggressive_acceleration, personality=self.personality) + self.mpc.update(sm['radarState'], frogpilot_planner.v_cruise, x, v, a, j, frogpilot_planner.aggressive_acceleration, + frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_follow, frogpilot_planner.standard_follow, frogpilot_planner.relaxed_follow, personality=self.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/frogpilot/assets/toggle_icons/icon_custom.png b/selfdrive/frogpilot/assets/toggle_icons/icon_custom.png new file mode 100644 index 0000000..f1c7f4d Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_custom.png differ diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index d7f77eb..7c7b14d 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -101,6 +101,14 @@ class FrogPilotPlanner: if not params.get_bool("ExperimentalMode"): params.put_bool("ExperimentalMode", True) + self.custom_personalities = params.get_bool("CustomPersonalities") + self.aggressive_follow = params.get_int("AggressiveFollow") / 10 + self.standard_follow = params.get_int("StandardFollow") / 10 + self.relaxed_follow = params.get_int("RelaxedFollow") / 10 + self.aggressive_jerk = params.get_int("AggressiveJerk") / 10 + self.standard_jerk = params.get_int("StandardJerk") / 10 + self.relaxed_jerk = params.get_int("RelaxedJerk") / 10 + lateral_tune = params.get_bool("LateralTune") self.average_desired_curvature = params.get_bool("AverageCurvature") and lateral_tune diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 740e319..3594d77 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -12,6 +12,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"CEStopLights", "Stop Lights and Stop Signs", "Switch to 'Experimental Mode' when a stop light or stop sign is detected.", ""}, {"CESignal", "Turn Signal When Below Highway Speeds", "Switch to 'Experimental Mode' when using turn signals below highway speeds to help assit with turns.", ""}, + {"CustomPersonalities", "Custom Driving Personalities", "Customize the driving personality profiles to your driving style.", "../frogpilot/assets/toggle_icons/icon_custom.png"}, + {"LateralTune", "Lateral Tuning", "Modify openpilot's steering behavior.", "../frogpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"AverageCurvature", "Average Desired Curvature", "Use Pfeiferj's distance-based curvature adjustment for improved curve handling.", ""}, @@ -72,6 +74,46 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil std::vector stopLightToggleNames{tr("With Lead")}; toggle = new FrogPilotParamToggleControl(param, title, desc, icon, stopLightToggles, stopLightToggleNames); + } else if (param == "CustomPersonalities") { + FrogPilotParamManageControl *customPersonalitiesToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(customPersonalitiesToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(false); + } + aggressiveProfile->setVisible(true); + standardProfile->setVisible(true); + relaxedProfile->setVisible(true); + }); + toggle = customPersonalitiesToggle; + + FrogPilotParamValueControl *aggressiveFollow = new FrogPilotParamValueControl("AggressiveFollow", "Follow", + "Set the 'Aggressive' personality' following distance. Represents seconds to follow behind the lead vehicle.\n\nStock: 1.25 seconds.", "../frogpilot/assets/other_images/aggressive.png", + 10, 50, std::map(), this, false, " sec", 10); + FrogPilotParamValueControl *aggressiveJerk = new FrogPilotParamValueControl("AggressiveJerk", " Jerk", + "Configure brake/gas pedal responsiveness for the 'Aggressive' personality. Higher values yield a more 'relaxed' response.\n\nStock: 0.5.", "", 1, 50, + std::map(), this, false, "", 10); + aggressiveProfile = new FrogPilotDualParamControl(aggressiveFollow, aggressiveJerk, this, true); + addItem(aggressiveProfile); + + FrogPilotParamValueControl *standardFollow = new FrogPilotParamValueControl("StandardFollow", "Follow", + "Set the 'Standard' personality following distance. Represents seconds to follow behind the lead vehicle.\n\nStock: 1.45 seconds.", "../frogpilot/assets/other_images/standard.png", + 10, 50, std::map(), this, false, " sec", 10); + FrogPilotParamValueControl *standardJerk = new FrogPilotParamValueControl("StandardJerk", " Jerk", + "Adjust brake/gas pedal responsiveness for the 'Standard' personality. Higher values yield a more 'relaxed' response.\n\nStock: 1.0.", "", 1, 50, + std::map(), this, false, "", 10); + standardProfile = new FrogPilotDualParamControl(standardFollow, standardJerk, this, true); + addItem(standardProfile); + + FrogPilotParamValueControl *relaxedFollow = new FrogPilotParamValueControl("RelaxedFollow", "Follow", + "Set the 'Relaxed' personality following distance. Represents seconds to follow behind the lead vehicle.\n\nStock: 1.75 seconds.", "../frogpilot/assets/other_images/relaxed.png", + 10, 50, std::map(), this, false, " sec", 10); + FrogPilotParamValueControl *relaxedJerk = new FrogPilotParamValueControl("RelaxedJerk", " Jerk", + "Set brake/gas pedal responsiveness for the 'Relaxed' personality. Higher values yield a more 'relaxed' response.\n\nStock: 1.0.", "", 1, 50, + std::map(), this, false, "", 10); + relaxedProfile = new FrogPilotDualParamControl(relaxedFollow, relaxedJerk, this, true); + addItem(relaxedProfile); + } else if (param == "LateralTune") { FrogPilotParamManageControl *lateralTuneToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); QObject::connect(lateralTuneToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { @@ -179,15 +221,21 @@ void FrogPilotControlsPanel::updateMetric() { } void FrogPilotControlsPanel::parentToggleClicked() { + aggressiveProfile->setVisible(false); conditionalSpeedsImperial->setVisible(false); conditionalSpeedsMetric->setVisible(false); + standardProfile->setVisible(false); + relaxedProfile->setVisible(false); this->openParentToggle(); } void FrogPilotControlsPanel::hideSubToggles() { + aggressiveProfile->setVisible(false); conditionalSpeedsImperial->setVisible(false); conditionalSpeedsMetric->setVisible(false); + standardProfile->setVisible(false); + relaxedProfile->setVisible(false); for (auto &[key, toggle] : toggles) { bool subToggles = conditionalExperimentalKeys.find(key.c_str()) != conditionalExperimentalKeys.end() || diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 629772a..473c94b 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -22,8 +22,11 @@ private: void updateMetric(); void updateToggles(); + FrogPilotDualParamControl *aggressiveProfile; FrogPilotDualParamControl *conditionalSpeedsImperial; FrogPilotDualParamControl *conditionalSpeedsMetric; + FrogPilotDualParamControl *standardProfile; + FrogPilotDualParamControl *relaxedProfile; std::set conditionalExperimentalKeys; std::set fireTheBabysitterKeys;