From c10a2ef2e40a49b527ab49b8b7fd4e924b847a2d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 24 May 2022 17:08:06 -0700 Subject: [PATCH] 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 * 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 --- selfdrive/ui/qt/onroad.cc | 6 ++++-- selfdrive/ui/qt/widgets/cameraview.cc | 28 ++++++++++++++++++--------- selfdrive/ui/qt/widgets/cameraview.h | 13 +++++++++---- 3 files changed, 32 insertions(+), 15 deletions(-) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 91e2a76f02..bedb570235 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/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]); } diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 646d286ee7..a0b3c43cde 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/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 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); } } } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index 6873e8e991..65d5edc221 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/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 program; QColor bg = QColor("#000000"); @@ -48,9 +52,10 @@ protected: std::atomic stream_type; QThread *vipc_thread = nullptr; - GLuint textures[3]; + std::deque> 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); };