From 8b46c5ac49c0613bc3ff993a17c4512cae22aeae Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 21 May 2024 23:52:28 -0700 Subject: [PATCH] FrogPilot setup - Setup UI --- selfdrive/ui/qt/offroad/settings.h | 1 + selfdrive/ui/qt/offroad/software_settings.cc | 2 +- selfdrive/ui/qt/onroad.cc | 127 +++++++++++++++++-- selfdrive/ui/qt/onroad.h | 31 ++++- selfdrive/ui/qt/sidebar.cc | 2 +- selfdrive/ui/qt/sidebar.h | 1 + selfdrive/ui/ui.cc | 10 +- selfdrive/ui/ui.h | 18 +++ 8 files changed, 178 insertions(+), 14 deletions(-) diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index dec811e..cf27f83 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -99,4 +99,5 @@ private: // FrogPilot variables Params paramsMemory{"/dev/shm/params"}; + UIScene &scene; }; diff --git a/selfdrive/ui/qt/offroad/software_settings.cc b/selfdrive/ui/qt/offroad/software_settings.cc index f22c60f..ef38f6f 100644 --- a/selfdrive/ui/qt/offroad/software_settings.cc +++ b/selfdrive/ui/qt/offroad/software_settings.cc @@ -20,7 +20,7 @@ void SoftwarePanel::checkForUpdates() { std::system("pkill -SIGUSR1 -f selfdrive.updated.updated"); } -SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) { +SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent), scene(uiState()->scene) { onroadLbl = new QLabel(tr("Updates are only downloaded while the car is off.")); onroadLbl->setStyleSheet("font-size: 50px; font-weight: 400; text-align: left; padding-top: 30px; padding-bottom: 30px;"); addItem(onroadLbl); diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 8db559f..75f67b8 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -28,7 +28,7 @@ static void drawIcon(QPainter &p, const QPoint ¢er, const QPixmap &img, cons p.setOpacity(1.0); } -OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { +OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent), scene(uiState()->scene) { QVBoxLayout *main_layout = new QVBoxLayout(this); main_layout->setMargin(UI_BORDER_SIZE); QStackedLayout *stacked_layout = new QStackedLayout; @@ -114,6 +114,7 @@ void OnroadWindow::offroadTransition(bool offroad) { QObject::connect(m, &MapPanel::mapPanelRequested, this, &OnroadWindow::mapPanelRequested); QObject::connect(nvg->map_settings_btn, &MapSettingsButton::clicked, m, &MapPanel::toggleMapSettings); + QObject::connect(nvg->map_settings_btn_bottom, &MapSettingsButton::clicked, m, &MapPanel::toggleMapSettings); nvg->map_settings_btn->setEnabled(true); m->setFixedWidth(topWidget(this)->width() / 2 - UI_BORDER_SIZE); @@ -170,11 +171,13 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { int margin = 40; int radius = 30; + int offset = true ? 25 : 0; if (alert.size == cereal::ControlsState::AlertSize::FULL) { margin = 0; radius = 0; + offset = 0; } - QRect r = QRect(0 + margin, height() - h + margin, width() - margin*2, h - margin*2); + QRect r = QRect(0 + margin, height() - h + margin - offset, width() - margin*2, h - margin*2); QPainter p(this); @@ -215,7 +218,7 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) { } // ExperimentalButton -ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent) { +ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(false), engageable(false), QPushButton(parent), scene(uiState()->scene) { setFixedSize(btn_size, btn_size); engage_img = loadPixmap("../assets/img_chffr_wheel.png", {img_size, img_size}); @@ -250,7 +253,7 @@ void ExperimentalButton::paintEvent(QPaintEvent *event) { // MapSettingsButton MapSettingsButton::MapSettingsButton(QWidget *parent) : QPushButton(parent) { - setFixedSize(btn_size, btn_size); + setFixedSize(btn_size, btn_size + 20); settings_img = loadPixmap("../assets/navigation/icon_directions_outlined.svg", {img_size, img_size}); // hidden by default, made visible if map is created (has prime or mapbox token) @@ -265,7 +268,7 @@ void MapSettingsButton::paintEvent(QPaintEvent *event) { // Window that shows camera view and variety of info drawn on top -AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent) { +AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent), scene(uiState()->scene) { pm = std::make_unique>({"uiDebug"}); main_layout = new QVBoxLayout(this); @@ -279,6 +282,9 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* par main_layout->addWidget(map_settings_btn, 0, Qt::AlignBottom | Qt::AlignRight); dm_img = loadPixmap("../assets/img_driver_face.png", {img_size + 5, img_size + 5}); + + // Initialize FrogPilot widgets + initializeFrogPilotWidgets(); } void AnnotatedCameraWidget::updateState(const UIState &s) { @@ -331,6 +337,9 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { map_settings_btn->setVisible(!hideBottomIcons); main_layout->setAlignment(map_settings_btn, (rightHandDM ? Qt::AlignLeft : Qt::AlignRight) | Qt::AlignBottom); } + + // Update FrogPilot widgets + updateFrogPilotWidgets(); } void AnnotatedCameraWidget::drawHud(QPainter &p) { @@ -429,6 +438,9 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { drawText(p, rect().center().x(), 290, speedUnit, 200); p.restore(); + + // Draw FrogPilot widgets + paintFrogPilotWidgets(p); } void AnnotatedCameraWidget::drawText(QPainter &p, int x, int y, const QString &text, int alpha) { @@ -471,7 +483,6 @@ void AnnotatedCameraWidget::updateFrameMat() { void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { painter.save(); - const UIScene &scene = s->scene; SubMaster &sm = *(s->sm); // lanelines @@ -488,7 +499,7 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { // paint path QLinearGradient bg(0, height(), 0, 0); - if (sm["controlsState"].getControlsState().getExperimentalMode()) { + if (experimentalMode) { // The first half of track_vertices are the points for the right side of the path // and the indices match the positions of accel from uiPlan const auto &acceleration = sm["uiPlan"].getUiPlan().getAccel(); @@ -528,13 +539,12 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { } void AnnotatedCameraWidget::drawDriverState(QPainter &painter, const UIState *s) { - const UIScene &scene = s->scene; - painter.save(); // base icon int offset = UI_BORDER_SIZE + btn_size / 2; int x = rightHandDM ? width() - offset : offset; + offset += true ? 25 : 0; int y = height() - offset; float opacity = dmActive ? 0.65 : 0.2; drawIcon(painter, QPoint(x, y), dm_img, blackColor(70), opacity); @@ -636,7 +646,7 @@ void AnnotatedCameraWidget::paintGL() { } else if (v_ego > 15) { wide_cam_requested = false; } - wide_cam_requested = wide_cam_requested && sm["controlsState"].getControlsState().getExperimentalMode(); + wide_cam_requested = wide_cam_requested && experimentalMode; // for replay of old routes, never go to widecam wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid; } @@ -704,3 +714,100 @@ void AnnotatedCameraWidget::showEvent(QShowEvent *event) { ui_update_params(uiState()); prev_draw_t = millis_since_boot(); } + +// FrogPilot widgets +void AnnotatedCameraWidget::initializeFrogPilotWidgets() { + bottom_layout = new QHBoxLayout(); + + QSpacerItem *spacer = new QSpacerItem(0, 0, QSizePolicy::Expanding, QSizePolicy::Minimum); + bottom_layout->addItem(spacer); + + map_settings_btn_bottom = new MapSettingsButton(this); + bottom_layout->addWidget(map_settings_btn_bottom); + + main_layout->addLayout(bottom_layout); +} + +void AnnotatedCameraWidget::updateFrogPilotWidgets() { + if (is_metric) { + accelerationUnit = tr(" m/s²"); + leadDistanceUnit = tr(mapOpen ? "m" : "meters"); + leadSpeedUnit = tr("kph"); + + accelerationConversion = 1.0f; + distanceConversion = 1.0f; + speedConversion = MS_TO_KPH; + } else { + accelerationUnit = tr(" ft/s²"); + leadDistanceUnit = tr(mapOpen ? "ft" : "feet"); + leadSpeedUnit = tr("mph"); + + accelerationConversion = METER_TO_FOOT; + distanceConversion = METER_TO_FOOT; + speedConversion = MS_TO_MPH; + } + + alertSize = scene.alert_size; + + experimentalMode = scene.experimental_mode; + + mapOpen = scene.map_open; +} + +void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) { + if (true) { + drawStatusBar(p); + } + + map_settings_btn_bottom->setEnabled(map_settings_btn->isEnabled()); + if (map_settings_btn_bottom->isEnabled()) { + map_settings_btn_bottom->setVisible(!hideBottomIcons); + bottom_layout->setAlignment(map_settings_btn_bottom, rightHandDM ? Qt::AlignLeft : Qt::AlignRight); + } +} + +void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { + p.save(); + + static bool displayStatusText = false; + + constexpr qreal fadeDuration = 1500.0; + constexpr qreal textDuration = 5000.0; + + static QElapsedTimer timer; + static QString lastShownStatus; + + QString newStatus; + + QRect currentRect = rect(); + QRect statusBarRect(currentRect.left() - 1, currentRect.bottom() - 50, currentRect.width() + 2, 100); + + p.setBrush(QColor(0, 0, 0, 150)); + p.setOpacity(1.0); + p.drawRoundedRect(statusBarRect, 30, 30); + + if (newStatus != lastShownStatus) { + displayStatusText = true; + lastShownStatus = newStatus; + timer.restart(); + } else if (displayStatusText && timer.hasExpired(textDuration + fadeDuration)) { + displayStatusText = false; + } + + p.setFont(InterFont(40, QFont::Bold)); + p.setPen(Qt::white); + p.setRenderHint(QPainter::TextAntialiasing); + + static qreal statusTextOpacity; + int elapsed = timer.elapsed(); + if (displayStatusText) { + statusTextOpacity = qBound(0.0, 1.0 - (elapsed - textDuration) / fadeDuration, 1.0); + } + + p.setOpacity(statusTextOpacity); + QRect textRect = p.fontMetrics().boundingRect(statusBarRect, Qt::AlignCenter | Qt::TextWordWrap, newStatus); + textRect.moveBottom(statusBarRect.bottom() - 50); + p.drawText(textRect, Qt::AlignCenter | Qt::TextWordWrap, newStatus); + + p.restore(); +} diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 0736801..8d1d317 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -20,7 +20,7 @@ class OnroadAlerts : public QWidget { Q_OBJECT public: - OnroadAlerts(QWidget *parent = 0) : QWidget(parent) {} + OnroadAlerts(QWidget *parent = 0) : QWidget(parent), scene(uiState()->scene) {} void updateAlert(const Alert &a); protected: @@ -29,6 +29,9 @@ protected: private: QColor bg; Alert alert = {}; + + // FrogPilot variables + UIScene &scene; }; class ExperimentalButton : public QPushButton { @@ -50,6 +53,7 @@ private: // FrogPilot variables Params paramsMemory{"/dev/shm/params"}; + UIScene &scene; }; @@ -74,6 +78,7 @@ public: void updateState(const UIState &s); MapSettingsButton *map_settings_btn; + MapSettingsButton *map_settings_btn_bottom; private: void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); @@ -100,8 +105,31 @@ private: int skip_frame_count = 0; bool wide_cam_requested = false; + // FrogPilot widgets + void initializeFrogPilotWidgets(); + void paintFrogPilotWidgets(QPainter &p); + void updateFrogPilotWidgets(); + + void drawStatusBar(QPainter &p); + // FrogPilot variables Params paramsMemory{"/dev/shm/params"}; + UIScene &scene; + + QHBoxLayout *bottom_layout; + + bool experimentalMode; + bool mapOpen; + + float accelerationConversion; + float distanceConversion; + float speedConversion; + + int alertSize; + + QString accelerationUnit; + QString leadDistanceUnit; + QString leadSpeedUnit; protected: void paintGL() override; @@ -142,6 +170,7 @@ private: QHBoxLayout* split; // FrogPilot variables + UIScene &scene; Params params; Params paramsMemory{"/dev/shm/params"}; diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc index a63c518..197bbe8 100644 --- a/selfdrive/ui/qt/sidebar.cc +++ b/selfdrive/ui/qt/sidebar.cc @@ -24,7 +24,7 @@ void Sidebar::drawMetric(QPainter &p, const QPair &label, QCol p.drawText(rect.adjusted(22, 0, 0, 0), Qt::AlignCenter, label.first + "\n" + label.second); } -Sidebar::Sidebar(QWidget *parent) : QFrame(parent), onroad(false), flag_pressed(false), settings_pressed(false) { +Sidebar::Sidebar(QWidget *parent) : QFrame(parent), onroad(false), flag_pressed(false), settings_pressed(false), scene(uiState()->scene) { home_img = loadPixmap("../assets/images/button_home.png", home_btn.size()); flag_img = loadPixmap("../assets/images/button_flag.png", home_btn.size()); settings_img = loadPixmap("../assets/images/button_settings.png", settings_btn.size(), Qt::IgnoreAspectRatio); diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h index e9fec78..f251040 100644 --- a/selfdrive/ui/qt/sidebar.h +++ b/selfdrive/ui/qt/sidebar.h @@ -62,4 +62,5 @@ private: // FrogPilot variables Params params; + UIScene &scene; }; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 4493b89..ce6245e 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -148,6 +148,8 @@ void update_dmonitoring(UIState *s, const cereal::DriverStateV2::Reader &drivers kpt_this = matvecmul3(r_xyz, kpt_this); scene.face_kpts_draw[kpi] = (vec3){{(float)kpt_this.v[0], (float)kpt_this.v[1], (float)(kpt_this.v[2] * (1.0-dm_fade_state) + 8 * dm_fade_state)}}; } + + scene.right_hand_drive = is_rhd; } static void update_sockets(UIState *s) { @@ -210,6 +212,9 @@ static void update_state(UIState *s) { } if (sm.updated("controlsState")) { auto controlsState = sm["controlsState"].getControlsState(); + scene.alert_size = controlsState.getAlertSize() == cereal::ControlsState::AlertSize::MID ? 350 : controlsState.getAlertSize() == cereal::ControlsState::AlertSize::SMALL ? 200 : 0; + scene.enabled = controlsState.getEnabled(); + scene.experimental_mode = controlsState.getExperimentalMode(); } if (sm.updated("deviceState")) { auto deviceState = sm["deviceState"].getDeviceState(); @@ -217,6 +222,9 @@ static void update_state(UIState *s) { if (sm.updated("frogpilotCarControl")) { auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl(); } + if (sm.updated("frogpilotCarState")) { + auto frogpilotCarState = sm["frogpilotCarState"].getFrogpilotCarState(); + } if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); } @@ -279,7 +287,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "liveLocationKalman", "driverStateV2", "wideRoadCameraState", "managerState", "navInstruction", "navRoute", "uiPlan", "carControl", "liveTorqueParameters", - "frogpilotCarControl", "frogpilotDeviceState", "frogpilotPlan", + "frogpilotCarControl", "frogpilotCarState", "frogpilotDeviceState", "frogpilotPlan", }); Params params; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index bfad139..831c663 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -107,6 +107,10 @@ typedef enum UIStatus { STATUS_DISENGAGED, STATUS_OVERRIDE, STATUS_ENGAGED, + + // FrogPilot statuses + STATUS_EXPERIMENTAL_MODE_ACTIVE, + STATUS_NAVIGATION_ACTIVE, } UIStatus; enum PrimeType { @@ -123,6 +127,10 @@ const QColor bg_colors [] = { [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8), [STATUS_OVERRIDE] = QColor(0x91, 0x9b, 0x95, 0xf1), [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), + + // FrogPilot colors + [STATUS_EXPERIMENTAL_MODE_ACTIVE] = QColor(0xda, 0x6f, 0x25, 0xf1), + [STATUS_NAVIGATION_ACTIVE] = QColor(0x31, 0xa1, 0xee, 0xf1), }; static std::map alert_colors = { @@ -164,6 +172,16 @@ typedef struct UIScene { bool started, ignition, is_metric, map_on_left, longitudinal_control; bool world_objects_visible = false; uint64_t started_frame; + + // FrogPilot variables + bool enabled; + bool experimental_mode; + bool map_open; + bool online; + bool right_hand_drive; + + int alert_size; + } UIScene; class UIState : public QObject {