From 56e9dbcf99faef5dfd3740903a694b85f1a9a60c Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 1 Jun 2021 20:59:41 -0700 Subject: [PATCH] Qt driverview (#21063) * CameraViewWidget * continue * cleanup * mv DriverViewWindow to ui/qt/offroad * write IsDriverViewEnabled in showEvent/hideEvnet * sm.update(0) in onTimeout() * CameraViewWidget * use unique_ptr for vipc_client * virtual draw * fix viewport * connected()->frameReceived() * bg_colors use QColor * fix draw * rebase master * whitespace * apply reviews * indent * like onroad continue * white space * continue * show == false * remove border * use widget's size * fix shadowed rect * cleanup driverview * fix transform * remove video_rect Co-authored-by: deanlee Co-authored-by: Comma Device old-commit-hash: 9876723169ba0fd0c61169699caaa63246473e85 --- selfdrive/ui/SConscript | 4 +- selfdrive/ui/paint.cc | 183 +++++----------------- selfdrive/ui/qt/home.cc | 21 ++- selfdrive/ui/qt/home.h | 3 + selfdrive/ui/qt/offroad/driverview.cc | 109 +++++++++++++ selfdrive/ui/qt/offroad/driverview.h | 48 ++++++ selfdrive/ui/qt/offroad/settings.cc | 6 +- selfdrive/ui/qt/offroad/settings.h | 2 + selfdrive/ui/qt/onroad.cc | 5 +- selfdrive/ui/qt/widgets/cameraview.cc | 217 ++++++++++++++++++++++++++ selfdrive/ui/qt/widgets/cameraview.h | 44 ++++++ selfdrive/ui/qt/window.cc | 3 + selfdrive/ui/ui.cc | 10 +- selfdrive/ui/ui.h | 24 +-- 14 files changed, 503 insertions(+), 176 deletions(-) create mode 100644 selfdrive/ui/qt/offroad/driverview.cc create mode 100644 selfdrive/ui/qt/offroad/driverview.h create mode 100644 selfdrive/ui/qt/widgets/cameraview.cc create mode 100644 selfdrive/ui/qt/widgets/cameraview.h diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index ec1867e9ff..d7625cf1ca 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -22,7 +22,7 @@ if arch == "Darwin": widgets_src = ["qt/widgets/input.cc", "qt/widgets/drive_stats.cc", "qt/widgets/ssh_keys.cc", "qt/widgets/toggle.cc", "qt/widgets/controls.cc", "qt/widgets/offroad_alerts.cc", "qt/widgets/setup.cc", "qt/widgets/keyboard.cc", - "qt/widgets/scrollview.cc", "#phonelibs/qrcode/QrCode.cc", "qt/api.cc", + "qt/widgets/scrollview.cc", "qt/widgets/cameraview.cc", "#phonelibs/qrcode/QrCode.cc", "qt/api.cc", "qt/request_repeater.cc"] if arch != 'aarch64': @@ -43,7 +43,7 @@ qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=base_libs) # build main UI qt_src = ["main.cc", "ui.cc", "paint.cc", "qt/sidebar.cc", "qt/onroad.cc", "qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc", - "qt/offroad/onboarding.cc", "#phonelibs/nanovg/nanovg.c"] + "qt/offroad/onboarding.cc", "qt/offroad/driverview.cc", "#phonelibs/nanovg/nanovg.c"] qt_env.Program("_ui", qt_src, LIBS=qt_libs) diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 854533fc3c..39c1aebda6 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -24,11 +24,6 @@ #include "selfdrive/ui/ui.h" -// TODO: this is also hardcoded in common/transformations/camera.py -// TODO: choose based on frame input size -const float y_offset = Hardware::TICI() ? 150.0 : 0.0; -const float zoom = Hardware::TICI() ? 2912.8 : 2138.5; - static void ui_draw_text(const UIState *s, float x, float y, const char *string, float size, NVGcolor color, const char *font_name) { nvgFontFace(s->vg, font_name); nvgFontSize(s->vg, size); @@ -115,14 +110,8 @@ static void ui_draw_line(UIState *s, const line_vertices_data &vd, NVGcolor *col } static void draw_frame(UIState *s) { - mat4 *out_mat; - if (s->scene.driver_view) { - glBindVertexArray(s->frame_vao[1]); - out_mat = &s->front_frame_mat; - } else { - glBindVertexArray(s->frame_vao[0]); - out_mat = &s->rear_frame_mat; - } + glBindVertexArray(s->frame_vao); + mat4 *out_mat = &s->rear_frame_mat; glActiveTexture(GL_TEXTURE0); if (s->last_frame) { @@ -227,7 +216,9 @@ static void ui_draw_vision_event(UIState *s) { const int radius = 96; const int center_x = s->viz_rect.right() - radius - bdr_s * 2; const int center_y = s->viz_rect.y + radius + (bdr_s * 1.5); - ui_draw_circle_image(s, center_x, center_y, radius, "wheel", bg_colors[s->status], 1.0f); + const QColor &color = bg_colors[s->status]; + NVGcolor nvg_color = nvgRGBA(color.red(), color.green(), color.blue(), color.alpha()); + ui_draw_circle_image(s, center_x, center_y, radius, "wheel", nvg_color, 1.0f); } } @@ -239,45 +230,6 @@ static void ui_draw_vision_face(UIState *s) { ui_draw_circle_image(s, center_x, center_y, radius, "driver_face", is_active); } -static void ui_draw_driver_view(UIState *s) { - const bool is_rhd = s->scene.is_rhd; - const int width = 4 * s->viz_rect.h / 3; - const Rect rect = {s->viz_rect.centerX() - width / 2, s->viz_rect.y, width, s->viz_rect.h}; // x, y, w, h - const Rect valid_rect = {is_rhd ? rect.right() - rect.h / 2 : rect.x, rect.y, rect.h / 2, rect.h}; - - // blackout - const int blackout_x_r = valid_rect.right(); - const Rect &blackout_rect = Hardware::TICI() ? s->viz_rect : rect; - const int blackout_w_r = blackout_rect.right() - valid_rect.right(); - const int blackout_x_l = blackout_rect.x; - const int blackout_w_l = valid_rect.x - blackout_x_l; - ui_fill_rect(s->vg, {blackout_x_l, rect.y, blackout_w_l, rect.h}, COLOR_BLACK_ALPHA(144)); - ui_fill_rect(s->vg, {blackout_x_r, rect.y, blackout_w_r, rect.h}, COLOR_BLACK_ALPHA(144)); - - auto driver_state = (*s->sm)["driverState"].getDriverState(); - const bool face_detected = driver_state.getFaceProb() > 0.4; - if (face_detected) { - auto fxy_list = driver_state.getFacePosition(); - float face_x = fxy_list[0]; - float face_y = fxy_list[1]; - int fbox_x = valid_rect.centerX() + (is_rhd ? face_x : -face_x) * valid_rect.w; - int fbox_y = valid_rect.centerY() + face_y * valid_rect.h; - - float alpha = 0.2; - if (face_x = std::abs(face_x), face_y = std::abs(face_y); face_x <= 0.35 && face_y <= 0.4) - alpha = 0.8 - (face_x > face_y ? face_x : face_y) * 0.6 / 0.375; - - const int box_size = 0.6 * rect.h / 2; - ui_draw_rect(s->vg, {fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size}, nvgRGBAf(1.0, 1.0, 1.0, alpha), 10, 35.); - } - - // draw face icon - const int face_radius = 85; - const int center_x = is_rhd ? rect.right() - face_radius - bdr_s * 2 : rect.x + face_radius + bdr_s * 2; - const int center_y = rect.bottom() - face_radius - bdr_s * 2.5; - ui_draw_circle_image(s, center_x, center_y, face_radius, "driver_face", face_detected); -} - static void ui_draw_vision_header(UIState *s) { NVGpaint gradient = nvgLinearGradient(s->vg, s->viz_rect.x, s->viz_rect.y+(header_h-(header_h/2.5)), @@ -304,24 +256,20 @@ static void ui_draw_vision_frame(UIState *s) { static void ui_draw_vision(UIState *s) { const UIScene *scene = &s->scene; - if (!scene->driver_view) { - // Draw augmented elements - if (scene->world_objects_visible) { - ui_draw_world(s); - } - // Set Speed, Current Speed, Status/Events - ui_draw_vision_header(s); - if ((*s->sm)["controlsState"].getControlsState().getAlertSize() == cereal::ControlsState::AlertSize::NONE) { - ui_draw_vision_face(s); - } - } else { - ui_draw_driver_view(s); + // Draw augmented elements + if (scene->world_objects_visible) { + ui_draw_world(s); + } + // Set Speed, Current Speed, Status/Events + ui_draw_vision_header(s); + if ((*s->sm)["controlsState"].getControlsState().getAlertSize() == cereal::ControlsState::AlertSize::NONE) { + ui_draw_vision_face(s); } } static void ui_draw_background(UIState *s) { - const NVGcolor color = bg_colors[s->status]; - glClearColor(color.r, color.g, color.b, 1.0); + const QColor &color = bg_colors[s->status]; + glClearColor(color.redF(), color.greenF(), color.blueF(), 1.0); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); } @@ -346,10 +294,6 @@ void ui_draw(UIState *s, int w, int h) { ui_draw_vision(s); } - if (s->scene.driver_view && !s->vipc_client->connected) { - nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); - ui_draw_text(s, s->viz_rect.centerX(), s->viz_rect.centerY(), "Please wait for camera to start", 40 * 2.5, COLOR_WHITE, "sans-bold"); - } nvgEndFrame(s->vg); glDisable(GL_BLEND); } @@ -424,37 +368,6 @@ static const mat4 device_transform = {{ 0.0, 0.0, 0.0, 1.0, }}; -static mat4 get_driver_view_transform() { - const float driver_view_ratio = 1.333; - mat4 transform; - if (Hardware::TICI()) { - // from dmonitoring.cc - const int full_width_tici = 1928; - const int full_height_tici = 1208; - const int adapt_width_tici = 668; - const int crop_x_offset = 32; - const int crop_y_offset = -196; - const float yscale = full_height_tici * driver_view_ratio / adapt_width_tici; - const float xscale = yscale*(1080-2*bdr_s)/(2160-2*bdr_s)*full_width_tici/full_height_tici; - transform = (mat4){{ - xscale, 0.0, 0.0, xscale*crop_x_offset/full_width_tici*2, - 0.0, yscale, 0.0, yscale*crop_y_offset/full_height_tici*2, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0, - }}; - - } else { - // frame from 4/3 to 16/9 display - transform = (mat4){{ - driver_view_ratio*(1080-2*bdr_s)/(1920-2*bdr_s), 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 1.0, - }}; - } - return transform; -} - void ui_nvg_init(UIState *s) { // init drawing @@ -494,50 +407,35 @@ void ui_nvg_init(UIState *s) { assert(glGetError() == GL_NO_ERROR); - for (int i = 0; i < 2; i++) { - float x1, x2, y1, y2; - if (i == 1) { - // flip horizontally so it looks like a mirror - x1 = 0.0; - x2 = 1.0; - y1 = 1.0; - y2 = 0.0; - } else { - x1 = 1.0; - x2 = 0.0; - y1 = 1.0; - y2 = 0.0; - } - const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3}; - const float frame_coords[4][4] = { - {-1.0, -1.0, x2, y1}, //bl - {-1.0, 1.0, x2, y2}, //tl - { 1.0, 1.0, x1, y2}, //tr - { 1.0, -1.0, x1, y1}, //br - }; - - glGenVertexArrays(1, &s->frame_vao[i]); - glBindVertexArray(s->frame_vao[i]); - glGenBuffers(1, &s->frame_vbo[i]); - glBindBuffer(GL_ARRAY_BUFFER, s->frame_vbo[i]); - glBufferData(GL_ARRAY_BUFFER, sizeof(frame_coords), frame_coords, GL_STATIC_DRAW); - glEnableVertexAttribArray(frame_pos_loc); - glVertexAttribPointer(frame_pos_loc, 2, GL_FLOAT, GL_FALSE, - sizeof(frame_coords[0]), (const void *)0); - glEnableVertexAttribArray(frame_texcoord_loc); - glVertexAttribPointer(frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE, - sizeof(frame_coords[0]), (const void *)(sizeof(float) * 2)); - glGenBuffers(1, &s->frame_ibo[i]); - glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, s->frame_ibo[i]); - glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(frame_indicies), frame_indicies, GL_STATIC_DRAW); - glBindBuffer(GL_ARRAY_BUFFER, 0); - glBindVertexArray(0); - } + float x1 = 1.0, x2 = 0.0, y1 = 1.0, y2 = 0.0; + const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3}; + const float frame_coords[4][4] = { + {-1.0, -1.0, x2, y1}, //bl + {-1.0, 1.0, x2, y2}, //tl + { 1.0, 1.0, x1, y2}, //tr + { 1.0, -1.0, x1, y1}, //br + }; + + glGenVertexArrays(1, &s->frame_vao); + glBindVertexArray(s->frame_vao); + glGenBuffers(1, &s->frame_vbo); + glBindBuffer(GL_ARRAY_BUFFER, s->frame_vbo); + glBufferData(GL_ARRAY_BUFFER, sizeof(frame_coords), frame_coords, GL_STATIC_DRAW); + glEnableVertexAttribArray(frame_pos_loc); + glVertexAttribPointer(frame_pos_loc, 2, GL_FLOAT, GL_FALSE, + sizeof(frame_coords[0]), (const void *)0); + glEnableVertexAttribArray(frame_texcoord_loc); + glVertexAttribPointer(frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE, + sizeof(frame_coords[0]), (const void *)(sizeof(float) * 2)); + glGenBuffers(1, &s->frame_ibo); + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, s->frame_ibo); + glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(frame_indicies), frame_indicies, GL_STATIC_DRAW); + glBindBuffer(GL_ARRAY_BUFFER, 0); + glBindVertexArray(0); ui_resize(s, s->fb_w, s->fb_h); } - void ui_resize(UIState *s, int width, int height){ s->fb_w = width; s->fb_h = height; @@ -561,7 +459,6 @@ void ui_resize(UIState *s, int width, int height){ 0.0, 0.0, 0.0, 1.0, }}; - s->front_frame_mat = matmul(device_transform, get_driver_view_transform()); s->rear_frame_mat = matmul(device_transform, frame_transform); // Apply transformation such that video pixel coordinates match video diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc index 128d46cfe0..58c669cf05 100644 --- a/selfdrive/ui/qt/home.cc +++ b/selfdrive/ui/qt/home.cc @@ -37,6 +37,12 @@ HomeWindow::HomeWindow(QWidget* parent) : QWidget(parent) { slayout->addWidget(home); QObject::connect(this, &HomeWindow::openSettings, home, &OffroadHome::refresh); + driver_view = new DriverViewWindow(this); + connect(driver_view, &DriverViewWindow::done, [=] { + showDriverView(false); + }); + slayout->addWidget(driver_view); + setLayout(layout); } @@ -50,14 +56,17 @@ void HomeWindow::offroadTransition(bool offroad) { emit offroadTransitionSignal(offroad); } -void HomeWindow::mousePressEvent(QMouseEvent* e) { - // TODO: make a nice driver view widget - if (QUIState::ui_state.scene.driver_view) { - Params().putBool("IsDriverViewEnabled", false); - QUIState::ui_state.scene.driver_view = false; - return; +void HomeWindow::showDriverView(bool show) { + if (show) { + emit closeSettings(); + slayout->setCurrentWidget(driver_view); + } else { + slayout->setCurrentWidget(home); } + sidebar->setVisible(show == false); +} +void HomeWindow::mousePressEvent(QMouseEvent* e) { // Handle sidebar collapsing if (onroad->isVisible() && (!sidebar->isVisible() || e->x() > sidebar->width())) { // Hide map first if visible, then hide sidebar diff --git a/selfdrive/ui/qt/home.h b/selfdrive/ui/qt/home.h index bac2b2dc51..db3e2f8f6e 100644 --- a/selfdrive/ui/qt/home.h +++ b/selfdrive/ui/qt/home.h @@ -7,6 +7,7 @@ #include #include +#include "selfdrive/ui/qt/offroad/driverview.h" #include "selfdrive/ui/qt/onroad.h" #include "selfdrive/ui/qt/sidebar.h" #include "selfdrive/ui/qt/widgets/offroad_alerts.h" @@ -52,6 +53,7 @@ signals: public slots: void offroadTransition(bool offroad); + void showDriverView(bool show); protected: void mousePressEvent(QMouseEvent* e) override; @@ -60,5 +62,6 @@ private: Sidebar *sidebar; OffroadHome *home; OnroadWindow *onroad; + DriverViewWindow *driver_view; QStackedLayout *slayout; }; diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc new file mode 100644 index 0000000000..f6a3ebaf7a --- /dev/null +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -0,0 +1,109 @@ +#include "selfdrive/ui/qt/offroad/driverview.h" + +#include + +#include "selfdrive/ui/qt/qt_window.h" +#include "selfdrive/ui/qt/util.h" + +const int FACE_IMG_SIZE = 130; + +DriverViewWindow::DriverViewWindow(QWidget* parent) : QWidget(parent) { + setAttribute(Qt::WA_OpaquePaintEvent); + layout = new QStackedLayout(this); + layout->setStackingMode(QStackedLayout::StackAll); + + cameraView = new CameraViewWidget(VISION_STREAM_RGB_FRONT, this); + layout->addWidget(cameraView); + + scene = new DriverViewScene(this); + connect(cameraView, &CameraViewWidget::frameUpdated, scene, &DriverViewScene::frameUpdated); + layout->addWidget(scene); + layout->setCurrentWidget(scene); +} + +void DriverViewWindow::mousePressEvent(QMouseEvent* e) { + emit done(); +} + +DriverViewScene::DriverViewScene(QWidget* parent) : sm({"driverState"}), QWidget(parent) { + face = QImage("../assets/img_driver_face.png").scaled(FACE_IMG_SIZE, FACE_IMG_SIZE, Qt::KeepAspectRatio, Qt::SmoothTransformation); +} + +void DriverViewScene::showEvent(QShowEvent* event) { + frame_updated = false; + is_rhd = params.getBool("IsRHD"); + params.putBool("IsDriverViewEnabled", true); +} + +void DriverViewScene::hideEvent(QHideEvent* event) { + params.putBool("IsDriverViewEnabled", false); +} + +void DriverViewScene::frameUpdated() { + frame_updated = true; + sm.update(0); + update(); +} + +void DriverViewScene::paintEvent(QPaintEvent* event) { + QPainter p(this); + + // startup msg + if (!frame_updated) { + p.setPen(QColor(0xff, 0xff, 0xff)); + p.setRenderHint(QPainter::TextAntialiasing); + configFont(p, "Inter", 100, "Bold"); + p.drawText(geometry(), Qt::AlignCenter, "camera starting"); + return; + } + + const int width = 4 * height() / 3; + const QRect rect2 = {rect().center().x() - width / 2, rect().top(), width, rect().height()}; + const QRect valid_rect = {is_rhd ? rect2.right() - rect2.height() / 2 : rect2.left(), rect2.top(), rect2.height() / 2, rect2.height()}; + + // blackout + const int blackout_x_r = valid_rect.right(); + const QRect& blackout_rect = Hardware::TICI() ? rect() : rect2; + const int blackout_w_r = blackout_rect.right() - valid_rect.right(); + const int blackout_x_l = blackout_rect.left(); + const int blackout_w_l = valid_rect.left() - blackout_x_l; + + QColor bg(0, 0, 0, 140); + p.setPen(QPen(bg)); + p.setBrush(QBrush(bg)); + p.drawRect(blackout_x_l, rect2.top(), blackout_w_l, rect2.height()); + p.drawRect(blackout_x_r, rect2.top(), blackout_w_r, rect2.height()); + + // face bounding box + cereal::DriverState::Reader driver_state = sm["driverState"].getDriverState(); + bool face_detected = driver_state.getFaceProb() > 0.4; + if (face_detected) { + auto fxy_list = driver_state.getFacePosition(); + float face_x = fxy_list[0]; + float face_y = fxy_list[1]; + int fbox_x = valid_rect.center().x() + (is_rhd ? face_x : -face_x) * valid_rect.width(); + int fbox_y = valid_rect.center().y() + face_y * valid_rect.height(); + + float alpha = 0.2; + face_x = std::abs(face_x); + face_y = std::abs(face_y); + if (face_x <= 0.35 && face_y <= 0.4) { + alpha = 0.8 - (face_x > face_y ? face_x : face_y) * 0.6 / 0.375; + } + + const int box_size = 0.6 * rect2.height() / 2; + QPen pen(QColor(255, 255, 255, alpha * 255)); + pen.setWidth(10); + p.setPen(pen); + p.setBrush(Qt::NoBrush); + p.drawRoundedRect(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size, 35.0, 35.0); + } + + // icon + const int img_offset = 30; + const int img_x = is_rhd ? rect2.right() - FACE_IMG_SIZE - img_offset : rect2.left() + img_offset; + const int img_y = rect2.bottom() - FACE_IMG_SIZE - img_offset; + p.setPen(Qt::NoPen); + p.setOpacity(face_detected ? 1.0 : 0.3); + p.drawImage(img_x, img_y, face); +} diff --git a/selfdrive/ui/qt/offroad/driverview.h b/selfdrive/ui/qt/offroad/driverview.h new file mode 100644 index 0000000000..002c8f19f0 --- /dev/null +++ b/selfdrive/ui/qt/offroad/driverview.h @@ -0,0 +1,48 @@ +#pragma once + +#include + +#include + +#include "selfdrive/common/util.h" +#include "selfdrive/ui/qt/widgets/cameraview.h" + +class DriverViewScene : public QWidget { + Q_OBJECT + +public: + explicit DriverViewScene(QWidget *parent); + +public slots: + void frameUpdated(); + +protected: + void showEvent(QShowEvent *event) override; + void hideEvent(QHideEvent *event) override; + void paintEvent(QPaintEvent *event) override; + +private: + Params params; + SubMaster sm; + QImage face; + bool is_rhd = false; + bool frame_updated = false; +}; + +class DriverViewWindow : public QWidget { + Q_OBJECT + +public: + explicit DriverViewWindow(QWidget *parent); + +signals: + void done(); + +protected: + void mousePressEvent(QMouseEvent* e) override; + +private: + CameraViewWidget *cameraView; + DriverViewScene *scene; + QStackedLayout *layout; +}; diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index b18c5ba4a9..175e11bb24 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -113,10 +113,7 @@ DevicePanel::DevicePanel(QWidget* parent) : QWidget(parent) { offroad_btns.append(new ButtonControl("Driver Camera", "PREVIEW", "Preview the driver facing camera to help optimize device mounting position for best driver monitoring experience. (vehicle must be off)", - [=]() { - Params().putBool("IsDriverViewEnabled", true); - QUIState::ui_state.scene.driver_view = true; - }, "", this)); + [=]() { emit showDriverView(); }, "", this)); QString resetCalibDesc = "openpilot requires the device to be mounted within 4° left or right and within 5° up or down. openpilot is continuously calibrating, resetting is rarely required."; ButtonControl *resetCalibBtn = new ButtonControl("Reset Calibration", "RESET", resetCalibDesc, [=]() { @@ -348,6 +345,7 @@ void SettingsWindow::showEvent(QShowEvent *event) { // setup panels DevicePanel *device = new DevicePanel(this); QObject::connect(device, &DevicePanel::reviewTrainingGuide, this, &SettingsWindow::reviewTrainingGuide); + QObject::connect(device, &DevicePanel::showDriverView, this, &SettingsWindow::showDriverView); QPair panels[] = { {"Device", device}, diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index 698f495634..ce524a19a7 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -21,6 +21,7 @@ public: explicit DevicePanel(QWidget* parent = nullptr); signals: void reviewTrainingGuide(); + void showDriverView(); }; class TogglesPanel : public QWidget { @@ -61,6 +62,7 @@ signals: void closeSettings(); void offroadTransition(bool offroad); void reviewTrainingGuide(); + void showDriverView(); private: QPushButton *sidebar_alert_widget; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 7d2711418e..1404379088 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -93,8 +93,7 @@ void OnroadAlerts::updateState(const UIState &s) { // TODO: add blinking back if performant //float alpha = 0.375 * cos((millis_since_boot() / 1000) * 2 * M_PI * blinking_rate) + 0.625; - auto c = bg_colors[s.status]; - bg.setRgbF(c.r, c.g, c.b, c.a); + bg = bg_colors[s.status]; } void OnroadAlerts::offroadTransition(bool offroad) { @@ -227,7 +226,7 @@ void NvgWindow::paintGL() { double cur_draw_t = millis_since_boot(); double dt = cur_draw_t - prev_draw_t; - if (dt > 66 && !QUIState::ui_state.scene.driver_view) { + if (dt > 66) { // warn on sub 15fps LOGW("slow frame time: %.2f", dt); } diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc new file mode 100644 index 0000000000..e8909c95b7 --- /dev/null +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -0,0 +1,217 @@ +#include "selfdrive/ui/qt/widgets/cameraview.h" + +#include "selfdrive/ui/qt/qt_window.h" + +namespace { +const char frame_vertex_shader[] = +#ifdef NANOVG_GL3_IMPLEMENTATION + "#version 150 core\n" +#else + "#version 300 es\n" +#endif + "in vec4 aPosition;\n" + "in vec4 aTexCoord;\n" + "uniform mat4 uTransform;\n" + "out vec4 vTexCoord;\n" + "void main() {\n" + " gl_Position = uTransform * aPosition;\n" + " vTexCoord = aTexCoord;\n" + "}\n"; + +const char frame_fragment_shader[] = +#ifdef NANOVG_GL3_IMPLEMENTATION + "#version 150 core\n" +#else + "#version 300 es\n" +#endif + "precision mediump float;\n" + "uniform sampler2D uTexture;\n" + "in vec4 vTexCoord;\n" + "out vec4 colorOut;\n" + "void main() {\n" + " colorOut = texture(uTexture, vTexCoord.xy);\n" +#ifdef QCOM + " vec3 dz = vec3(0.0627f, 0.0627f, 0.0627f);\n" + " colorOut.rgb = ((vec3(1.0f, 1.0f, 1.0f) - dz) * colorOut.rgb / vec3(1.0f, 1.0f, 1.0f)) + dz;\n" +#endif + "}\n"; + +const mat4 device_transform = {{ + 1.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, +}}; + +mat4 get_driver_view_transform() { + const float driver_view_ratio = 1.333; + mat4 transform; + if (Hardware::TICI()) { + // from dmonitoring.cc + const int full_width_tici = 1928; + const int full_height_tici = 1208; + const int adapt_width_tici = 668; + const int crop_x_offset = 32; + const int crop_y_offset = -196; + const float yscale = full_height_tici * driver_view_ratio / adapt_width_tici; + const float xscale = yscale*(1080)/(2160)*full_width_tici/full_height_tici; + transform = (mat4){{ + xscale, 0.0, 0.0, xscale*crop_x_offset/full_width_tici*2, + 0.0, yscale, 0.0, yscale*crop_y_offset/full_height_tici*2, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, + }}; + } else { + // frame from 4/3 to 16/9 display + transform = (mat4){{ + driver_view_ratio*(1080)/(1920), 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, + }}; + } + return transform; +} + +} // namespace + +CameraViewWidget::CameraViewWidget(VisionStreamType stream_type, QWidget* parent) : stream_type(stream_type), QOpenGLWidget(parent) { + setAttribute(Qt::WA_OpaquePaintEvent); + + timer = new QTimer(this); + connect(timer, &QTimer::timeout, this, &CameraViewWidget::updateFrame); +} + +CameraViewWidget::~CameraViewWidget() { + makeCurrent(); + doneCurrent(); + glDeleteVertexArrays(1, &frame_vao); + glDeleteBuffers(1, &frame_vbo); + glDeleteBuffers(1, &frame_ibo); +} + +void CameraViewWidget::initializeGL() { + initializeOpenGLFunctions(); + + gl_shader = std::make_unique(frame_vertex_shader, frame_fragment_shader); + GLint frame_pos_loc = glGetAttribLocation(gl_shader->prog, "aPosition"); + GLint frame_texcoord_loc = glGetAttribLocation(gl_shader->prog, "aTexCoord"); + + auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_RGB_FRONT ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f); + const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3}; + const float frame_coords[4][4] = { + {-1.0, -1.0, x2, y1}, //bl + {-1.0, 1.0, x2, y2}, //tl + { 1.0, 1.0, x1, y2}, //tr + { 1.0, -1.0, x1, y1}, //br + }; + + glGenVertexArrays(1, &frame_vao); + glBindVertexArray(frame_vao); + glGenBuffers(1, &frame_vbo); + glBindBuffer(GL_ARRAY_BUFFER, frame_vbo); + glBufferData(GL_ARRAY_BUFFER, sizeof(frame_coords), frame_coords, GL_STATIC_DRAW); + glEnableVertexAttribArray(frame_pos_loc); + glVertexAttribPointer(frame_pos_loc, 2, GL_FLOAT, GL_FALSE, + sizeof(frame_coords[0]), (const void *)0); + glEnableVertexAttribArray(frame_texcoord_loc); + glVertexAttribPointer(frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE, + sizeof(frame_coords[0]), (const void *)(sizeof(float) * 2)); + glGenBuffers(1, &frame_ibo); + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, frame_ibo); + glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(frame_indicies), frame_indicies, GL_STATIC_DRAW); + glBindBuffer(GL_ARRAY_BUFFER, 0); + glBindVertexArray(0); + + if (stream_type == VISION_STREAM_RGB_FRONT) { + frame_mat = matmul(device_transform, get_driver_view_transform()); + } else { + auto intrinsic_matrix = stream_type == VISION_STREAM_RGB_WIDE ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; + float zoom_ = zoom / intrinsic_matrix.v[0]; + if (stream_type == VISION_STREAM_RGB_WIDE) { + zoom_ *= 0.5; + } + float zx = zoom_ * 2 * intrinsic_matrix.v[2] / width(); + float zy = zoom_ * 2 * intrinsic_matrix.v[5] / height(); + + const mat4 frame_transform = {{ + zx, 0.0, 0.0, 0.0, + 0.0, zy, 0.0, -y_offset / height() * 2, + 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 1.0, + }}; + frame_mat = matmul(device_transform, frame_transform); + } + vipc_client = std::make_unique("camerad", stream_type, true); +} + +void CameraViewWidget::showEvent(QShowEvent *event) { + timer->start(0); +} + +void CameraViewWidget::hideEvent(QHideEvent *event) { + timer->stop(); + vipc_client->connected = false; + latest_frame = nullptr; +} + +void CameraViewWidget::paintGL() { + if (!latest_frame) { + glClearColor(0, 0, 0, 1.0); + glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); + return; + } + + glViewport(0, 0, width(), height()); + + glBindVertexArray(frame_vao); + glActiveTexture(GL_TEXTURE0); + + glBindTexture(GL_TEXTURE_2D, texture[latest_frame->idx]->frame_tex); + if (!Hardware::EON()) { + // this is handled in ion on QCOM + glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, latest_frame->width, latest_frame->height, + 0, GL_RGB, GL_UNSIGNED_BYTE, latest_frame->addr); + } + + glUseProgram(gl_shader->prog); + glUniform1i(gl_shader->getUniformLocation("uTexture"), 0); + glUniformMatrix4fv(gl_shader->getUniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v); + + assert(glGetError() == GL_NO_ERROR); + glEnableVertexAttribArray(0); + glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0); + glDisableVertexAttribArray(0); + glBindVertexArray(0); +} + +void CameraViewWidget::updateFrame() { + if (!vipc_client->connected && vipc_client->connect(false)) { + // init vision + for (int i = 0; i < vipc_client->num_buffers; i++) { + texture[i].reset(new EGLImageTexture(&vipc_client->buffers[i])); + + glBindTexture(GL_TEXTURE_2D, texture[i]->frame_tex); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + + // BGR + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED); + assert(glGetError() == GL_NO_ERROR); + } + latest_frame = nullptr; + } + + if (vipc_client->connected) { + VisionBuf *buf = vipc_client->recv(); + if (buf != nullptr) { + latest_frame = buf; + update(); + emit frameUpdated(); + } else { + LOGE("visionIPC receive timeout"); + } + } +} diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h new file mode 100644 index 0000000000..91ff9befb1 --- /dev/null +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -0,0 +1,44 @@ +#pragma once + +#include + +#include +#include + +#include "cereal/visionipc/visionipc_client.h" +#include "selfdrive/common/glutil.h" +#include "selfdrive/common/mat.h" +#include "selfdrive/common/visionimg.h" +#include "selfdrive/ui/ui.h" + +class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions { +Q_OBJECT + +public: + using QOpenGLWidget::QOpenGLWidget; + explicit CameraViewWidget(VisionStreamType stream_type, QWidget* parent = nullptr); + ~CameraViewWidget(); + +signals: + void frameUpdated(); + +protected: + void paintGL() override; + void initializeGL() override; + void showEvent(QShowEvent *event) override; + void hideEvent(QHideEvent *event) override; + +protected slots: + void updateFrame(); + +private: + VisionBuf *latest_frame = nullptr; + GLuint frame_vao, frame_vbo, frame_ibo; + mat4 frame_mat; + std::unique_ptr vipc_client; + std::unique_ptr texture[UI_BUF_COUNT]; + std::unique_ptr gl_shader; + + VisionStreamType stream_type; + QTimer* timer; +}; diff --git a/selfdrive/ui/qt/window.cc b/selfdrive/ui/qt/window.cc index 19844afd41..fe7539616d 100644 --- a/selfdrive/ui/qt/window.cc +++ b/selfdrive/ui/qt/window.cc @@ -20,6 +20,9 @@ MainWindow::MainWindow(QWidget *parent) : QWidget(parent) { QObject::connect(settingsWindow, &SettingsWindow::closeSettings, this, &MainWindow::closeSettings); QObject::connect(&qs, &QUIState::offroadTransition, settingsWindow, &SettingsWindow::offroadTransition); QObject::connect(settingsWindow, &SettingsWindow::reviewTrainingGuide, this, &MainWindow::reviewTrainingGuide); + QObject::connect(settingsWindow, &SettingsWindow::showDriverView, [=] { + homeWindow->showDriverView(true); + }); onboardingWindow = new OnboardingWindow(this); onboardingDone = onboardingWindow->isOnboardingDone(); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 43948d5fba..caf64c0958 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -203,7 +203,7 @@ static void update_state(UIState *s) { scene.light_sensor = std::clamp((1023.0 / max_lines) * (max_lines - camera_state.getIntegLines() * gain), 0.0, 1023.0); } - scene.started = sm["deviceState"].getDeviceState().getStarted() || scene.driver_view; + scene.started = sm["deviceState"].getDeviceState().getStarted(); } static void update_params(UIState *s) { @@ -251,7 +251,6 @@ static void update_status(UIState *s) { s->status = STATUS_DISENGAGED; s->scene.started_frame = s->sm->frame; - s->scene.is_rhd = Params().getBool("IsRHD"); s->scene.end_to_end = Params().getBool("EndToEndToggle"); s->wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false; @@ -259,9 +258,7 @@ static void update_status(UIState *s) { ui_resize(s, s->fb_w, s->fb_h); // Choose vision ipc client - if (s->scene.driver_view) { - s->vipc_client = s->vipc_client_front; - } else if (s->wide_camera){ + if (s->wide_camera){ s->vipc_client = s->vipc_client_wide; } else { s->vipc_client = s->vipc_client_rear; @@ -277,7 +274,7 @@ static void update_status(UIState *s) { QUIState::QUIState(QObject *parent) : QObject(parent) { ui_state.sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "liveLocationKalman", - "pandaState", "carParams", "driverState", "driverMonitoringState", "sensorEvents", "carState", "ubloxGnss", + "pandaState", "carParams", "driverMonitoringState", "sensorEvents", "carState", "ubloxGnss", "gpsLocationExternal", "roadCameraState", }); @@ -288,7 +285,6 @@ QUIState::QUIState(QObject *parent) : QObject(parent) { ui_state.wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false; ui_state.vipc_client_rear = new VisionIpcClient("camerad", VISION_STREAM_RGB_BACK, true); - ui_state.vipc_client_front = new VisionIpcClient("camerad", VISION_STREAM_RGB_FRONT, true); ui_state.vipc_client_wide = new VisionIpcClient("camerad", VISION_STREAM_RGB_WIDE, true); ui_state.vipc_client = ui_state.vipc_client_rear; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 763f3aaf8c..8be67ae2f2 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -7,6 +7,7 @@ #include #include +#include #include "nanovg.h" @@ -30,6 +31,11 @@ #define COLOR_YELLOW nvgRGBA(218, 202, 37, 255) #define COLOR_RED nvgRGBA(201, 34, 49, 255) +// TODO: this is also hardcoded in common/transformations/camera.py +// TODO: choose based on frame input size +const float y_offset = Hardware::TICI() ? 150.0 : 0.0; +const float zoom = Hardware::TICI() ? 2912.8 : 2138.5; + typedef struct Rect { int x, y, w, h; int centerX() const { return x + w / 2; } @@ -54,11 +60,11 @@ typedef enum UIStatus { STATUS_ALERT, } UIStatus; -static std::map bg_colors = { - {STATUS_DISENGAGED, nvgRGBA(0x17, 0x33, 0x49, 0xc8)}, - {STATUS_ENGAGED, nvgRGBA(0x17, 0x86, 0x44, 0xf1)}, - {STATUS_WARNING, nvgRGBA(0xDA, 0x6F, 0x25, 0xf1)}, - {STATUS_ALERT, nvgRGBA(0xC9, 0x22, 0x31, 0xf1)}, +static QColor bg_colors [] = { + [STATUS_DISENGAGED] = QColor(0x17, 0x33, 0x49, 0xc8), + [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), + [STATUS_WARNING] = QColor(0xDA, 0x6F, 0x25, 0xf1), + [STATUS_ALERT] = QColor(0xC9, 0x22, 0x31, 0xf1), }; typedef struct { @@ -75,9 +81,6 @@ typedef struct UIScene { mat3 view_from_calib; bool world_objects_visible; - bool is_rhd; - bool driver_view; - cereal::PandaState::PandaType pandaType; // gps @@ -101,7 +104,6 @@ typedef struct UIScene { typedef struct UIState { VisionIpcClient * vipc_client; - VisionIpcClient * vipc_client_front; VisionIpcClient * vipc_client_rear; VisionIpcClient * vipc_client_wide; VisionBuf * last_frame; @@ -124,8 +126,8 @@ typedef struct UIState { std::unique_ptr gl_shader; std::unique_ptr texture[UI_BUF_COUNT]; - GLuint frame_vao[2], frame_vbo[2], frame_ibo[2]; - mat4 rear_frame_mat, front_frame_mat; + GLuint frame_vao, frame_vbo, frame_ibo; + mat4 rear_frame_mat; bool awake;