Traffic Mode

Added toggle for "Traffic Mode" that enables more aggressive behavior from openpilot while in stop and go traffic.
This commit is contained in:
FrogAi
2024-04-08 15:24:52 -07:00
parent d74145f345
commit ee4f54cd8e
14 changed files with 90 additions and 16 deletions
+1
View File
@@ -11,6 +11,7 @@ $Cxx.namespace("cereal");
struct FrogPilotCarControl @0x81c2f05a394cf4af {
alwaysOnLateral @0 :Bool;
speedLimitChanged @1 :Bool;
trafficModeActive @2 :Bool;
}
struct FrogPilotCarState @0xaedffd8f31e7b55d {
+4
View File
@@ -429,6 +429,10 @@ std::unordered_map<std::string, uint32_t> keys = {
{"TacoTune", PERSISTENT},
{"TetheringEnabled", PERSISTENT},
{"ToyotaDoors", PERSISTENT},
{"TrafficFollow", PERSISTENT},
{"TrafficJerk", PERSISTENT},
{"TrafficMode", PERSISTENT},
{"TrafficModeActive", CLEAR_ON_OFFROAD_TRANSITION},
{"UnlimitedLength", PERSISTENT},
{"UnlockDoors", PERSISTENT},
{"Updated", PERSISTENT},
+8 -1
View File
@@ -221,6 +221,7 @@ class CarInterfaceBase(ABC):
self.disable_resumeRequired = False
self.prev_distance_button = False
self.resumeRequired_shown = False
self.traffic_mode_changed = False
self.gap_counter = 0
@@ -492,7 +493,7 @@ class CarInterfaceBase(ABC):
elif not prev_distance_button:
self.gap_counter = 0
if self.gap_counter == CRUISE_LONG_PRESS * 1.5 and frogpilot_variables.experimental_mode_via_distance:
if self.gap_counter == CRUISE_LONG_PRESS * 1.5 and frogpilot_variables.experimental_mode_via_distance or self.traffic_mode_changed:
if frogpilot_variables.conditional_experimental_mode:
conditional_status = self.params_memory.get_int("CEStatus")
override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 1 if conditional_status >= 7 else 2
@@ -500,6 +501,12 @@ class CarInterfaceBase(ABC):
else:
experimental_mode = self.params.get_bool("ExperimentalMode")
self.params.put_bool("ExperimentalMode", not experimental_mode)
self.traffic_mode_changed = False
if self.gap_counter == CRUISE_LONG_PRESS * 5 and frogpilot_variables.traffic_mode:
traffic_mode = self.params_memory.get_bool("TrafficModeActive")
self.params_memory.put_bool("TrafficModeActive", not traffic_mode)
self.traffic_mode_changed = frogpilot_variables.experimental_mode_via_distance
return self.gap_counter >= CRUISE_LONG_PRESS
+3
View File
@@ -1128,6 +1128,8 @@ class Controls:
signal_check = CS.vEgo >= self.pause_lateral_below_speed or not (CS.leftBlinker or CS.rightBlinker) or CS.standstill
self.speed_check = CS.vEgo >= self.pause_lateral_below_speed or CS.standstill or signal_check and self.pause_lateral_below_signal
self.FPCC.trafficModeActive = self.frogpilot_variables.traffic_mode and self.params_memory.get_bool("TrafficModeActive")
fpcc_send = messaging.new_message('frogpilotCarControl')
fpcc_send.valid = CS.canValid
fpcc_send.frogpilotCarControl = self.FPCC
@@ -1170,6 +1172,7 @@ class Controls:
longitudinal_tune = self.CP.openpilotLongitudinalControl and self.params.get_bool("LongitudinalTune")
self.frogpilot_variables.sport_plus = longitudinal_tune and self.params.get_int("AccelerationProfile") == 3
self.frogpilot_variables.traffic_mode = longitudinal_tune and self.params.get_bool("TrafficMode")
quality_of_life = self.params.get_bool("QOLControls")
self.frogpilot_variables.custom_cruise_increase = self.params.get_int("CustomCruise") if quality_of_life else 1
@@ -358,11 +358,12 @@ class LongitudinalMpc:
self.cruise_min_a = min_a
self.max_a = max_a
def update(self, lead_one, lead_two, v_cruise, x, v, a, j, radarless_model, t_follow, personality=log.LongitudinalPersonality.standard):
def update(self, lead_one, lead_two, v_cruise, x, v, a, j, radarless_model, t_follow, trafficModeActive, personality=log.LongitudinalPersonality.standard):
v_ego = self.x0[1]
self.status = lead_one.status or lead_two.status
lead_xv_0 = self.process_lead(lead_one, self.increased_stopping_distance)
increase_distance = max(self.increased_stopping_distance - v_ego if not trafficModeActive else 0, 0)
lead_xv_0 = self.process_lead(lead_one, increase_distance)
lead_xv_1 = self.process_lead(lead_two)
# To estimate a safe distance from a moving lead, we calculate how much stopping
Binary file not shown.

After

Width:  |  Height:  |  Size: 57 KiB

Binary file not shown.

After

Width:  |  Height:  |  Size: 12 KiB

@@ -35,6 +35,8 @@ A_CRUISE_MAX_VALS_ECO = [3.5, 3.2, 2.3, 2.0, 1.15, .80, .58, .36, .30, .091]
A_CRUISE_MIN_VALS_SPORT = [-0.50, -0.52, -0.55, -0.57, -0.60]
A_CRUISE_MAX_VALS_SPORT = [3.5, 3.5, 3.3, 2.8, 1.5, 1.0, .75, .6, .38, .2]
TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
def get_min_accel_eco(v_ego):
return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_ECO)
@@ -112,7 +114,7 @@ class FrogPilotPlanner:
self.safe_obstacle_distance = int(np.mean(get_safe_obstacle_distance(v_ego, self.t_follow)))
self.safe_obstacle_distance_stock = int(np.mean(get_safe_obstacle_distance(v_ego, base_t_follow)))
self.stopped_equivalence_factor = int(np.mean(get_stopped_equivalence_factor(v_lead)))
self.jerk, self.t_follow = self.update_follow_values(base_jerk, self.lead_one, base_t_follow, v_ego, v_lead)
self.jerk, self.t_follow = self.update_follow_values(base_jerk, self.lead_one, base_t_follow, frogpilotCarControl.trafficModeActive, v_ego, v_lead)
else:
self.safe_obstacle_distance = 0
self.safe_obstacle_distance_stock = 0
@@ -134,8 +136,12 @@ class FrogPilotPlanner:
else:
self.lead_one = radarState.leadOne
def update_follow_values(self, jerk, lead_one, t_follow, v_ego, v_lead):
stopping_distance = STOP_DISTANCE + max(self.increased_stopping_distance - v_ego, 0)
def update_follow_values(self, jerk, lead_one, t_follow, trafficModeActive, v_ego, v_lead):
if trafficModeActive:
jerk = interp(v_ego, TRAFFIC_MODE_BP, self.traffic_mode_jerk)
t_follow = interp(v_ego, TRAFFIC_MODE_BP, self.traffic_mode_t_follow)
stopping_distance = STOP_DISTANCE + max(self.increased_stopping_distance - v_ego if not trafficModeActive else 0, 0)
lead_distance = self.lead_one.dRel + stopping_distance
# Offset by FrogAi for FrogPilot for a more natural takeoff with a lead
@@ -285,6 +291,10 @@ class FrogPilotPlanner:
self.standard_follow = self.params.get_float("StandardFollow")
self.relaxed_jerk = self.params.get_float("RelaxedJerk")
self.relaxed_follow = self.params.get_float("RelaxedFollow")
self.traffic_jerk = self.params.get_float("TrafficJerk")
self.traffic_follow = self.params.get_float("TrafficFollow")
self.traffic_mode_jerk = [self.traffic_jerk, self.aggressive_jerk] if self.custom_personalities and not self.release else [1.0, 0.5]
self.traffic_mode_t_follow = [self.traffic_follow, self.aggressive_follow] if self.custom_personalities and not self.release else [0.5, 1.0]
custom_ui = self.params.get_bool("CustomUI")
self.adjacent_lanes = custom_ui and self.params.get_bool("AdjacentPath")
@@ -88,6 +88,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
{"AggressiveAcceleration", tr("Increase Acceleration Behind Faster Lead"), tr("Increase aggressiveness when following a faster lead."), ""},
{"StoppingDistance", tr("Increase Stop Distance Behind Lead"), tr("Increase the stopping distance for a more comfortable stop from lead vehicles."), ""},
{"SmoothBraking", tr("Smoother Braking"), tr("Smoothen out the braking behavior when approaching slower vehicles."), ""},
{"TrafficMode", tr("Traffic Mode"), tr("Enable the ability to activate 'Traffic Mode' by holding down the 'distance' button for 2.5 seconds. When 'Traffic Mode' is active the onroad UI will turn red and openpilot will drive catered towards stop and go traffic."), ""},
{"MTSCEnabled", tr("Map Turn Speed Control"), tr("Slow down for anticipated curves detected by the downloaded maps."), "../frogpilot/assets/toggle_icons/icon_speed_map.png"},
{"DisableMTSCSmoothing", tr("Disable MTSC UI Smoothing"), tr("Disables the smoothing for the requested speed in the onroad UI to show exactly what speed MTSC is currently requesting."), ""},
@@ -194,16 +195,38 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
aggressiveProfile->setVisible(true);
standardProfile->setVisible(true);
relaxedProfile->setVisible(true);
trafficProfile->setVisible(!isRelease && params.getBool("TrafficMode"));
});
toggle = customPersonalitiesToggle;
FrogPilotParamValueControl *trafficFollow = new FrogPilotParamValueControl("TrafficFollow", tr("Follow"),
tr("Set the minimum following distance when using 'Traffic Mode'. Your following distance will dynamically "
"adjust between this distance and the following distance from the 'Aggressive' profile when driving between "
"0 and %1.\n\nFor example:\n\nTraffic Mode: 0.5s\nAggressive: 1.0s\n\n0%2 = 0.5s\n%3 = 0.75s\n%1 = 1.0s")
.arg(isMetric ? "90ph" : "55mph")
.arg(isMetric ? "kph" : "mph")
.arg(isMetric ? "45kph" : "27.5mph"),
"../frogpilot/assets/other_images/traffic.png",
0.5, 5, std::map<int, QString>(), this, false, tr(" sec"), 1, 0.01);
FrogPilotParamValueControl *trafficJerk = new FrogPilotParamValueControl("TrafficJerk", tr("Jerk"),
tr("Set the minimum jerk value when using 'Traffic Mode'. Your jerk will dynamically "
"adjust between this jerk and the jerk from the 'Aggressive' profile when driving between "
"0 and %1.\n\nFor example:\n\nTraffic Mode: 0.1\nAggressive: 1.0\n\n0%2 = 0.1\n%3 = 0.5\n%1 = 1.0")
.arg(isMetric ? "90ph" : "55mph")
.arg(isMetric ? "kph" : "mph")
.arg(isMetric ? "45kph" : "27.5mph"),
"",
0.01, 5, std::map<int, QString>(), this, false, "", 1, 0.01);
trafficProfile = new FrogPilotDualParamControl(trafficFollow, trafficJerk, this, true);
addItem(trafficProfile);
FrogPilotParamValueControl *aggressiveFollow = new FrogPilotParamValueControl("AggressiveFollow", tr("Follow"),
tr("Set the 'Aggressive' personality' following distance. "
tr("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",
1, 5, std::map<int, QString>(), this, false, tr(" sec"), 1, 0.01);
FrogPilotParamValueControl *aggressiveJerk = new FrogPilotParamValueControl("AggressiveJerk", tr("Jerk"),
tr("Configure brake/gas pedal responsiveness for the 'Aggressive' personality. "
tr("Adjust brake/gas pedal responsiveness for the 'Aggressive' personality. "
"Higher jerk value = Less likely to use the gas/brake.\nLower jerk value = More likely.\n\nStock: 0.5."),
"",
0.01, 5, std::map<int, QString>(), this, false, "", 1, 0.01);
@@ -229,7 +252,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
"../frogpilot/assets/other_images/relaxed.png",
1, 5, std::map<int, QString>(), this, false, tr(" sec"), 1, 0.01);
FrogPilotParamValueControl *relaxedJerk = new FrogPilotParamValueControl("RelaxedJerk", tr("Jerk"),
tr("Set brake/gas pedal responsiveness for the 'Relaxed' personality. "
tr("Adjust brake/gas pedal responsiveness for the 'Relaxed' personality. "
"Higher jerk value = Less likely to use the gas/brake.\nLower jerk value = More likely.\n\nStock: 1.0."),
"",
0.01, 5, std::map<int, QString>(), this, false, "", 1, 0.01);
@@ -959,6 +982,7 @@ void FrogPilotControlsPanel::hideToggles() {
slcPriorityButton->setVisible(false);
standardProfile->setVisible(false);
relaxedProfile->setVisible(false);
trafficProfile->setVisible(false);
std::set<QString> longitudinalKeys = {"ConditionalExperimental", "CustomPersonalities", "ExperimentalModeActivation",
"LongitudinalTune", "MTSCEnabled", "SpeedLimitController"};
@@ -33,8 +33,9 @@ private:
FrogPilotDualParamControl *aggressiveProfile;
FrogPilotDualParamControl *conditionalSpeedsImperial;
FrogPilotDualParamControl *conditionalSpeedsMetric;
FrogPilotDualParamControl *standardProfile;
FrogPilotDualParamControl *relaxedProfile;
FrogPilotDualParamControl *standardProfile;
FrogPilotDualParamControl *trafficProfile;
FrogPilotParamManageControl *modelManagerToggle;
@@ -44,7 +45,7 @@ private:
std::set<QString> experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaTap"};
std::set<QString> laneChangeKeys = {"LaneChangeTime", "LaneDetectionWidth", "OneLaneChange"};
std::set<QString> lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite", "TacoTune"};
std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "SmoothBraking", "StoppingDistance"};
std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "SmoothBraking", "StoppingDistance", "TrafficMode"};
std::set<QString> mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck"};
std::set<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "PauseLateralSpeed", "ReverseCruise", "SetSpeedOffset"};
std::set<QString> speedLimitControllerKeys = {"SLCControls", "SLCQOL", "SLCVisuals"};
+18 -5
View File
@@ -509,7 +509,8 @@ void ExperimentalButton::paintEvent(QPaintEvent *event) {
(scene.always_on_lateral_active ? QColor(10, 186, 181, 255) :
(scene.conditional_status == 1 || scene.conditional_status == 3 || scene.conditional_status == 5 ? QColor(255, 246, 0, 255) :
(experimental_mode ? QColor(218, 111, 37, 241) :
(scene.navigate_on_openpilot ? QColor(49, 161, 238, 255) : QColor(0, 0, 0, 166))))) :
(scene.traffic_mode_active ? QColor(201, 34, 49, 255) :
(scene.navigate_on_openpilot ? QColor(49, 161, 238, 255) : QColor(0, 0, 0, 166)))))) :
QColor(0, 0, 0, 166);
if (!(scene.map_open && scene.big_map)) {
@@ -676,6 +677,8 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
), 10));
} else if (scene.reverse_cruise) {
p.setPen(QPen(blueColor(), 6));
} else if (trafficModeActive) {
p.setPen(QPen(redColor(), 10));
} else {
p.setPen(QPen(whiteColor(75), 6));
}
@@ -925,6 +928,10 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
pe.setColorAt(0.0, QColor::fromHslF(25 / 360., 0.71, 0.50, 1.0));
pe.setColorAt(0.5, QColor::fromHslF(25 / 360., 0.71, 0.50, 0.5));
pe.setColorAt(1.0, QColor::fromHslF(25 / 360., 0.71, 0.50, 0.1));
} else if (trafficModeActive) {
pe.setColorAt(0.0, QColor::fromHslF(355 / 360., 0.71, 0.46, 1.0));
pe.setColorAt(0.5, QColor::fromHslF(355 / 360., 0.71, 0.46, 0.5));
pe.setColorAt(1.0, QColor::fromHslF(355 / 360., 0.71, 0.46, 0.1));
} else if (scene.navigate_on_openpilot) {
pe.setColorAt(0.0, QColor::fromHslF(205 / 360., 0.85, 0.56, 1.0));
pe.setColorAt(0.5, QColor::fromHslF(205 / 360., 0.85, 0.56, 0.5));
@@ -1343,6 +1350,8 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() {
slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH);
useViennaSLCSign = scene.use_vienna_slc_sign;
trafficModeActive = scene.traffic_mode_active;
turnSignalLeft = scene.turn_signal_left;
turnSignalRight = scene.turn_signal_right;
@@ -1545,12 +1554,14 @@ DistanceButton::DistanceButton(QWidget *parent) : QPushButton(parent), scene(uiS
setFixedSize(btn_size * 1.5, btn_size * 1.5);
profile_data = {
{QPixmap("../frogpilot/assets/other_images/traffic.png"), "Traffic"},
{QPixmap("../frogpilot/assets/other_images/aggressive.png"), "Aggressive"},
{QPixmap("../frogpilot/assets/other_images/standard.png"), "Standard"},
{QPixmap("../frogpilot/assets/other_images/relaxed.png"), "Relaxed"}
};
profile_data_kaofui = {
{QPixmap("../frogpilot/assets/other_images/traffic_kaofui.png"), "Traffic"},
{QPixmap("../frogpilot/assets/other_images/aggressive_kaofui.png"), "Aggressive"},
{QPixmap("../frogpilot/assets/other_images/standard_kaofui.png"), "Standard"},
{QPixmap("../frogpilot/assets/other_images/relaxed_kaofui.png"), "Relaxed"}
@@ -1571,11 +1582,12 @@ void DistanceButton::buttonReleased() {
}
void DistanceButton::updateState() {
if (personality != static_cast<int>(scene.personality)) {
if (trafficModeActive != scene.traffic_mode_active || personality != static_cast<int>(scene.personality) && !trafficModeActive) {
transitionTimer.restart();
}
personality = static_cast<int>(scene.personality);
trafficModeActive = scene.traffic_mode_active;
}
void DistanceButton::paintEvent(QPaintEvent *event) {
@@ -1589,7 +1601,8 @@ void DistanceButton::paintEvent(QPaintEvent *event) {
qreal textOpacity = qBound(0.0, 1.0 - ((elapsed - textDuration) / fadeDuration), 1.0);
qreal imageOpacity = qBound(0.0, (elapsed - textDuration) / fadeDuration, 1.0);
auto &[profileImage, profileText] = scene.use_kaofui_icons ? profile_data_kaofui[personality] : profile_data[personality];
int profile = trafficModeActive ? 0 : personality + 1;
auto &[profileImage, profileText] = scene.use_kaofui_icons ? profile_data_kaofui[profile] : profile_data[profile];
if (textOpacity != 0.0) {
p.setOpacity(textOpacity);
@@ -1636,7 +1649,7 @@ void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) {
double acceleration = std::round(scene.acceleration * 100) / 100;
int randomEvent = scene.current_random_event;
if (acceleration > maxAcceleration && status == STATUS_ENGAGED) {
if (acceleration > maxAcceleration && (status == STATUS_ENGAGED || status == STATUS_TRAFFIC_MODE_ACTIVE)) {
maxAcceleration = acceleration;
isFiveSecondsPassed = false;
timer.start();
@@ -1708,7 +1721,7 @@ void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) {
drawText(accelText, Qt::white);
if (!maxAccSuffix.isEmpty()) {
drawText(maxAccSuffix, isFiveSecondsPassed ? Qt::white : Qt::red);
drawText(maxAccSuffix, isFiveSecondsPassed ? Qt::white : redColor());
}
drawText(obstacleText, Qt::white);
drawText(createDiffText(obstacleDistance, obstacleDistanceStock), (obstacleDistance - obstacleDistanceStock) > 0 ? Qt::green : Qt::red);
+3
View File
@@ -78,6 +78,8 @@ private:
UIScene &scene;
bool trafficModeActive;
int personality;
QElapsedTimer transitionTimer;
@@ -231,6 +233,7 @@ private:
bool showSLCOffset;
bool slcOverridden;
bool speedLimitController;
bool trafficModeActive;
bool turnSignalLeft;
bool turnSignalRight;
bool useViennaSLCSign;
+3
View File
@@ -253,6 +253,7 @@ static void update_state(UIState *s) {
auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl();
scene.always_on_lateral_active = !scene.enabled && frogpilotCarControl.getAlwaysOnLateral();
scene.speed_limit_changed = scene.speed_limit_controller && frogpilotCarControl.getSpeedLimitChanged();
scene.traffic_mode_active = frogpilotCarControl.getTrafficModeActive();
}
if (sm.updated("frogpilotPlan")) {
auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
@@ -405,6 +406,8 @@ void UIState::updateStatus() {
status = STATUS_OVERRIDE;
} else if (scene.always_on_lateral_active) {
status = STATUS_ALWAYS_ON_LATERAL_ACTIVE;
} else if (scene.traffic_mode_active && scene.enabled) {
status = STATUS_TRAFFIC_MODE_ACTIVE;
} else {
status = controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED;
}
+4
View File
@@ -114,6 +114,7 @@ typedef enum UIStatus {
// FrogPilot statuses
STATUS_ALWAYS_ON_LATERAL_ACTIVE,
STATUS_TRAFFIC_MODE_ACTIVE,
} UIStatus;
@@ -134,6 +135,7 @@ const QColor bg_colors [] = {
// FrogPilot colors
[STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1),
[STATUS_TRAFFIC_MODE_ACTIVE] = QColor(0xc9, 0x22, 0x31, 0xf1),
};
static std::map<cereal::ControlsState::AlertStatus, QColor> alert_colors = {
@@ -233,6 +235,8 @@ typedef struct UIScene {
bool standby_mode;
bool standstill;
bool tethering_enabled;
bool traffic_mode;
bool traffic_mode_active;
bool turn_signal_left;
bool turn_signal_right;
bool unlimited_road_ui_length;