diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc index df9bb24651..08f6f10642 100644 --- a/selfdrive/ui/qt/offroad/driverview.cc +++ b/selfdrive/ui/qt/offroad/driverview.cc @@ -7,7 +7,7 @@ const int FACE_IMG_SIZE = 130; -DriverViewWindow::DriverViewWindow(QWidget* parent) : CameraWidget("camerad", VISION_STREAM_DRIVER, true, parent) { +DriverViewWindow::DriverViewWindow(QWidget* parent) : CameraView("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,22 +20,20 @@ DriverViewWindow::DriverViewWindow(QWidget* parent) : CameraWidget("camerad", VI void DriverViewWindow::showEvent(QShowEvent* event) { params.putBool("IsDriverViewEnabled", true); device()->resetInteractiveTimeout(60); - CameraWidget::showEvent(event); + CameraView::showEvent(event); } void DriverViewWindow::hideEvent(QHideEvent* event) { params.putBool("IsDriverViewEnabled", false); - stopVipcThread(); - CameraWidget::hideEvent(event); + CameraView::hideEvent(event); } void DriverViewWindow::paintGL() { - CameraWidget::paintGL(); + CameraView::paintGL(); - std::lock_guard lk(frame_lock); QPainter p(this); // startup msg - if (frames.empty()) { + if (!frame) { p.setPen(Qt::white); p.setRenderHint(QPainter::TextAntialiasing); p.setFont(InterFont(100, QFont::Bold)); @@ -47,7 +45,6 @@ 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(); diff --git a/selfdrive/ui/qt/offroad/driverview.h b/selfdrive/ui/qt/offroad/driverview.h index 155e4ede32..97dd2faa78 100644 --- a/selfdrive/ui/qt/offroad/driverview.h +++ b/selfdrive/ui/qt/offroad/driverview.h @@ -2,7 +2,7 @@ #include "selfdrive/ui/qt/widgets/cameraview.h" -class DriverViewWindow : public CameraWidget { +class DriverViewWindow : public CameraView { Q_OBJECT public: diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 4df0ed81cb..ed4bdc15fb 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/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 CameraWidget("camerad", VISION_STREAM_ROAD, true, this); + CameraWidget *arCam = new CameraView("camerad", VISION_STREAM_ROAD, true, this); split->insertWidget(0, arCam); } if (getenv("MAP_RENDER_VIEW")) { - CameraWidget *map_render = new CameraWidget("navd", VISION_STREAM_MAP, false, this); + CameraWidget *map_render = new CameraView("navd", VISION_STREAM_MAP, false, this); split->insertWidget(0, map_render); } @@ -126,6 +126,7 @@ void OnroadWindow::offroadTransition(bool offroad) { #endif alerts->updateAlert({}); + nvg->disconnectVipc(); } void OnroadWindow::primeChanged(bool prime) { @@ -328,6 +329,8 @@ 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) { @@ -609,21 +612,6 @@ 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) { @@ -639,14 +627,18 @@ void AnnotatedCameraWidget::paintGL() { } CameraWidget::setStreamType(wide_cam_requested ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); - s->scene.wide_cam = CameraWidget::getStreamType() == VISION_STREAM_WIDE_ROAD; + s->scene.wide_cam = CameraWidget::streamType() == 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); } - CameraWidget::setFrameId(model.getFrameId()); + + if (!CameraWidget::receiveFrame(sm["uiPlan"].getUiPlan().getFrameId())) { + qDebug() << "skipping frame, not ready"; + return; + } CameraWidget::paintGL(); } diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index b3ba411453..f6b80a75de 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -93,8 +93,6 @@ private: bool v_ego_cluster_seen = false; int status = STATUS_DISENGAGED; std::unique_ptr pm; - - int skip_frame_count = 0; bool wide_cam_requested = false; protected: diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 7b1f2f1d24..4d370b8cc5 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -6,14 +6,6 @@ #include #endif -#include -#include -#include -#include - -#include -#include - namespace { const char frame_vertex_shader[] = @@ -95,24 +87,25 @@ 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) { - setAttribute(Qt::WA_OpaquePaintEvent); - qRegisterMetaType>("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(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() { 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(); } @@ -176,45 +169,11 @@ 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 streams) { - available_streams = streams; -} - void CameraWidget::updateFrameMat() { int w = glWidth(), h = glHeight(); if (zoomed_view) { - if (active_stream_type == VISION_STREAM_DRIVER) { + if (streamType() == VISION_STREAM_DRIVER) { if (stream_width > 0 && stream_height > 0) { frame_mat = get_driver_view_transform(w, h, stream_width, stream_height); } @@ -223,7 +182,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 (active_stream_type == VISION_STREAM_WIDE_ROAD) { + if (streamType() == VISION_STREAM_WIDE_ROAD) { intrinsic_matrix = ECAM_INTRINSIC_MATRIX; zoom = 2.0; } else { @@ -269,25 +228,15 @@ 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; - - // 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; - // } + if (!frame) return; // Log duplicate/dropped frames - 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; + 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; } - prev_frame_id = frames[frame_idx].first; - VisionBuf *frame = frames[frame_idx].second; - assert(frame != nullptr); + prev_frame_id = frame_id; updateFrameMat(); @@ -327,7 +276,7 @@ void CameraWidget::paintGL() { glPixelStorei(GL_UNPACK_ROW_LENGTH, 0); } -void CameraWidget::vipcConnected(VisionIpcClient *vipc_client) { +void CameraWidget::vipcConnected() { makeCurrent(); stream_width = vipc_client->buffers[0].width; stream_height = vipc_client->buffers[0].height; @@ -377,59 +326,69 @@ void CameraWidget::vipcConnected(VisionIpcClient *vipc_client) { #endif } -void CameraWidget::vipcFrameReceived() { - update(); -} +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::vipcThread() { - VisionStreamType cur_stream = requested_stream_type; - std::unique_ptr 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)); + 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; } - active_stream_type = cur_stream; - - if (!vipc_client->connected) { - clearFrames(); - auto streams = VisionIpcClient::getAvailableStreams(stream_name, false); - if (streams.empty()) { - QThread::msleep(100); - continue; - } - emit vipcAvailableStreamsUpdated(streams); + emit vipcAvailableStreamsUpdated(); + vipcConnected(); + } - if (!vipc_client->connect(false)) { - QThread::msleep(100); - continue; - } - emit vipcThreadConnected(vipc_client.get()); + VisionIpcBufExtra meta_main = {}; + while (auto buf = vipc_client->recv(&meta_main, 0)) { + if (recent_frames.size() > FRAME_BUFFER_SIZE) { + recent_frames.pop_front(); } + recent_frames.emplace_back(meta_main.frame_id, buf); + } - 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(); - } - } - emit vipcThreadFrameReceived(); - } else { - if (!isVisible()) { - vipc_client->connected = false; - } + 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; } + } 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) { + vipc_client->connected = false; } } -void CameraWidget::clearFrames() { - std::lock_guard lk(frame_lock); - frames.clear(); - available_streams.clear(); +// 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(); } diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h index c97038cf43..a862079f1a 100644 --- a/selfdrive/ui/qt/widgets/cameraview.h +++ b/selfdrive/ui/qt/widgets/cameraview.h @@ -3,7 +3,6 @@ #include #include #include -#include #include #include #include @@ -11,7 +10,7 @@ #include #include #include -#include +#include #ifdef QCOM2 #define EGL_EGLEXT_PROTOTYPES @@ -33,31 +32,27 @@ 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; } - VisionStreamType getStreamType() { return active_stream_type; } - void stopVipcThread(); + inline VisionStreamType streamType() const { return requested_stream_type; } + inline const std::set &availableStreams() const { return available_streams; } + bool receiveFrame(uint64_t request_frame_id = 0); signals: + void vipcAvailableStreamsUpdated(); void clicked(); - void vipcThreadConnected(VisionIpcClient *); - void vipcThreadFrameReceived(); - void vipcAvailableStreamsUpdated(std::set); 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 vipcThread(); - void clearFrames(); + void vipcConnected(); int glWidth(); int glHeight(); @@ -73,14 +68,18 @@ protected: std::map egl_images; #endif + // vipc std::string stream_name; int stream_width = 0; int stream_height = 0; int stream_stride = 0; - std::atomic active_stream_type; - std::atomic requested_stream_type; + VisionStreamType requested_stream_type; std::set available_streams; - QThread *vipc_thread = nullptr; + std::unique_ptr vipc_client; + std::deque> recent_frames; + VisionBuf *frame = nullptr; + uint64_t frame_id = 0; + uint64_t prev_frame_id = 0; // Calibration float x_offset = 0; @@ -88,16 +87,16 @@ protected: float zoom = 1.0; mat3 calibration = DEFAULT_CALIBRATION; mat3 intrinsic_matrix = FCAM_INTRINSIC_MATRIX; +}; - std::recursive_mutex frame_lock; - std::deque> frames; - uint32_t draw_frame_id = 0; - uint32_t prev_frame_id = 0; +// 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; -protected slots: - void vipcConnected(VisionIpcClient *vipc_client); - void vipcFrameReceived(); - void availableStreamsUpdated(std::set streams); +private: + QTimer *timer; }; - -Q_DECLARE_METATYPE(std::set); diff --git a/selfdrive/ui/watch3.cc b/selfdrive/ui/watch3.cc index ec35c29b6b..cb12f82ea8 100644 --- a/selfdrive/ui/watch3.cc +++ b/selfdrive/ui/watch3.cc @@ -19,15 +19,15 @@ int main(int argc, char *argv[]) { { QHBoxLayout *hlayout = new QHBoxLayout(); layout->addLayout(hlayout); - hlayout->addWidget(new CameraWidget("navd", VISION_STREAM_MAP, false)); - hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_ROAD, false)); + hlayout->addWidget(new CameraView("navd", VISION_STREAM_MAP, false)); + hlayout->addWidget(new CameraView("camerad", VISION_STREAM_ROAD, false)); } { QHBoxLayout *hlayout = new QHBoxLayout(); layout->addLayout(hlayout); - hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_DRIVER, false)); - hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_WIDE_ROAD, false)); + hlayout->addWidget(new CameraView("camerad", VISION_STREAM_DRIVER, false)); + hlayout->addWidget(new CameraView("camerad", VISION_STREAM_WIDE_ROAD, false)); } return a.exec(); diff --git a/tools/cabana/videowidget.cc b/tools/cabana/videowidget.cc index a6fd0b2b64..654c34ed37 100644 --- a/tools/cabana/videowidget.cc +++ b/tools/cabana/videowidget.cc @@ -139,7 +139,7 @@ QWidget *VideoWidget::createCameraWidget() { QStackedLayout *stacked = new QStackedLayout(); stacked->setStackingMode(QStackedLayout::StackAll); - stacked->addWidget(cam_widget = new CameraWidget("camerad", VISION_STREAM_ROAD, false)); + stacked->addWidget(cam_widget = new CameraView("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,12 +160,13 @@ QWidget *VideoWidget::createCameraWidget() { return w; } -void VideoWidget::vipcAvailableStreamsUpdated(std::set streams) { +void VideoWidget::vipcAvailableStreamsUpdated() { 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()); diff --git a/tools/cabana/videowidget.h b/tools/cabana/videowidget.h index b2039e09a4..ae095fd559 100644 --- a/tools/cabana/videowidget.h +++ b/tools/cabana/videowidget.h @@ -73,9 +73,9 @@ protected: QWidget *createCameraWidget(); QHBoxLayout *createPlaybackController(); void loopPlaybackClicked(); - void vipcAvailableStreamsUpdated(std::set streams); + void vipcAvailableStreamsUpdated(); - CameraWidget *cam_widget; + CameraView *cam_widget; double maximum_time = 0; QToolButton *time_btn = nullptr; ToolButton *seek_backward_btn = nullptr;