brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Custom driving personality profiles

Browse Source 
Added toggles to customize the driving personality profiles.
This commit is contained in:
FrogAi
2024-01-12 22:39:30 -07:00
parent ccf674762e
commit fb8103b646
7 changed files with 109 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
common/params.cc
View File

@@ -214,6 +214,8 @@ std::unordered_map<std::string, uint32_t> 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<std::string, uint32_t> 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},

32
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@@ -56,7 +56,17 @@ T_DIFFS = np.diff(T_IDXS, prepend=[0.])
COMFORT_BRAKE = 2.5
STOP_DISTANCE = 6.0
def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
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:
if personality==log.LongitudinalPersonality.relaxed:
return 1.0
elif personality==log.LongitudinalPersonality.standard:
@@ -67,7 +77,17 @@ def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
raise NotImplementedError("Longitudinal personality not supported")
def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
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:
if personality==log.LongitudinalPersonality.relaxed:
return 1.75
elif personality==log.LongitudinalPersonality.standard:
@@ -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

5
selfdrive/controls/lib/longitudinal_planner.py
View File

@@ -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)

BIN
selfdrive/frogpilot/assets/toggle_icons/icon_custom.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 13 KiB

8
selfdrive/frogpilot/functions/frogpilot_planner.py
View File

@@ -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

48
selfdrive/frogpilot/ui/control_settings.cc
View File

@@ -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<QString> 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<int, QString>(), 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<int, QString>(), 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<int, QString>(), 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<int, QString>(), 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<int, QString>(), 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<int, QString>(), 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() ||

3
selfdrive/frogpilot/ui/control_settings.h
View File

@@ -22,8 +22,11 @@ private:
void updateMetric();
void updateToggles();
FrogPilotDualParamControl *aggressiveProfile;
FrogPilotDualParamControl *conditionalSpeedsImperial;
FrogPilotDualParamControl *conditionalSpeedsMetric;
FrogPilotDualParamControl *standardProfile;
FrogPilotDualParamControl *relaxedProfile;
std::set<QString> conditionalExperimentalKeys;
std::set<QString> fireTheBabysitterKeys;