diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc index 00f7415cbabad0..5dfd5326f12efa 100644 --- a/selfdrive/ui/qt/offroad/driverview.cc +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -4,6 +4,7 @@ #include #include "selfdrive/ui/qt/util.h" +#include "selfdrive/ui/ui.h" const int FACE_IMG_SIZE = 130; @@ -25,17 +26,14 @@ void DriverViewWindow::showEvent(QShowEvent* event) { void DriverViewWindow::hideEvent(QHideEvent* event) { params.putBool("IsDriverViewEnabled", false); - stopVipcThread(); CameraWidget::hideEvent(event); } void DriverViewWindow::paintGL() { CameraWidget::paintGL(); - std::lock_guard lk(frame_lock); QPainter p(this); - // startup msg - if (frames.empty()) { + if (!connected()) { p.setPen(Qt::white); p.setRenderHint(QPainter::TextAntialiasing); p.setFont(InterFont(100, QFont::Bold)); diff --git a/selfdrive/ui/qt/offroad/driverview.h b/selfdrive/ui/qt/offroad/driverview.h index 00acb68d29c76f..83c26cc2eb1c6c 100644 --- a/selfdrive/ui/qt/offroad/driverview.h +++ b/selfdrive/ui/qt/offroad/driverview.h @@ -1,5 +1,6 @@ #pragma once +#include "common/params.h" #include "selfdrive/ui/qt/widgets/cameraview.h" class DriverViewWindow : public CameraWidget { diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index f504ad69f1e2c2..9c90178782255c 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -22,9 +22,10 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget *par } void AnnotatedCameraWidget::updateState(const UIState &s) { - // update engageability/experimental mode button experimental_btn->updateState(s); dmon.updateState(s); + + update(); } void AnnotatedCameraWidget::initializeGL() { @@ -46,7 +47,7 @@ mat4 AnnotatedCameraWidget::calcFrameMatrix() { // Select intrinsic matrix and calibration based on camera type auto *s = uiState(); - bool wide_cam = active_stream_type == VISION_STREAM_WIDE_ROAD; + bool wide_cam = getStreamType() == VISION_STREAM_WIDE_ROAD; const auto &intrinsic_matrix = wide_cam ? ECAM_INTRINSIC_MATRIX : FCAM_INTRINSIC_MATRIX; const auto &calibration = wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib; @@ -94,21 +95,6 @@ void AnnotatedCameraWidget::paintGL() { // draw camera frame { - std::lock_guard lk(frame_lock); - - if (frames.empty()) { - if (skip_frame_count > 0) { - skip_frame_count--; - qDebug() << "skipping frame, not ready"; - return; - } - } else { - // skip drawing up to this many frames if we're - // missing camera frames. this smooths out the - // transitions from the narrow and wide cameras - skip_frame_count = 5; - } - // Wide or narrow cam dependent on speed bool has_wide_cam = available_streams.count(VISION_STREAM_WIDE_ROAD); if (has_wide_cam) { @@ -121,7 +107,6 @@ void AnnotatedCameraWidget::paintGL() { wide_cam_requested = wide_cam_requested && sm["selfdriveState"].getSelfdriveState().getExperimentalMode(); } CameraWidget::setStreamType(wide_cam_requested ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); - CameraWidget::setFrameId(sm["modelV2"].getModelV2().getFrameId()); CameraWidget::paintGL(); } diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index d205579f6c9059..a15643fc87bbeb 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -22,8 +22,6 @@ class AnnotatedCameraWidget : public CameraWidget { HudRenderer hud; ModelRenderer model; std::unique_ptr pm; - - int skip_frame_count = 0; bool wide_cam_requested = false; protected: diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 674e5e999cb2cd..7ebf86c124c76e 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -6,9 +6,6 @@ #include #endif -#include -#include - namespace { const char frame_vertex_shader[] = @@ -62,19 +59,14 @@ const char frame_fragment_shader[] = } // namespace -CameraWidget::CameraWidget(std::string stream_name, VisionStreamType type, QWidget* parent) : - stream_name(stream_name), active_stream_type(type), requested_stream_type(type), QOpenGLWidget(parent) { - setAttribute(Qt::WA_OpaquePaintEvent); +CameraWidget::CameraWidget(const std::string &stream_name, VisionStreamType type, QWidget *parent) + : stream_name(stream_name), QOpenGLWidget(parent) { qRegisterMetaType>("availableStreams"); - QObject::connect(this, &CameraWidget::vipcThreadConnected, this, &CameraWidget::vipcConnected, Qt::BlockingQueuedConnection); - QObject::connect(this, &CameraWidget::vipcThreadFrameReceived, this, &CameraWidget::vipcFrameReceived, Qt::QueuedConnection); - QObject::connect(this, &CameraWidget::vipcAvailableStreamsUpdated, this, &CameraWidget::availableStreamsUpdated, Qt::QueuedConnection); - QObject::connect(QApplication::instance(), &QCoreApplication::aboutToQuit, this, &CameraWidget::stopVipcThread); + vipc_client = std::make_unique(stream_name, type, false); } CameraWidget::~CameraWidget() { makeCurrent(); - stopVipcThread(); if (isValid()) { glDeleteVertexArrays(1, &frame_vao); glDeleteBuffers(1, &frame_vbo); @@ -82,16 +74,17 @@ CameraWidget::~CameraWidget() { glDeleteTextures(2, textures); } doneCurrent(); + clearEGLImages(); } -// Qt uses device-independent pixels, depending on platform this may be -// different to what OpenGL uses -int CameraWidget::glWidth() { - return width() * devicePixelRatio(); -} - -int CameraWidget::glHeight() { - return height() * devicePixelRatio(); +void CameraWidget::clearEGLImages() { +#ifdef QCOM2 + EGLDisplay egl_display = eglGetCurrentDisplay(); + for (auto &pair : egl_images) { + eglDestroyImageKHR(egl_display, pair.second); + } + egl_images.clear(); +#endif } void CameraWidget::initializeGL() { @@ -107,7 +100,7 @@ void CameraWidget::initializeGL() { GLint frame_pos_loc = program->attributeLocation("aPosition"); GLint frame_texcoord_loc = program->attributeLocation("aTexCoord"); - auto [x1, x2, y1, y2] = requested_stream_type == VISION_STREAM_DRIVER ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f); + auto [x1, x2, y1, y2] = vipc_client->type == VISION_STREAM_DRIVER ? 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 @@ -144,40 +137,6 @@ void CameraWidget::initializeGL() { #endif } -void CameraWidget::showEvent(QShowEvent *event) { - if (!vipc_thread) { - clearFrames(); - vipc_thread = new QThread(); - connect(vipc_thread, &QThread::started, [=]() { vipcThread(); }); - connect(vipc_thread, &QThread::finished, vipc_thread, &QObject::deleteLater); - vipc_thread->start(); - } -} - -void CameraWidget::stopVipcThread() { - makeCurrent(); - if (vipc_thread) { - vipc_thread->requestInterruption(); - vipc_thread->quit(); - vipc_thread->wait(); - vipc_thread = nullptr; - } - -#ifdef QCOM2 - EGLDisplay egl_display = eglGetCurrentDisplay(); - assert(egl_display != EGL_NO_DISPLAY); - for (auto &pair : egl_images) { - eglDestroyImageKHR(egl_display, pair.second); - assert(eglGetError() == EGL_SUCCESS); - } - egl_images.clear(); -#endif -} - -void CameraWidget::availableStreamsUpdated(std::set streams) { - available_streams = streams; -} - mat4 CameraWidget::calcFrameMatrix() { // Scale the frame to fit the widget while maintaining the aspect ratio. float widget_aspect_ratio = (float)width() / height(); @@ -197,27 +156,9 @@ void CameraWidget::paintGL() { glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF()); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); - std::lock_guard lk(frame_lock); - if (frames.empty()) return; - - int frame_idx = frames.size() - 1; - - // Always draw latest frame until sync logic is more stable - // for (frame_idx = 0; frame_idx < frames.size() - 1; frame_idx++) { - // if (frames[frame_idx].first == draw_frame_id) break; - // } - - // Log duplicate/dropped frames - if (frames[frame_idx].first == prev_frame_id) { - qDebug() << "Drawing same frame twice" << frames[frame_idx].first; - } else if (frames[frame_idx].first != prev_frame_id + 1) { - qDebug() << "Skipped frame" << frames[frame_idx].first; - } - prev_frame_id = frames[frame_idx].first; - VisionBuf *frame = frames[frame_idx].second; - assert(frame != nullptr); - auto frame_mat = calcFrameMatrix(); + auto frame = receiveFrame(); + if (!frame) return; glViewport(0, 0, glWidth(), glHeight()); glBindVertexArray(frame_vao); @@ -230,7 +171,6 @@ void CameraWidget::paintGL() { glEGLImageTargetTexture2DOES(GL_TEXTURE_EXTERNAL_OES, egl_images[frame->idx]); assert(glGetError() == GL_NO_ERROR); #else - // fallback to copy glPixelStorei(GL_UNPACK_ROW_LENGTH, stream_stride); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, textures[0]); @@ -255,20 +195,15 @@ void CameraWidget::paintGL() { glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); } -void CameraWidget::vipcConnected(VisionIpcClient *vipc_client) { +void CameraWidget::vipcConnected() { makeCurrent(); stream_width = vipc_client->buffers[0].width; stream_height = vipc_client->buffers[0].height; stream_stride = vipc_client->buffers[0].stride; #ifdef QCOM2 + clearEGLImages(); EGLDisplay egl_display = eglGetCurrentDisplay(); - assert(egl_display != EGL_NO_DISPLAY); - for (auto &pair : egl_images) { - eglDestroyImageKHR(egl_display, pair.second); - } - egl_images.clear(); - for (int i = 0; i < vipc_client->num_buffers; i++) { // import buffers into OpenGL int fd = dup(vipc_client->buffers[i].fd); // eglDestroyImageKHR will close, so duplicate EGLint img_attrs[] = { @@ -305,59 +240,26 @@ void CameraWidget::vipcConnected(VisionIpcClient *vipc_client) { #endif } -void CameraWidget::vipcFrameReceived() { - update(); +void CameraWidget::setStreamType(VisionStreamType type) { + if (type != vipc_client->type) { + qDebug().nospace() << "connecting to stream " << type; + vipc_client.reset(new VisionIpcClient(stream_name, type, false)); + } } -void CameraWidget::vipcThread() { - VisionStreamType cur_stream = requested_stream_type; - std::unique_ptr vipc_client; - VisionIpcBufExtra meta_main = {0}; - - while (!QThread::currentThread()->isInterruptionRequested()) { - if (!vipc_client || cur_stream != requested_stream_type) { - clearFrames(); - qDebug().nospace() << "connecting to stream " << requested_stream_type << ", was connected to " << cur_stream; - cur_stream = requested_stream_type; - vipc_client.reset(new VisionIpcClient(stream_name, cur_stream, false)); - } - active_stream_type = cur_stream; - - if (!vipc_client->connected) { - clearFrames(); - auto streams = VisionIpcClient::getAvailableStreams(stream_name, false); - if (streams.empty()) { - QThread::msleep(100); - continue; - } - emit vipcAvailableStreamsUpdated(streams); - - if (!vipc_client->connect(false)) { - QThread::msleep(100); - continue; - } - emit vipcThreadConnected(vipc_client.get()); - } - - if (VisionBuf *buf = vipc_client->recv(&meta_main, 1000)) { - { - std::lock_guard lk(frame_lock); - frames.push_back(std::make_pair(meta_main.frame_id, buf)); - while (frames.size() > FRAME_BUFFER_SIZE) { - frames.pop_front(); - } - } - emit vipcThreadFrameReceived(); - } else { - if (!isVisible()) { - vipc_client->connected = false; - } +VisionBuf *CameraWidget::receiveFrame() { + if (!vipc_client->connected) { + vision_buf = nullptr; + available_streams = VisionIpcClient::getAvailableStreams(stream_name, false); + if (available_streams.empty() || !vipc_client->connect(false)) { + return nullptr; } + emit vipcAvailableStreamsUpdated(available_streams); + vipcConnected(); } -} -void CameraWidget::clearFrames() { - std::lock_guard lk(frame_lock); - frames.clear(); - available_streams.clear(); + if (auto frame = vipc_client->recv(nullptr, 0)) { + vision_buf = frame; + } + return vision_buf; } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 29aa8493c72dfa..9fffed1f0250fd 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -1,17 +1,13 @@ #pragma once -#include #include #include -#include #include #include -#include #include #include #include -#include #ifdef QCOM2 #define EGL_EGLEXT_PROTOTYPES @@ -23,46 +19,38 @@ #endif #include "msgq/visionipc/visionipc_client.h" -#include "selfdrive/ui/ui.h" - -const int FRAME_BUFFER_SIZE = 5; +#include "common/mat.h" class CameraWidget : public QOpenGLWidget, protected QOpenGLFunctions { Q_OBJECT public: - using QOpenGLWidget::QOpenGLWidget; - explicit CameraWidget(std::string stream_name, VisionStreamType stream_type, QWidget* parent = nullptr); + explicit CameraWidget(const std::string &stream_name, VisionStreamType stream_type, QWidget* parent = nullptr); ~CameraWidget(); void setBackgroundColor(const QColor &color) { bg = color; } - void setFrameId(int frame_id) { draw_frame_id = frame_id; } - void setStreamType(VisionStreamType type) { requested_stream_type = type; } - VisionStreamType getStreamType() { return active_stream_type; } - void stopVipcThread(); + void setStreamType(VisionStreamType type); + VisionStreamType getStreamType() const { return vipc_client->type; } + bool connected() const { return vipc_client->connected; } signals: void clicked(); - void vipcThreadConnected(VisionIpcClient *); - void vipcThreadFrameReceived(); void vipcAvailableStreamsUpdated(std::set); protected: void paintGL() override; void initializeGL() override; - void showEvent(QShowEvent *event) override; void mouseReleaseEvent(QMouseEvent *event) override { emit clicked(); } virtual mat4 calcFrameMatrix(); - void vipcThread(); - void clearFrames(); - - int glWidth(); - int glHeight(); + void vipcConnected(); + VisionBuf *receiveFrame(); + void clearEGLImages(); + int glWidth() { return width() * devicePixelRatio(); } + int glHeight() { return height() * devicePixelRatio(); } GLuint frame_vao, frame_vbo, frame_ibo; GLuint textures[2]; std::unique_ptr program; QColor bg = QColor("#000000"); - #ifdef QCOM2 std::map egl_images; #endif @@ -71,19 +59,7 @@ class CameraWidget : public QOpenGLWidget, protected QOpenGLFunctions { int stream_width = 0; int stream_height = 0; int stream_stride = 0; - std::atomic active_stream_type; - std::atomic requested_stream_type; std::set available_streams; - QThread *vipc_thread = nullptr; - std::recursive_mutex frame_lock; - std::deque> frames; - uint32_t draw_frame_id = 0; - uint32_t prev_frame_id = 0; - -protected slots: - void vipcConnected(VisionIpcClient *vipc_client); - void vipcFrameReceived(); - void availableStreamsUpdated(std::set streams); + std::unique_ptr vipc_client; + VisionBuf *vision_buf = nullptr; }; - -Q_DECLARE_METATYPE(std::set); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index b6ce6cb02e20f6..d774ca39f64d82 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -15,7 +15,8 @@ #define BACKLIGHT_TS 10.00 static void update_sockets(UIState *s) { - s->sm->update(0); + // Wait for 10 ms to ensure modelV2 is updated when onroad. + s->sm->update(s->scene.started ? 10 : 0); } static void update_state(UIState *s) { @@ -94,7 +95,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2", "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan", - }); + }, std::vector{"modelV2"}); prime_state = new PrimeState(this); language = QString::fromStdString(Params().get("LanguageSetting")); diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index 66a6efe5697555..91085fbb6b650a 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -234,6 +234,7 @@ void VideoWidget::updateState() { } else { time_btn->setText(formatTime(can->currentSec(), true)); } + cam_widget->update(); } void VideoWidget::updatePlayBtnState() {