Revert "UI: single-threaded CameraView (#30397)"

This reverts commit 17ac1d3c7b.

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();

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:

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();
   }
 

selfdrive/ui/qt/onroad.h

@@ -93,6 +93,8 @@ private:
   bool v_ego_cluster_seen = false;
   int status = STATUS_DISENGAGED;
   std::unique_ptr<PubMaster> pm;
+
+  int skip_frame_count = 0;
   bool wide_cam_requested = false;
 
 protected:

selfdrive/ui/qt/widgets/cameraview.cc

@@ -6,6 +6,14 @@
 #include <GLES3/gl3.h>
 #endif
 
+#include <cmath>
+#include <set>
+#include <string>
+#include <utility>
+
+#include <QOpenGLBuffer>
+#include <QOffscreenSurface>
+
 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<std::set<VisionStreamType>>("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<VisionStreamType> 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();
+}
+
+void CameraWidget::vipcThread() {
+  VisionStreamType cur_stream = requested_stream_type;
+  std::unique_ptr<VisionIpcClient> 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) {
-    frame = nullptr;
-    recent_frames.clear();
-    available_streams = VisionIpcClient::getAvailableStreams(stream_name, false);
-    if (available_streams.empty() || !vipc_client->connect(false)) {
-      return false;
-    }
-    emit vipcAvailableStreamsUpdated();
-    vipcConnected();
+      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;
       }
-    recent_frames.emplace_back(meta_main.frame_id, buf);
+      emit vipcThreadConnected(vipc_client.get());
     }
 
-  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();
         }
-  } 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) {
+      emit vipcThreadFrameReceived();
+    } else {
+      if (!isVisible()) {
         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();
 }

selfdrive/ui/qt/widgets/cameraview.h

@@ -3,6 +3,7 @@
 #include <deque>
 #include <map>
 #include <memory>
+#include <mutex>
 #include <set>
 #include <string>
 #include <utility>
@@ -10,7 +11,7 @@
 #include <QOpenGLFunctions>
 #include <QOpenGLShaderProgram>
 #include <QOpenGLWidget>
-#include <QTimer>
+#include <QThread>
 
 #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<VisionStreamType> &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<VisionStreamType>);
 
 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<int, EGLImageKHR> 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<VisionStreamType> active_stream_type;
+  std::atomic<VisionStreamType> requested_stream_type;
   std::set<VisionStreamType> available_streams;
-  std::unique_ptr<VisionIpcClient> vipc_client;
-  std::deque<std::pair<uint32_t, VisionBuf*>> 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<std::pair<uint32_t, VisionBuf*>> 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<VisionStreamType> streams);
 };
+
+Q_DECLARE_METATYPE(std::set<VisionStreamType>);

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();

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<VisionStreamType> 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());

tools/cabana/videowidget.h

@@ -73,9 +73,9 @@ protected:
   QWidget *createCameraWidget();
   QHBoxLayout *createPlaybackController();
   void loopPlaybackClicked();
-  void vipcAvailableStreamsUpdated();
+  void vipcAvailableStreamsUpdated(std::set<VisionStreamType> streams);
 
-  CameraView *cam_widget;
+  CameraWidget *cam_widget;
   double maximum_time = 0;
   QToolButton *time_btn = nullptr;
   ToolButton *seek_backward_btn = nullptr;