This commit is contained in:
Your Name
2024-05-11 23:00:32 -05:00
parent bef1770b58
commit 65bcdde866
9 changed files with 159 additions and 134 deletions

View File

@@ -1,5 +1,7 @@
- Swap dark blue always on color for gray
- totally remove path when not engaged
- make side lanes 10% wider and the disengage mode 10% brighter
- make 25% of side lanes 50% darker than base
- hello world alert triggered by lkas btn
- hello world bootstrap dashboard
- button stuff:
@@ -7,6 +9,11 @@
- read paddle right ("")
- write pause, res, accel, mode, drive
test:
- disable all canbus
- set speed limit during stock long
-
OP -> Oscar
- oscar - global clearpilot state var
- oscar.cs - clearpilot car state (populated by op variables for cp.planner)

View File

@@ -118,13 +118,15 @@ class Controls:
# detect sound card presence and ensure successful init
sounds_available = HARDWARE.get_sound_card_online()
car_recognized = self.CP.carName != 'mock'
# cleanup old params
# clearpilot: evaluate
if not self.CP.experimentalLongitudinalAvailable:
self.params.remove("ExperimentalLongitudinalEnabled")
if not self.CP.openpilotLongitudinalControl:
self.params.remove("ExperimentalMode")
# if not self.CP.openpilotLongitudinalControl:
# self.params.remove("ExperimentalMode")
self.CC = car.CarControl.new_message()
self.CS_prev = car.CarState.new_message()
@@ -389,6 +391,7 @@ class Controls:
# Handle lane change
# CLEARPILOT - Disabled lane change helper
# CLEARPILOT TODO: Make this a toggle
# Clearpilot abstract this into a behavior
NoLaneChange = False
if not NoLaneChange:
if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
@@ -663,6 +666,7 @@ class Controls:
clearpilot_disable_lat_on_lane_change = True
# Enable blinkers while lane changing
# Abstract this into behavior
if model_v2.meta.laneChangeState != LaneChangeState.off:
CC.leftBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.left
CC.rightBlinker = model_v2.meta.laneChangeDirection == LaneChangeDirection.right
@@ -935,6 +939,7 @@ class Controls:
def params_thread(self, evt):
while not evt.is_set():
self.is_metric = self.params.get_bool("IsMetric")
# Clearpilot experimental mode - maybe change things here
if self.CP.openpilotLongitudinalControl and not self.frogpilot_variables.conditional_experimental_mode:
self.experimental_mode = self.params.get_bool("ExperimentalMode") or self.speed_limit_controller and SpeedLimitController.experimental_mode
self.personality = self.read_personality_param()
@@ -1101,7 +1106,9 @@ class Controls:
self.FPCC.alwaysOnLateral &= self.speed_check
self.FPCC.alwaysOnLateral &= not (CS.brakePressed and CS.vEgo < self.always_on_lateral_pause_speed) or CS.standstill
if self.CP.openpilotLongitudinalControl and self.frogpilot_variables.conditional_experimental_mode:
# clearpilot allow experimental in stock long
# removed "self.CP.openpilotLongitudinalControl and"
if self.frogpilot_variables.conditional_experimental_mode:
self.experimental_mode = self.sm['frogpilotPlan'].conditionalExperimental
self.drive_distance += CS.vEgo * DT_CTRL
@@ -1123,6 +1130,7 @@ class Controls:
self.params_storage.put_int_nonblocking("FrogPilotDrives", current_total_drives + 1)
self.drive_added = True
# Clearpilot - todo: override conditional on cruise button tap
# if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents) and self.experimental_mode_via_lkas:
# if self.frogpilot_variables.conditional_experimental_mode:
# conditional_status = self.params_memory.get_int("CEStatus")
@@ -1157,7 +1165,9 @@ class Controls:
def update_frogpilot_params(self):
self.always_on_lateral_pause_speed = self.always_on_lateral and self.params.get_int("PauseAOLOnBrake")
self.frogpilot_variables.conditional_experimental_mode = self.CP.openpilotLongitudinalControl and self.params.get_bool("ConditionalExperimental")
# clearpilot allow experimental in stock long
# removed "self.CP.openpilotLongitudinalControl and"
self.frogpilot_variables.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental")
custom_alerts = self.params.get_bool("CustomAlerts")
self.green_light_alert = custom_alerts and self.params.get_bool("GreenLightAlert")
@@ -1174,7 +1184,9 @@ class Controls:
device_management = self.params.get_bool("DeviceManagement")
self.increase_thermal_limits = device_management and self.params.get_bool("IncreaseThermalLimits")
experimental_mode_activation = self.CP.openpilotLongitudinalControl and self.params.get_bool("ExperimentalModeActivation")
# clearpilot allow experimental in stock long
# removed "self.CP.openpilotLongitudinalControl and"
experimental_mode_activation = self.params.get_bool("ExperimentalModeActivation")
self.frogpilot_variables.experimental_mode_via_distance = experimental_mode_activation and self.params.get_bool("ExperimentalModeViaDistance")
self.experimental_mode_via_lkas = experimental_mode_activation and self.params.get_bool("ExperimentalModeViaLKAS")
@@ -1219,6 +1231,7 @@ class Controls:
def update_clearpilot_events(self, CS):
if any(be.pressed and be.type == FrogPilotButtonType.lkas for be in CS.buttonEvents):
exit() # Just cause a crash for testing
self.events.add(EventName.clpDebug)
# clearpilot_notice(CP_NOTICE_DEBUG, "LKAS Debug Action Invoked")
foo = "bar"

