diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 31c4558dc1..0292020a01 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -374,7 +374,7 @@ 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::draw_frame_id = model.getFrameId(); + CameraViewWidget::setFrameId(model.getFrameId()); CameraViewWidget::paintGL(); QPainter painter(this); diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 13f20a9edc..22a4e45623 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -104,6 +104,7 @@ mat4 get_fit_view_transform(float widget_aspect_ratio, float frame_aspect_ratio) CameraViewWidget::CameraViewWidget(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); + qRegisterMetaType("uint32_t"); connect(this, &CameraViewWidget::vipcThreadConnected, this, &CameraViewWidget::vipcConnected, Qt::BlockingQueuedConnection); connect(this, &CameraViewWidget::vipcThreadFrameReceived, this, &CameraViewWidget::vipcFrameReceived); } @@ -167,7 +168,6 @@ void CameraViewWidget::initializeGL() { void CameraViewWidget::showEvent(QShowEvent *event) { frames.clear(); - frame_ids.clear(); if (!vipc_thread) { vipc_thread = new QThread(); connect(vipc_thread, &QThread::started, [=]() { vipcThread(); }); @@ -218,13 +218,13 @@ void CameraViewWidget::paintGL() { glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF()); glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT); - assert(frames.size() == frame_ids.size()); if (frames.size() == 0) return; - VisionBuf *frame; - std::deque::iterator it = std::find(frame_ids.begin(), frame_ids.end(), draw_frame_id); - int frame_idx = (it == frame_ids.end()) ? (frames.size() - 1) : (it - frame_ids.begin()); - frame = frames[frame_idx]; + auto it = std::find_if(frames.begin(), frames.end(), [this](const std::pair & element) { + return element.first == draw_frame_id; + }); + int frame_idx = (it == frames.end()) ? (frames.size() - 1) : (it - frames.begin()); + VisionBuf *frame = frames[frame_idx].second; glPixelStorei(GL_UNPACK_ALIGNMENT, 1); glViewport(0, 0, width(), height()); @@ -255,7 +255,6 @@ void CameraViewWidget::paintGL() { void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { makeCurrent(); frames.clear(); - frame_ids.clear(); stream_width = vipc_client->buffers[0].width; stream_height = vipc_client->buffers[0].height; @@ -274,15 +273,11 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) { updateFrameMat(width(), height()); } -void CameraViewWidget::vipcFrameReceived(VisionBuf *buf, quint32 frame_id) { - frames.push_back(buf); - frame_ids.push_back(frame_id); +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(); } - while (frame_ids.size() > FRAME_BUFFER_SIZE) { - frame_ids.pop_front(); - } update(); } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 29050538a0..d995fa8c4d 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -19,12 +19,12 @@ public: ~CameraViewWidget(); void setStreamType(VisionStreamType type) { stream_type = type; } void setBackgroundColor(const QColor &color) { bg = color; } - quint32 draw_frame_id; + void setFrameId(int frame_id) { draw_frame_id = frame_id; } signals: void clicked(); void vipcThreadConnected(VisionIpcClient *); - void vipcThreadFrameReceived(VisionBuf *, quint32); + void vipcThreadFrameReceived(VisionBuf *, uint32_t); protected: void paintGL() override; @@ -37,9 +37,8 @@ protected: void vipcThread(); bool zoomed_view; - std::deque frames; - std::deque frame_ids; GLuint frame_vao, frame_vbo, frame_ibo; + GLuint textures[3]; mat4 frame_mat; std::unique_ptr program; QColor bg = QColor("#000000"); @@ -50,9 +49,10 @@ protected: std::atomic stream_type; QThread *vipc_thread = nullptr; - GLuint textures[3]; + std::deque> frames; + uint32_t draw_frame_id; protected slots: void vipcConnected(VisionIpcClient *vipc_client); - void vipcFrameReceived(VisionBuf *vipc_client, quint32 frame_id); + void vipcFrameReceived(VisionBuf *vipc_client, uint32_t frame_id); };