From 9833cbec4efddc6cbb81b24d1844b98652cd281b Mon Sep 17 00:00:00 2001
From: Dean Lee <deanlee3@gmail.com>
Date: Thu, 27 Oct 2022 10:47:32 +0800
Subject: [PATCH] UI: support switching streams in CameraView (#26248)

old-commit-hash: 6b6162d2c822c717cd62312f51113180c21b5664
---
 selfdrive/ui/qt/widgets/cameraview.cc | 35 ++++++++++++++++++---------
 selfdrive/ui/qt/widgets/cameraview.h  |  7 ++++--
 2 files changed, 28 insertions(+), 14 deletions(-)

diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc
index 200257235d..4f46497776 100644
--- a/selfdrive/ui/qt/widgets/cameraview.cc
+++ b/selfdrive/ui/qt/widgets/cameraview.cc
@@ -96,8 +96,8 @@ mat4 get_fit_view_transform(float widget_aspect_ratio, float frame_aspect_ratio)
 CameraWidget::CameraWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget* parent) :
                                    stream_name(stream_name), stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) {
   setAttribute(Qt::WA_OpaquePaintEvent);
-  connect(this, &CameraWidget::vipcThreadConnected, this, &CameraWidget::vipcConnected, Qt::BlockingQueuedConnection);
-  connect(this, &CameraWidget::vipcThreadFrameReceived, this, &CameraWidget::vipcFrameReceived);
+  QObject::connect(this, &CameraWidget::vipcThreadConnected, this, &CameraWidget::vipcConnected, Qt::BlockingQueuedConnection);
+  QObject::connect(this, &CameraWidget::vipcThreadFrameReceived, this, &CameraWidget::vipcFrameReceived, Qt::QueuedConnection);
 }
 
 CameraWidget::~CameraWidget() {
@@ -162,13 +162,13 @@ void CameraWidget::initializeGL() {
 }
 
 void CameraWidget::showEvent(QShowEvent *event) {
-  frames.clear();
   if (!vipc_thread) {
     vipc_thread = new QThread();
     connect(vipc_thread, &QThread::started, [=]() { vipcThread(); });
     connect(vipc_thread, &QThread::finished, vipc_thread, &QObject::deleteLater);
     vipc_thread->start();
   }
+  clearFrames();
 }
 
 void CameraWidget::hideEvent(QHideEvent *event) {
@@ -178,6 +178,7 @@ void CameraWidget::hideEvent(QHideEvent *event) {
     vipc_thread->wait();
     vipc_thread = nullptr;
   }
+  clearFrames();
 }
 
 void CameraWidget::updateFrameMat() {
@@ -233,6 +234,7 @@ 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;
@@ -249,14 +251,14 @@ void CameraWidget::paintGL() {
     qDebug() << "Skipped frame" << frames[frame_idx].first;
   }
   prev_frame_id = frames[frame_idx].first;
+  VisionBuf *frame = frames[frame_idx].second;
+  assert(frame != nullptr);
 
   glViewport(0, 0, width(), height());
   glBindVertexArray(frame_vao);
   glUseProgram(program->programId());
   glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
 
-  VisionBuf *frame = frames[frame_idx].second;
-
 #ifdef QCOM2
   glActiveTexture(GL_TEXTURE0);
   glEGLImageTargetTexture2DOES(GL_TEXTURE_EXTERNAL_OES, egl_images[frame->idx]);
@@ -288,7 +290,6 @@ void CameraWidget::paintGL() {
 
 void CameraWidget::vipcConnected(VisionIpcClient *vipc_client) {
   makeCurrent();
-  frames.clear();
   stream_width = vipc_client->buffers[0].width;
   stream_height = vipc_client->buffers[0].height;
   stream_stride = vipc_client->buffers[0].stride;
@@ -339,11 +340,7 @@ void CameraWidget::vipcConnected(VisionIpcClient *vipc_client) {
   updateFrameMat();
 }
 
-void CameraWidget::vipcFrameReceived(VisionBuf *buf, uint32_t frame_id) {
-  frames.push_back(std::make_pair(frame_id, buf));
-  while (frames.size() > FRAME_BUFFER_SIZE) {
-    frames.pop_front();
-  }
+void CameraWidget::vipcFrameReceived() {
   update();
 }
 
@@ -354,11 +351,13 @@ void CameraWidget::vipcThread() {
 
   while (!QThread::currentThread()->isInterruptionRequested()) {
     if (!vipc_client || cur_stream_type != stream_type) {
+      clearFrames();
       cur_stream_type = stream_type;
       vipc_client.reset(new VisionIpcClient(stream_name, cur_stream_type, false));
     }
 
     if (!vipc_client->connected) {
+      clearFrames();
       if (!vipc_client->connect(false)) {
         QThread::msleep(100);
         continue;
@@ -367,7 +366,14 @@ void CameraWidget::vipcThread() {
     }
 
     if (VisionBuf *buf = vipc_client->recv(&meta_main, 1000)) {
-      emit vipcThreadFrameReceived(buf, meta_main.frame_id);
+      {
+        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();
     }
   }
 
@@ -378,3 +384,8 @@ void CameraWidget::vipcThread() {
   egl_images.clear();
 #endif
 }
+
+void CameraWidget::clearFrames() {
+  std::lock_guard lk(frame_lock);
+  frames.clear();
+}
diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h
index 9bcad935c0..ef828f8ff6 100644
--- a/selfdrive/ui/qt/widgets/cameraview.h
+++ b/selfdrive/ui/qt/widgets/cameraview.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <memory>
+#include <mutex>
 
 #include <QOpenGLFunctions>
 #include <QOpenGLShaderProgram>
@@ -37,7 +38,7 @@ public:
 signals:
   void clicked();
   void vipcThreadConnected(VisionIpcClient *);
-  void vipcThreadFrameReceived(VisionBuf *, quint32);
+  void vipcThreadFrameReceived();
 
 protected:
   void paintGL() override;
@@ -49,6 +50,7 @@ protected:
   virtual void updateFrameMat();
   void updateCalibration(const mat3 &calib);
   void vipcThread();
+  void clearFrames();
 
   bool zoomed_view;
   GLuint frame_vao, frame_vbo, frame_ibo;
@@ -76,11 +78,12 @@ protected:
   mat3 calibration = DEFAULT_CALIBRATION;
   mat3 intrinsic_matrix = fcam_intrinsic_matrix;
 
+  std::mutex frame_lock;
   std::deque<std::pair<uint32_t, VisionBuf*>> frames;
   uint32_t draw_frame_id = 0;
   uint32_t prev_frame_id = 0;
 
 protected slots:
   void vipcConnected(VisionIpcClient *vipc_client);
-  void vipcFrameReceived(VisionBuf *vipc_client, uint32_t frame_id);
+  void vipcFrameReceived();
 };