View File

@@ -181,6 +181,7 @@ class LongitudinalPlanner:
force_slow_decel = sm['controlsState'].forceDecel
# Reset current state when not engaged, or user is controlling the speed
# clearpilot - might need changing
reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['controlsState'].enabled
# No change cost when user is controlling the speed, or when standstill

View File

@@ -126,7 +126,8 @@ class FrogPilotPlanner:
self.v_cruise = self.update_v_cruise(carState, controlsState, controlsState.enabled, liveLocationKalman, modelData, road_curvature, v_cruise, v_ego)
if self.conditional_experimental_mode and self.CP.openpilotLongitudinalControl or self.green_light_alert:
# clearpilot allow experimental on stock long
if self.conditional_experimental_mode or self.green_light_alert:
self.cem.update(carState, controlsState.enabled, frogpilotNavigation, self.lead_one, modelData, road_curvature, self.t_follow, v_ego)
if self.radarless_model:
@@ -277,7 +278,8 @@ class FrogPilotPlanner:
def update_frogpilot_params(self):
self.is_metric = self.params.get_bool("IsMetric")
self.conditional_experimental_mode = self.CP.openpilotLongitudinalControl and self.params.get_bool("ConditionalExperimental")
# clearpilot allow experimental in stock long
self.conditional_experimental_mode = self.params.get_bool("ConditionalExperimental")
if self.conditional_experimental_mode:
self.cem.update_frogpilot_params()
@@ -312,7 +314,7 @@ class FrogPilotPlanner:
self.smoother_braking_far_lead = self.smoother_braking and self.params.get_bool("SmoothBrakingFarLead") and not self.release
self.smoother_braking_jerk = self.smoother_braking and self.params.get_bool("SmoothBrakingJerk") and not self.release
self.map_turn_speed_controller = self.CP.openpilotLongitudinalControl and self.params.get_bool("MTSCEnabled")
self.map_turn_speed_controller = self.params.get_bool("MTSCEnabled")
self.mtsc_curvature_check = self.map_turn_speed_controller and self.params.get_bool("MTSCCurvatureCheck")
self.params_memory.put_float("MapTargetLatA", 2 * (self.params.get_int("MTSCAggressiveness") / 100))
@@ -320,6 +322,6 @@ class FrogPilotPlanner:
self.speed_limit_confirmation = self.speed_limit_controller and self.params.get_bool("SLCConfirmation")
self.speed_limit_controller_override = self.speed_limit_controller and self.params.get_int("SLCOverride")
self.vision_turn_controller = self.CP.openpilotLongitudinalControl and self.params.get_bool("VisionTurnControl")
self.vision_turn_controller = self.params.get_bool("VisionTurnControl")
self.curve_sensitivity = self.params.get_int("CurveSensitivity") / 100 if self.vision_turn_controller else 1
self.turn_aggressiveness = self.params.get_int("TurnAggressiveness") / 100 if self.vision_turn_controller else 1

