From 7243b9b68e9d83459547f0fd76a97fa58cfae36e Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 7 Jan 2024 14:54:39 -0800 Subject: [PATCH] Revert "UI: single-threaded CameraView (#30397)" This reverts commit 69dcd240fcf3bac7a6bddb1647219b710d255554. old-commit-hash: 073fc89ad49461c01ea474125a89a505543d093d --- selfdrive/ui/qt/offroad/driverview.cc | 13 +- selfdrive/ui/qt/offroad/driverview.h | 2 +- selfdrive/ui/qt/onroad.cc | 30 ++-- selfdrive/ui/qt/onroad.h | 2 + selfdrive/ui/qt/widgets/cameraview.cc | 189 ++++++++++++++++---------- selfdrive/ui/qt/widgets/cameraview.h | 49 +++---- selfdrive/ui/watch3.cc | 8 +- tools/cabana/videowidget.cc | 5 +- tools/cabana/videowidget.h | 4 +- 9 files changed, 178 insertions(+), 124 deletions(-) diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc index 08f6f10642..df9bb24651 100644 --- a/selfdrive/ui/qt/offroad/driverview.cc +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -7,7 +7,7 @@ const int FACE_IMG_SIZE = 130; -DriverViewWindow::DriverViewWindow(QWidget* parent) : CameraView("camerad", VISION_STREAM_DRIVER, true, parent) { +DriverViewWindow::DriverViewWindow(QWidget* parent) : CameraWidget("camerad", VISION_STREAM_DRIVER, true, parent) { face_img = loadPixmap("../assets/img_driver_face_static.png", {FACE_IMG_SIZE, FACE_IMG_SIZE}); QObject::connect(this, &CameraWidget::clicked, this, &DriverViewWindow::done); QObject::connect(device(), &Device::interactiveTimeout, this, [this]() { @@ -20,20 +20,22 @@ DriverViewWindow::DriverViewWindow(QWidget* parent) : CameraView("camerad", VISI void DriverViewWindow::showEvent(QShowEvent* event) { params.putBool("IsDriverViewEnabled", true); device()->resetInteractiveTimeout(60); - CameraView::showEvent(event); + CameraWidget::showEvent(event); } void DriverViewWindow::hideEvent(QHideEvent* event) { params.putBool("IsDriverViewEnabled", false); - CameraView::hideEvent(event); + stopVipcThread(); + CameraWidget::hideEvent(event); } void DriverViewWindow::paintGL() { - CameraView::paintGL(); + CameraWidget::paintGL(); + std::lock_guard lk(frame_lock); QPainter p(this); // startup msg - if (!frame) { + if (frames.empty()) { p.setPen(Qt::white); p.setRenderHint(QPainter::TextAntialiasing); p.setFont(InterFont(100, QFont::Bold)); @@ -45,6 +47,7 @@ void DriverViewWindow::paintGL() { cereal::DriverStateV2::Reader driver_state = sm["driverStateV2"].getDriverStateV2(); bool is_rhd = driver_state.getWheelOnRightProb() > 0.5; auto driver_data = is_rhd ? driver_state.getRightDriverData() : driver_state.getLeftDriverData(); + bool face_detected = driver_data.getFaceProb() > 0.7; if (face_detected) { auto fxy_list = driver_data.getFacePosition(); diff --git a/selfdrive/ui/qt/offroad/driverview.h b/selfdrive/ui/qt/offroad/driverview.h index 97dd2faa78..155e4ede32 100644 --- a/selfdrive/ui/qt/offroad/driverview.h +++ b/selfdrive/ui/qt/offroad/driverview.h @@ -2,7 +2,7 @@ #include "selfdrive/ui/qt/widgets/cameraview.h" -class DriverViewWindow : public CameraView { +class DriverViewWindow : public CameraWidget { Q_OBJECT public: diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index ed4bdc15fb..4df0ed81cb 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -44,12 +44,12 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) { split->addWidget(nvg); if (getenv("DUAL_CAMERA_VIEW")) { - CameraWidget *arCam = new CameraView("camerad", VISION_STREAM_ROAD, true, this); + CameraWidget *arCam = new CameraWidget("camerad", VISION_STREAM_ROAD, true, this); split->insertWidget(0, arCam); } if (getenv("MAP_RENDER_VIEW")) { - CameraWidget *map_render = new CameraView("navd", VISION_STREAM_MAP, false, this); + CameraWidget *map_render = new CameraWidget("navd", VISION_STREAM_MAP, false, this); split->insertWidget(0, map_render); } @@ -126,7 +126,6 @@ void OnroadWindow::offroadTransition(bool offroad) { #endif alerts->updateAlert({}); - nvg->disconnectVipc(); } void OnroadWindow::primeChanged(bool prime) { @@ -329,8 +328,6 @@ 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(); } void AnnotatedCameraWidget::drawHud(QPainter &p) { @@ -612,6 +609,21 @@ 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) { @@ -627,18 +639,14 @@ void AnnotatedCameraWidget::paintGL() { } CameraWidget::setStreamType(wide_cam_requested ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); - s->scene.wide_cam = CameraWidget::streamType() == VISION_STREAM_WIDE_ROAD; + s->scene.wide_cam = CameraWidget::getStreamType() == VISION_STREAM_WIDE_ROAD; if (s->scene.calibration_valid) { auto calib = s->scene.wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib; CameraWidget::updateCalibration(calib); } else { CameraWidget::updateCalibration(DEFAULT_CALIBRATION); } - - if (!CameraWidget::receiveFrame(sm["uiPlan"].getUiPlan().getFrameId())) { - qDebug() << "skipping frame, not ready"; - return; - } + CameraWidget::setFrameId(model.getFrameId()); CameraWidget::paintGL(); } diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index f6b80a75de..b3ba411453 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -93,6 +93,8 @@ private: bool v_ego_cluster_seen = false; int status = STATUS_DISENGAGED; 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 4d370b8cc5..7b1f2f1d24 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -6,6 +6,14 @@ #include #endif +#include +#include +#include +#include + +#include +#include + namespace { const char frame_vertex_shader[] = @@ -87,25 +95,24 @@ mat4 get_fit_view_transform(float widget_aspect_ratio, float frame_aspect_ratio) } // namespace -CameraWidget::CameraWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget *parent) - : stream_name(stream_name), requested_stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { +CameraWidget::CameraWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget* parent) : + stream_name(stream_name), requested_stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) { + setAttribute(Qt::WA_OpaquePaintEvent); + 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); } CameraWidget::~CameraWidget() { makeCurrent(); + stopVipcThread(); if (isValid()) { glDeleteVertexArrays(1, &frame_vao); glDeleteBuffers(1, &frame_vbo); glDeleteBuffers(1, &frame_ibo); glDeleteBuffers(2, textures); } -#ifdef QCOM2 - EGLDisplay egl_display = eglGetCurrentDisplay(); - for (auto &pair : egl_images) { - eglDestroyImageKHR(egl_display, pair.second); - } - egl_images.clear(); -#endif doneCurrent(); } @@ -169,11 +176,45 @@ 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; +} + void CameraWidget::updateFrameMat() { int w = glWidth(), h = glHeight(); if (zoomed_view) { - if (streamType() == VISION_STREAM_DRIVER) { + if (active_stream_type == VISION_STREAM_DRIVER) { if (stream_width > 0 && stream_height > 0) { frame_mat = get_driver_view_transform(w, h, stream_width, stream_height); } @@ -182,7 +223,7 @@ void CameraWidget::updateFrameMat() { // to ensure this ends up in the middle of the screen // for narrow come and a little lower for wide cam. // TODO: use proper perspective transform? - if (streamType() == VISION_STREAM_WIDE_ROAD) { + if (active_stream_type == VISION_STREAM_WIDE_ROAD) { intrinsic_matrix = ECAM_INTRINSIC_MATRIX; zoom = 2.0; } else { @@ -228,15 +269,25 @@ void CameraWidget::paintGL() { glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF()); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); - if (!frame) return; + 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 (frame_id == prev_frame_id) { - qDebug() << "Drawing same frame twice" << frame_id; - } else if (frame_id != prev_frame_id + 1) { - qDebug() << "Skipped frame" << frame_id; + 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 = frame_id; + prev_frame_id = frames[frame_idx].first; + VisionBuf *frame = frames[frame_idx].second; + assert(frame != nullptr); updateFrameMat(); @@ -276,7 +327,7 @@ void CameraWidget::paintGL() { glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); } -void CameraWidget::vipcConnected() { +void CameraWidget::vipcConnected(VisionIpcClient *vipc_client) { makeCurrent(); stream_width = vipc_client->buffers[0].width; stream_height = vipc_client->buffers[0].height; @@ -326,69 +377,59 @@ void CameraWidget::vipcConnected() { #endif } -bool CameraWidget::receiveFrame(uint64_t request_frame_id) { - if (!vipc_client || vipc_client->type != requested_stream_type) { - qDebug().nospace() << "connecting to stream" << requested_stream_type - << (vipc_client ? QString(", was connected to %1").arg(vipc_client->type) : ""); - vipc_client.reset(new VisionIpcClient(stream_name, requested_stream_type, false)); - } +void CameraWidget::vipcFrameReceived() { + update(); +} - if (!vipc_client->connected) { - frame = nullptr; - recent_frames.clear(); - available_streams = VisionIpcClient::getAvailableStreams(stream_name, false); - if (available_streams.empty() || !vipc_client->connect(false)) { - return 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)); } - emit vipcAvailableStreamsUpdated(); - vipcConnected(); - } + 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); - VisionIpcBufExtra meta_main = {}; - while (auto buf = vipc_client->recv(&meta_main, 0)) { - if (recent_frames.size() > FRAME_BUFFER_SIZE) { - recent_frames.pop_front(); + if (!vipc_client->connect(false)) { + QThread::msleep(100); + continue; + } + emit vipcThreadConnected(vipc_client.get()); } - recent_frames.emplace_back(meta_main.frame_id, buf); - } - frame = nullptr; - if (request_frame_id > 0) { - auto it = std::find_if(recent_frames.begin(), recent_frames.end(), - [request_frame_id](auto &f) { return f.first == request_frame_id; }); - if (it != recent_frames.end()) { - std::tie(frame_id, frame) = *it; + 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; + } } - } else if (!recent_frames.empty()) { - std::tie(frame_id, frame) = recent_frames.back(); - } - return frame != nullptr; -} - -void CameraWidget::disconnectVipc() { - recent_frames.clear(); - frame = nullptr; - frame_id = 0; - prev_frame_id = 0; - if (vipc_client) { - vipc_client->connected = false; } } -// Cameraview - -CameraView::CameraView(const std::string &name, VisionStreamType stream_type, bool zoom, QWidget *parent) - : CameraWidget(name, stream_type, zoom, parent) { - timer = new QTimer(this); - timer->setInterval(1000.0 / UI_FREQ); - timer->callOnTimeout(this, [this]() { if (receiveFrame()) update(); }); -} - -void CameraView::showEvent(QShowEvent *event) { - timer->start(); -} - -void CameraView::hideEvent(QHideEvent *event) { - timer->stop(); - disconnectVipc(); +void CameraWidget::clearFrames() { + std::lock_guard lk(frame_lock); + frames.clear(); + available_streams.clear(); } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index a862079f1a..c97038cf43 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -10,7 +11,7 @@ #include #include #include -#include +#include #ifdef QCOM2 #define EGL_EGLEXT_PROTOTYPES @@ -32,27 +33,31 @@ class CameraWidget : public QOpenGLWidget, protected QOpenGLFunctions { Q_OBJECT public: + using QOpenGLWidget::QOpenGLWidget; explicit CameraWidget(std::string stream_name, VisionStreamType stream_type, bool zoom, QWidget* parent = nullptr); ~CameraWidget(); - void disconnectVipc(); 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; } - inline VisionStreamType streamType() const { return requested_stream_type; } - inline const std::set &availableStreams() const { return available_streams; } - bool receiveFrame(uint64_t request_frame_id = 0); + VisionStreamType getStreamType() { return active_stream_type; } + void stopVipcThread(); signals: - void vipcAvailableStreamsUpdated(); void clicked(); + void vipcThreadConnected(VisionIpcClient *); + void vipcThreadFrameReceived(); + void vipcAvailableStreamsUpdated(std::set); protected: void paintGL() override; void initializeGL() override; void resizeGL(int w, int h) override { updateFrameMat(); } + void showEvent(QShowEvent *event) override; void mouseReleaseEvent(QMouseEvent *event) override { emit clicked(); } virtual void updateFrameMat(); void updateCalibration(const mat3 &calib); - void vipcConnected(); + void vipcThread(); + void clearFrames(); int glWidth(); int glHeight(); @@ -68,18 +73,14 @@ protected: std::map egl_images; #endif - // vipc std::string stream_name; int stream_width = 0; int stream_height = 0; int stream_stride = 0; - VisionStreamType requested_stream_type; + std::atomic active_stream_type; + std::atomic requested_stream_type; std::set available_streams; - std::unique_ptr vipc_client; - std::deque> recent_frames; - VisionBuf *frame = nullptr; - uint64_t frame_id = 0; - uint64_t prev_frame_id = 0; + QThread *vipc_thread = nullptr; // Calibration float x_offset = 0; @@ -87,16 +88,16 @@ protected: float zoom = 1.0; mat3 calibration = DEFAULT_CALIBRATION; mat3 intrinsic_matrix = FCAM_INTRINSIC_MATRIX; -}; -// update frames based on timer -class CameraView : public CameraWidget { - Q_OBJECT -public: - CameraView(const std::string &name, VisionStreamType stream_type, bool zoom, QWidget *parent = nullptr); - void showEvent(QShowEvent *event) override; - void hideEvent(QHideEvent *event) override; + std::recursive_mutex frame_lock; + std::deque> frames; + uint32_t draw_frame_id = 0; + uint32_t prev_frame_id = 0; -private: - QTimer *timer; +protected slots: + void vipcConnected(VisionIpcClient *vipc_client); + void vipcFrameReceived(); + void availableStreamsUpdated(std::set streams); }; + +Q_DECLARE_METATYPE(std::set); diff --git a/selfdrive/ui/watch3.cc b/selfdrive/ui/watch3.cc index cb12f82ea8..ec35c29b6b 100644 --- a/selfdrive/ui/watch3.cc +++ b/selfdrive/ui/watch3.cc @@ -19,15 +19,15 @@ int main(int argc, char *argv[]) { { QHBoxLayout *hlayout = new QHBoxLayout(); layout->addLayout(hlayout); - hlayout->addWidget(new CameraView("navd", VISION_STREAM_MAP, false)); - hlayout->addWidget(new CameraView("camerad", VISION_STREAM_ROAD, false)); + hlayout->addWidget(new CameraWidget("navd", VISION_STREAM_MAP, false)); + hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_ROAD, false)); } { QHBoxLayout *hlayout = new QHBoxLayout(); layout->addLayout(hlayout); - hlayout->addWidget(new CameraView("camerad", VISION_STREAM_DRIVER, false)); - hlayout->addWidget(new CameraView("camerad", VISION_STREAM_WIDE_ROAD, false)); + hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_DRIVER, false)); + hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_WIDE_ROAD, false)); } return a.exec(); diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index 654c34ed37..a6fd0b2b64 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -139,7 +139,7 @@ QWidget *VideoWidget::createCameraWidget() { QStackedLayout *stacked = new QStackedLayout(); stacked->setStackingMode(QStackedLayout::StackAll); - stacked->addWidget(cam_widget = new CameraView("camerad", VISION_STREAM_ROAD, false)); + stacked->addWidget(cam_widget = new CameraWidget("camerad", VISION_STREAM_ROAD, false)); cam_widget->setMinimumHeight(MIN_VIDEO_HEIGHT); cam_widget->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::MinimumExpanding); stacked->addWidget(alert_label = new InfoLabel(this)); @@ -160,13 +160,12 @@ QWidget *VideoWidget::createCameraWidget() { return w; } -void VideoWidget::vipcAvailableStreamsUpdated() { +void VideoWidget::vipcAvailableStreamsUpdated(std::set streams) { static const QString stream_names[] = { [VISION_STREAM_ROAD] = "Road camera", [VISION_STREAM_WIDE_ROAD] = "Wide road camera", [VISION_STREAM_DRIVER] = "Driver camera"}; - const auto &streams = cam_widget->availableStreams(); for (int i = 0; i < streams.size(); ++i) { if (camera_tab->count() <= i) { camera_tab->addTab(QString()); diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h index ae095fd559..b2039e09a4 100644 --- a/tools/cabana/videowidget.h +++ b/tools/cabana/videowidget.h @@ -73,9 +73,9 @@ protected: QWidget *createCameraWidget(); QHBoxLayout *createPlaybackController(); void loopPlaybackClicked(); - void vipcAvailableStreamsUpdated(); + void vipcAvailableStreamsUpdated(std::set streams); - CameraView *cam_widget; + CameraWidget *cam_widget; double maximum_time = 0; QToolButton *time_btn = nullptr; ToolButton *seek_backward_btn = nullptr;