UI: draw camera on same frame from modelV2 (#24335)

* draw camera on same frame from modelV2

* fix reversing

* optimze

* sleep shorter

* heavy debugging

* clean up

* fix ui hang

* try this

* more generic implementation

* add unused flag back

* draw latest frames when camerad is coming up with no modeld

* only get modelV2 once

* fix drawing

* conflate, reduce buffer, and remove special case when starting

* make tests pass

* remove comments

* frame_id is a uint32_t

* not sure why passing to cameraview causes overflow

* ternary, not too complicated

* change to size

* rename to frame

* use unique_ptr

* Revert "use unique_ptr"

This reverts commit 955842d9511a26468d4deedd1807f096540cb40c.

* Assert buffer size

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* use one deque with pairs and uint32_t

* formatting

* frame offset

* Revert "frame offset"

This reverts commit a2cff8a2d57a02f314d9f6adb7221b193a150e4c.

* use array

* add prints

* Revert "add prints"

This reverts commit d6978746e5c316fb2d5a619091c9aaf849c31967.

* clean up

* this handles all cases

* we want to clip

* fixes going backwards when replay or modeld lags multiple frames, then skips multiple frames

fixes going backwards when replay or modeld lags multiple frames, then skips multiple frames

* fix rare case where camera is behind model

simpler comment

* draw latest frames if we start with no model, then hang on current frame until model catches up

fix

* store if it's updated

* accurate comment

* reset to latest on connect and showevent

* better order

* use quint32 for sending signal function

* use empty

* draft

* kinda works

* works

* one line

* debug

* clean up

* revert this

* to preserve behavior, think we need to poll these

* revert

* no timeout when modeld is not alive

* Update selfdrive/ui/qt/onroad.cc

* changes from other PR

* see if we're drawing dup model or camera frames

* looks good, just need to clean up

* debug

* clean up

* clean up

* revert these changes

* fix

* pretty simple, test on device

* clean up

* revert this

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

selfdrive/ui/qt/onroad.cc

@@ -372,19 +372,21 @@ void NvgWindow::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV
 }
 
 void NvgWindow::paintGL() {
+  UIState *s = uiState();
+  const cereal::ModelDataV2::Reader &model = (*s->sm)["modelV2"].getModelV2();
+  CameraViewWidget::setFrameId(model.getFrameId());
   CameraViewWidget::paintGL();
 
   QPainter painter(this);
   painter.setRenderHint(QPainter::Antialiasing);
   painter.setPen(Qt::NoPen);
 
-  UIState *s = uiState();
   if (s->worldObjectsVisible()) {
 
     drawLaneLines(painter, s);
 
     if (s->scene.longitudinal_control) {
-      auto leads = (*s->sm)["modelV2"].getModelV2().getLeadsV3();
+      const auto leads = model.getLeadsV3();
       if (leads[0].getProb() > .5) {
         drawLead(painter, leads[0], s->scene.lead_vertices[0]);
       }

selfdrive/ui/qt/widgets/cameraview.cc

@@ -163,7 +163,7 @@ void CameraViewWidget::initializeGL() {
 }
 
 void CameraViewWidget::showEvent(QShowEvent *event) {
-  latest_frame = nullptr;
+  frames.clear();
   if (!vipc_thread) {
     vipc_thread = new QThread();
     connect(vipc_thread, &QThread::started, [=]() { vipcThread(); });
@@ -214,14 +214,20 @@ void CameraViewWidget::paintGL() {
   glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF());
   glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
-  if (latest_frame == nullptr) return;
+  if (frames.empty()) return;
+
+  int frame_idx;
+  for (frame_idx = 0; frame_idx < frames.size() - 1; frame_idx++) {
+    if (frames[frame_idx].first == draw_frame_id) break;
+  }
+  VisionBuf *frame = frames[frame_idx].second;
 
   glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
   glViewport(0, 0, width(), height());
   glBindVertexArray(frame_vao);
 
   glUseProgram(program->programId());
-  uint8_t *address[3] = {latest_frame->y, latest_frame->u, latest_frame->v};
+  uint8_t *address[3] = {frame->y, frame->u, frame->v};
   for (int i = 0; i < 3; ++i) {
     glActiveTexture(GL_TEXTURE0 + i);
     glBindTexture(GL_TEXTURE_2D, textures[i]);
@@ -244,7 +250,7 @@ void CameraViewWidget::paintGL() {
 
 void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) {
   makeCurrent();
-  latest_frame = nullptr;
+  frames.clear();
   stream_width = vipc_client->buffers[0].width;
   stream_height = vipc_client->buffers[0].height;
 
@@ -263,19 +269,23 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) {
   updateFrameMat(width(), height());
 }
 
-void CameraViewWidget::vipcFrameReceived(VisionBuf *buf) {
-  latest_frame = buf;
+void CameraViewWidget::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();
+  }
   update();
 }
 
 void CameraViewWidget::vipcThread() {
   VisionStreamType cur_stream_type = stream_type;
   std::unique_ptr<VisionIpcClient> vipc_client;
+  VisionIpcBufExtra meta_main = {0};
 
   while (!QThread::currentThread()->isInterruptionRequested()) {
     if (!vipc_client || cur_stream_type != stream_type) {
       cur_stream_type = stream_type;
-      vipc_client.reset(new VisionIpcClient(stream_name, cur_stream_type, true));
+      vipc_client.reset(new VisionIpcClient(stream_name, cur_stream_type, false));
     }
 
     if (!vipc_client->connected) {
@@ -286,8 +296,8 @@ void CameraViewWidget::vipcThread() {
       emit vipcThreadConnected(vipc_client.get());
     }
 
-    if (VisionBuf *buf = vipc_client->recv(nullptr, 1000)) {
-      emit vipcThreadFrameReceived(buf);
+    if (VisionBuf *buf = vipc_client->recv(&meta_main, 1000)) {
+      emit vipcThreadFrameReceived(buf, meta_main.frame_id);
     }
   }
 }

selfdrive/ui/qt/widgets/cameraview.h

@@ -10,6 +10,9 @@
 #include "selfdrive/camerad/cameras/camera_common.h"
 #include "selfdrive/ui/ui.h"
 
+const int FRAME_BUFFER_SIZE = 5;
+static_assert(FRAME_BUFFER_SIZE <= YUV_BUFFER_COUNT);
+
 class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions {
   Q_OBJECT
 
@@ -19,11 +22,12 @@ public:
   ~CameraViewWidget();
   void setStreamType(VisionStreamType type) { stream_type = type; }
   void setBackgroundColor(const QColor &color) { bg = color; }
+  void setFrameId(int frame_id) { draw_frame_id = frame_id; }
 
 signals:
   void clicked();
   void vipcThreadConnected(VisionIpcClient *);
-  void vipcThreadFrameReceived(VisionBuf *);
+  void vipcThreadFrameReceived(VisionBuf *, quint32);
 
 protected:
   void paintGL() override;
@@ -36,8 +40,8 @@ protected:
   void vipcThread();
 
   bool zoomed_view;
-  VisionBuf *latest_frame = nullptr;
   GLuint frame_vao, frame_vbo, frame_ibo;
+  GLuint textures[3];
   mat4 frame_mat;
   std::unique_ptr<QOpenGLShaderProgram> program;
   QColor bg = QColor("#000000");
@@ -48,9 +52,10 @@ protected:
   std::atomic<VisionStreamType> stream_type;
   QThread *vipc_thread = nullptr;
 
-  GLuint textures[3];
+  std::deque<std::pair<uint32_t, VisionBuf*>> frames;
+  uint32_t draw_frame_id = 0;
 
 protected slots:
   void vipcConnected(VisionIpcClient *vipc_client);
-  void vipcFrameReceived(VisionBuf *vipc_client);
+  void vipcFrameReceived(VisionBuf *vipc_client, uint32_t frame_id);
 };