View File

@@ -416,8 +416,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
toggle->setVisible(false);
}
deleteModelBtn->setVisible(true);
downloadModelBtn->setVisible(true);
// deleteModelBtn->setVisible(true);
// downloadModelBtn->setVisible(true);
selectModelBtn->setVisible(true);
});
toggle = modelsToggle;
@@ -425,138 +425,138 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
// CLEARPILOT changed path.
QDir modelDir("/data/openpilot/selfdrive/clearpilot/models/");
deleteModelBtn = new ButtonControl(tr("Delete Model"), tr("DELETE"), "");
QObject::connect(deleteModelBtn, &ButtonControl::clicked, [=]() {
std::string currentModel = params.get("Model") + ".thneed";
// deleteModelBtn = new ButtonControl(tr("Delete Model"), tr("DELETE"), "");
// QObject::connect(deleteModelBtn, &ButtonControl::clicked, [=]() {
// std::string currentModel = params.get("Model") + ".thneed";
QStringList availableModels = QString::fromStdString(params.get("AvailableModels")).split(",");
QStringList modelLabels = QString::fromStdString(params.get("AvailableModelsNames")).split(",");
// QStringList availableModels = QString::fromStdString(params.get("AvailableModels")).split(",");
// QStringList modelLabels = QString::fromStdString(params.get("AvailableModelsNames")).split(",");
QStringList existingModelFiles = modelDir.entryList({"*.thneed"}, QDir::Files);
QMap<QString, QString> labelToFileMap;
QStringList deletableModelLabels;
for (int i = 0; i < availableModels.size(); ++i) {
QString modelFileName = availableModels[i] + ".thneed";
if (existingModelFiles.contains(modelFileName) && modelFileName != QString::fromStdString(currentModel)) {
QString readableName = modelLabels[i];
deletableModelLabels.append(readableName);
labelToFileMap[readableName] = modelFileName;
}
}
// QStringList existingModelFiles = modelDir.entryList({"*.thneed"}, QDir::Files);
// QMap<QString, QString> labelToFileMap;
// QStringList deletableModelLabels;
// for (int i = 0; i < availableModels.size(); ++i) {
// QString modelFileName = availableModels[i] + ".thneed";
// if (existingModelFiles.contains(modelFileName) && modelFileName != QString::fromStdString(currentModel)) {
// QString readableName = modelLabels[i];
// deletableModelLabels.append(readableName);
// labelToFileMap[readableName] = modelFileName;
// }
// }
QString selectedModel = MultiOptionDialog::getSelection(tr("Select a model to delete"), deletableModelLabels, "", this);
if (!selectedModel.isEmpty() && ConfirmationDialog::confirm(tr("Are you sure you want to delete this model?"), tr("Delete"), this)) {
std::thread([=]() {
deleteModelBtn->setValue(tr("Deleting..."));
// QString selectedModel = MultiOptionDialog::getSelection(tr("Select a model to delete"), deletableModelLabels, "", this);
// if (!selectedModel.isEmpty() && ConfirmationDialog::confirm(tr("Are you sure you want to delete this model?"), tr("Delete"), this)) {
// std::thread([=]() {
// deleteModelBtn->setValue(tr("Deleting..."));
deleteModelBtn->setEnabled(false);
downloadModelBtn->setEnabled(false);
selectModelBtn->setEnabled(false);
// deleteModelBtn->setEnabled(false);
// downloadModelBtn->setEnabled(false);
// selectModelBtn->setEnabled(false);
QString modelToDelete = labelToFileMap[selectedModel];
// QString modelToDelete = labelToFileMap[selectedModel];
QFile::remove(modelDir.absoluteFilePath(modelToDelete));
// QFile::remove(modelDir.absoluteFilePath(modelToDelete));
deleteModelBtn->setEnabled(true);
downloadModelBtn->setEnabled(true);
selectModelBtn->setEnabled(true);
// deleteModelBtn->setEnabled(true);
// downloadModelBtn->setEnabled(true);
// selectModelBtn->setEnabled(true);
deleteModelBtn->setValue(tr("Deleted!"));
std::this_thread::sleep_for(std::chrono::seconds(3));
deleteModelBtn->setValue("");
}).detach();
}
});
addItem(deleteModelBtn);
// deleteModelBtn->setValue(tr("Deleted!"));
// std::this_thread::sleep_for(std::chrono::seconds(3));
// deleteModelBtn->setValue("");
// }).detach();
// }
// });
// addItem(deleteModelBtn);
downloadModelBtn = new ButtonControl(tr("Download Model"), tr("DOWNLOAD"), "");
QObject::connect(downloadModelBtn, &ButtonControl::clicked, [=]() {
QStringList availableModels = QString::fromStdString(params.get("AvailableModels")).split(",");
QStringList modelLabels = QString::fromStdString(params.get("AvailableModelsNames")).split(",");
// downloadModelBtn = new ButtonControl(tr("Download Model"), tr("DOWNLOAD"), "");
// QObject::connect(downloadModelBtn, &ButtonControl::clicked, [=]() {
// QStringList availableModels = QString::fromStdString(params.get("AvailableModels")).split(",");
// QStringList modelLabels = QString::fromStdString(params.get("AvailableModelsNames")).split(",");
QMap<QString, QString> labelToModelMap;
QStringList downloadableModelLabels;
QStringList existingModelFiles = modelDir.entryList({"*.thneed"}, QDir::Files);
for (int i = 0; i < availableModels.size(); ++i) {
QString modelFileName = availableModels.at(i) + ".thneed";
if (!existingModelFiles.contains(modelFileName)) {
QString readableName = modelLabels.at(i);
if (!readableName.endsWith("(Default)")) {
downloadableModelLabels.append(readableName);
labelToModelMap.insert(readableName, availableModels.at(i));
}
}
}
// QMap<QString, QString> labelToModelMap;
// QStringList downloadableModelLabels;
// QStringList existingModelFiles = modelDir.entryList({"*.thneed"}, QDir::Files);
// for (int i = 0; i < availableModels.size(); ++i) {
// QString modelFileName = availableModels.at(i) + ".thneed";
// if (!existingModelFiles.contains(modelFileName)) {
// QString readableName = modelLabels.at(i);
// if (!readableName.endsWith("(Default)")) {
// downloadableModelLabels.append(readableName);
// labelToModelMap.insert(readableName, availableModels.at(i));
// }
// }
// }
QString modelToDownload = MultiOptionDialog::getSelection(tr("Select a driving model to download"), downloadableModelLabels, "", this);
if (!modelToDownload.isEmpty()) {
QString selectedModelValue = labelToModelMap.value(modelToDownload);
paramsMemory.put("ModelToDownload", selectedModelValue.toStdString());
// QString modelToDownload = MultiOptionDialog::getSelection(tr("Select a driving model to download"), downloadableModelLabels, "", this);
// if (!modelToDownload.isEmpty()) {
// QString selectedModelValue = labelToModelMap.value(modelToDownload);
// paramsMemory.put("ModelToDownload", selectedModelValue.toStdString());
deleteModelBtn->setEnabled(false);
downloadModelBtn->setEnabled(false);
selectModelBtn->setEnabled(false);
// deleteModelBtn->setEnabled(false);
// downloadModelBtn->setEnabled(false);
// selectModelBtn->setEnabled(false);
QTimer *failureTimer = new QTimer(this);
failureTimer->setSingleShot(true);
// QTimer *failureTimer = new QTimer(this);
// failureTimer->setSingleShot(true);
QTimer *progressTimer = new QTimer(this);
progressTimer->setInterval(100);
// QTimer *progressTimer = new QTimer(this);
// progressTimer->setInterval(100);
connect(failureTimer, &QTimer::timeout, this, [=]() {
deleteModelBtn->setEnabled(true);
downloadModelBtn->setEnabled(true);
selectModelBtn->setEnabled(true);
// connect(failureTimer, &QTimer::timeout, this, [=]() {
// deleteModelBtn->setEnabled(true);
// downloadModelBtn->setEnabled(true);
// selectModelBtn->setEnabled(true);
downloadModelBtn->setValue(tr("Download failed..."));
paramsMemory.remove("ModelDownloadProgress");
paramsMemory.remove("ModelToDownload");
// downloadModelBtn->setValue(tr("Download failed..."));
// paramsMemory.remove("ModelDownloadProgress");
// paramsMemory.remove("ModelToDownload");
progressTimer->stop();
progressTimer->deleteLater();
// progressTimer->stop();
// progressTimer->deleteLater();
QTimer::singleShot(3000, this, [this]() {
downloadModelBtn->setValue("");
});
});
// QTimer::singleShot(3000, this, [this]() {
// downloadModelBtn->setValue("");
// });
// });
connect(progressTimer, &QTimer::timeout, this, [=]() mutable {
static int lastProgress = -1;
int progress = paramsMemory.getInt("ModelDownloadProgress");
// connect(progressTimer, &QTimer::timeout, this, [=]() mutable {
// static int lastProgress = -1;
// int progress = paramsMemory.getInt("ModelDownloadProgress");
if (progress == lastProgress) {
if (!failureTimer->isActive()) {
failureTimer->start(30000);
}
} else {
lastProgress = progress;
downloadModelBtn->setValue(QString::number(progress) + "%");
failureTimer->stop();
// if (progress == lastProgress) {
// if (!failureTimer->isActive()) {
// failureTimer->start(30000);
// }
// } else {
// lastProgress = progress;
// downloadModelBtn->setValue(QString::number(progress) + "%");
// failureTimer->stop();
if (progress == 100) {
deleteModelBtn->setEnabled(true);
downloadModelBtn->setEnabled(true);
selectModelBtn->setEnabled(true);
// if (progress == 100) {
// deleteModelBtn->setEnabled(true);
// downloadModelBtn->setEnabled(true);
// selectModelBtn->setEnabled(true);
downloadModelBtn->setValue(tr("Downloaded!"));
paramsMemory.remove("ModelDownloadProgress");
paramsMemory.remove("ModelToDownload");
// downloadModelBtn->setValue(tr("Downloaded!"));
// paramsMemory.remove("ModelDownloadProgress");
// paramsMemory.remove("ModelToDownload");
progressTimer->stop();
progressTimer->deleteLater();
// progressTimer->stop();
// progressTimer->deleteLater();
QTimer::singleShot(3000, this, [this]() {
if (paramsMemory.get("ModelDownloadProgress").empty()) {
downloadModelBtn->setValue("");
}
});
}
}
});
progressTimer->start();
}
});
addItem(downloadModelBtn);
// QTimer::singleShot(3000, this, [this]() {
// if (paramsMemory.get("ModelDownloadProgress").empty()) {
// downloadModelBtn->setValue("");
// }
// });
// }
// }
// });
// progressTimer->start();
// }
// });
// addItem(downloadModelBtn);
selectModelBtn = new ButtonControl(tr("Select Model"), tr("SELECT"), "");
QObject::connect(selectModelBtn, &ButtonControl::clicked, [=]() {
@@ -882,7 +882,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil
void FrogPilotControlsPanel::showEvent(QShowEvent *event, const UIState &s) {
hasOpenpilotLongitudinal = hasOpenpilotLongitudinal && !params.getBool("DisableOpenpilotLongitudinal");
downloadModelBtn->setEnabled(s.scene.online && (!s.scene.started || s.scene.parked));
// downloadModelBtn->setEnabled(s.scene.online && (!s.scene.started || s.scene.parked));
}
void FrogPilotControlsPanel::updateState(const UIState &s) {
@@ -1040,16 +1040,16 @@ void FrogPilotControlsPanel::hideToggles() {
aggressiveProfile->setVisible(false);
conditionalSpeedsImperial->setVisible(false);
conditionalSpeedsMetric->setVisible(false);
deleteModelBtn->setVisible(false);
downloadModelBtn->setVisible(false);
// deleteModelBtn->setVisible(false);
// downloadModelBtn->setVisible(false);
selectModelBtn->setVisible(false);
slcPriorityButton->setVisible(false);
standardProfile->setVisible(false);
relaxedProfile->setVisible(false);
trafficProfile->setVisible(false);
//"ConditionalExperimental" , "ExperimentalModeActivation", "VisionTurnControl"
std::set<QString> longitudinalKeys = {"CustomPersonalities", "LongitudinalTune", "MTSCEnabled", "SpeedLimitController"};
//"ConditionalExperimental" , "ExperimentalModeActivation", "VisionTurnControl", "MTSCEnabled"
std::set<QString> longitudinalKeys = {"CustomPersonalities", "LongitudinalTune", "SpeedLimitController"};
for (auto &[key, toggle] : toggles) {
toggle->setVisible(false);

View File

@@ -25,8 +25,8 @@ private:
void updateState(const UIState &s);
void updateToggles();
ButtonControl *deleteModelBtn;
ButtonControl *downloadModelBtn;
// ButtonControl *deleteModelBtn;
// ButtonControl *downloadModelBtn;
ButtonControl *selectModelBtn;
ButtonControl *slcPriorityButton;

View File

@@ -900,10 +900,10 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
path_gradient.setColorAt(0.5, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.4)));
path_gradient.setColorAt(1.0, QColor(center_lane_color.red(), center_lane_color.green(), center_lane_color.blue(), static_cast<int>(CENTER_LANE_ALPHA * 255 * 0.0)));
}
}
painter.setBrush(path_gradient);
painter.drawPolygon(scene.track_vertices);
painter.setBrush(path_gradient);
painter.drawPolygon(scene.track_vertices);
}
// Paint path edges ,Use current background color
if (edgeColor != bg_colors[STATUS_DISENGAGED]) {
@@ -1134,7 +1134,9 @@ void AnnotatedCameraWidget::paintEvent(QPaintEvent *event) {
update_model(s, model, sm["uiPlan"].getUiPlan());
drawLaneLines(painter, s);
if (s->scene.longitudinal_control && sm.rcv_frame("modelV2") > s->scene.started_frame) {
// Clearpilot - draw leads even in stock long (test)
// if (s->scene.longitudinal_control && sm.rcv_frame("modelV2") > s->scene.started_frame) {
if (sm.rcv_frame("modelV2") > s->scene.started_frame) {
update_leads(s, model);
float prev_drel = -1;
for (int i = 0; i < model.getLeadsV3().size() && i < 2; i++) {

View File

@@ -44,7 +44,7 @@ const QColor bg_colors [] = {
[STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8),
[STATUS_OVERRIDE] = QColor(94, 112, 255, 0xd1),
[STATUS_ENGAGED] = QColor(76, 97, 255, 0xd1),
[STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(177, 183, 224, 0xd1),
[STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(209, 215, 250, 0xd1),
// [STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(184, 193, 255, 0xd1),
[STATUS_TRAFFIC_MODE_ACTIVE] = QColor(0xc9, 0x22, 0x31, 0xd1), // ? unused?
[STATUS_EXPERIMENTAL_ACTIVE] = QColor(201, 41, 204, 0xd1), // Magenta