camera client color picker

scripts/mock_camera_stream.sh

@@ -0,0 +1,3 @@
+#!/usr/bin/env bash
+PORT=${1:-8082}
+gst-launch-1.0 -q videotestsrc pattern=smpte ! video/x-raw,width=640,height=480,framerate=15/1 ! x265enc tune=zerolatency ! rtph265pay ! udpsink host=127.0.0.1 port="${PORT}"

teleoperation/camera_client/include/CameraClientMainWindow.hpp

@@ -25,6 +25,17 @@ namespace mrover {
 
         std::unordered_map<std::string, std::vector<std::string>> mConfigs;
 
+        QDockWidget* mColorPickerDock = nullptr;
+        QLabel* mColorCameraLabel = nullptr;
+        QFrame* mColorSwatchFrame = nullptr;
+        QLabel* mColorRLabel = nullptr;
+        QLabel* mColorGLabel = nullptr;
+        QLabel* mColorBLabel = nullptr;
+        QLabel* mColorHexLabel = nullptr;
+
+        void buildColorPickerDock();
+        void onColorPicked(QString const& cameraName, QColor const& color);
+
     public:
         explicit CameraClientMainWindow(QWidget* parent = nullptr);
 

teleoperation/camera_client/include/GstVideoWidgets.hpp

@@ -20,10 +20,33 @@ namespace mrover {
         [[nodiscard]] auto cameraName() const -> std::string const& { return mCameraName; }
     };
 
-    class GstVideoWidget : public QVideoWidget {
+    class GstVideoWidget;
+
+    class VideoSurface : public QAbstractVideoSurface {
+        Q_OBJECT
+        GstVideoWidget* mWidget;
+
+    public:
+        explicit VideoSurface(GstVideoWidget* widget, QObject* parent = nullptr);
+
+        QList<QVideoFrame::PixelFormat> supportedPixelFormats(
+                QAbstractVideoBuffer::HandleType handleType) const override;
+
+        bool present(QVideoFrame const& frame) override;
+    };
+
+    class GstVideoWidget : public QWidget {
         Q_OBJECT
 
         QMediaPlayer* mPlayer;
+        VideoSurface* mSurface;
+        QImage mLastFrame;
+
+        QColor mLastPickedColor;
+        QPoint mLastCursorPos;
+        bool mMouseInWidget = false;
+
+        void sampleColorAtCursor();
 
         int mRotation{0};
         std::string mBasePipeline;
@@ -42,6 +65,17 @@ namespace mrover {
         auto pause() -> void;
         auto stop() -> void;
         auto rotate90() -> void;
+
+        void updateFrame(QImage const& frame);
+
+    protected:
+        void mouseMoveEvent(QMouseEvent* event) override;
+        void enterEvent(QEvent* event) override;
+        void leaveEvent(QEvent* event) override;
+        void paintEvent(QPaintEvent* event) override;
+
+    signals:
+        void colorPicked(QColor color);
     };
 
     class GstVideoGridWidget : public QWidget {
@@ -101,6 +135,9 @@ namespace mrover {
         [[nodiscard]] auto error() const -> GstVideoGridWidget::Error;
         [[nodiscard]] auto errorString() const -> QString;
         [[nodiscard]] auto isError() const -> bool;
+
+    signals:
+        void colorPicked(QString cameraName, QColor color);
     };
 
 } // namespace mrover

teleoperation/camera_client/include/pch.hpp

@@ -9,6 +9,7 @@
 #include <unordered_map>
 #include <vector>
 
+#include <QAbstractVideoSurface>
 #include <QApplication>
 #include <QCheckBox>
 #include <QComboBox>
@@ -20,7 +21,9 @@
 #include <QDropEvent>
 #include <QFileDialog>
 #include <QFormLayout>
+#include <QFrame>
 #include <QGridLayout>
+#include <QHBoxLayout>
 #include <QImage>
 #include <QLabel>
 #include <QLineEdit>
@@ -31,6 +34,7 @@
 #include <QMimeData>
 #include <QMouseEvent>
 #include <QObject>
+#include <QPainter>
 #include <QPixmap>
 #include <QPushButton>
 #include <QResizeEvent>
@@ -39,7 +43,8 @@
 #include <QTimer>
 #include <QToolBar>
 #include <QVBoxLayout>
-#include <QVideoWidget>
+#include <QVideoFrame>
+#include <QVideoSurfaceFormat>
 #include <QWidget>
 
 #include <opencv2/core.hpp>

teleoperation/camera_client/src/CameraClientMainWindow.cpp

@@ -30,14 +30,18 @@ namespace mrover {
         mCentralScrollArea->setWidgetResizable(true);
         setCentralWidget(mCentralScrollArea);
 
+        buildColorPickerDock();
+
         addDockWidget(Qt::LeftDockWidgetArea, mCameraSelectorDock);
         splitDockWidget(mCameraSelectorDock, mGstRtpVideoCreatorDock, Qt::Vertical);
-        splitDockWidget(mCameraSelectorDock, mCameraConfigDock, Qt::Vertical);
+        splitDockWidget(mGstRtpVideoCreatorDock, mColorPickerDock, Qt::Vertical);
+        splitDockWidget(mColorPickerDock, mCameraConfigDock, Qt::Vertical);
+
+        connect(mCameraGridWidget, &GstVideoGridWidget::colorPicked,
+                this, &CameraClientMainWindow::onColorPicked);
 
         connect(mGstRtpVideoCreatorWidget, &GstRtpVideoCreatorWidget::createRequested,
                 this, [this](std::string const& name, std::string const& pipeline) {
-                    qDebug() << "Creating camera with name:" << QString::fromStdString(name) << "and pipeline:" << QString::fromStdString(pipeline);
-
                     CameraCallbacks callbacks{
                             .onHide = [this, name]() {
                                 mCameraGridWidget->hideVideo(name);
@@ -108,4 +112,58 @@ namespace mrover {
         mConfigs = configs;
         loadCameraConfigSlot(CAMERA_CONFIGS[CAMERA_CONFIG::ARM]);
     }
+
+    void CameraClientMainWindow::buildColorPickerDock() {
+        mColorPickerDock = new QDockWidget("Color Picker", this);
+
+        auto* panel = new QWidget();
+        auto* panelLayout = new QVBoxLayout(panel);
+        panelLayout->setAlignment(Qt::AlignTop);
+        panelLayout->setSpacing(8);
+
+        mColorSwatchFrame = new QFrame();
+        mColorSwatchFrame->setFixedSize(60, 60);
+        mColorSwatchFrame->setFrameShape(QFrame::Box);
+        mColorSwatchFrame->setStyleSheet("background-color: #000000; border: 1px solid gray;");
+
+        mColorCameraLabel = new QLabel("--");
+        mColorCameraLabel->setWordWrap(true);
+        mColorHexLabel = new QLabel("--");
+
+        auto* middleCol = new QVBoxLayout();
+        middleCol->setAlignment(Qt::AlignVCenter);
+        middleCol->addWidget(mColorCameraLabel);
+        middleCol->addWidget(mColorHexLabel);
+
+        mColorRLabel = new QLabel("--");
+        mColorGLabel = new QLabel("--");
+        mColorBLabel = new QLabel("--");
+
+        auto* rgbForm = new QFormLayout();
+        rgbForm->addRow("R:", mColorRLabel);
+        rgbForm->addRow("G:", mColorGLabel);
+        rgbForm->addRow("B:", mColorBLabel);
+
+        auto* row = new QHBoxLayout();
+        row->addWidget(mColorSwatchFrame, 1, Qt::AlignCenter);
+        row->addLayout(middleCol, 1);
+        row->addLayout(rgbForm, 1);
+
+        panelLayout->addLayout(row);
+
+        panel->setLayout(panelLayout);
+        mColorPickerDock->setWidget(panel);
+        addDockWidget(Qt::LeftDockWidgetArea, mColorPickerDock);
+    }
+
+    void CameraClientMainWindow::onColorPicked(QString const& cameraName, QColor const& color) {
+        mColorCameraLabel->setText(QString("Camera: %1").arg(cameraName));
+        mColorSwatchFrame->setStyleSheet(
+                QString("background-color: %1; border: 1px solid gray;").arg(color.name()));
+        mColorRLabel->setText(QString::number(color.red()));
+        mColorGLabel->setText(QString::number(color.green()));
+        mColorBLabel->setText(QString::number(color.blue()));
+        mColorHexLabel->setText(color.name().toUpper());
+    }
+
 } // namespace mrover

teleoperation/camera_client/src/GstVideoWidgets.cpp

@@ -46,13 +46,48 @@ void DraggableVideoFrame::mouseMoveEvent(QMouseEvent* event) {
     drag->exec(Qt::MoveAction);
 }
 
+// --------------------------------------------
+// VideoSurface
+// --------------------------------------------
+
+VideoSurface::VideoSurface(GstVideoWidget* widget, QObject* parent)
+    : QAbstractVideoSurface(parent), mWidget(widget) {}
+
+QList<QVideoFrame::PixelFormat> VideoSurface::supportedPixelFormats(QAbstractVideoBuffer::HandleType handleType) const {
+    if (handleType == QAbstractVideoBuffer::NoHandle) {
+        return {
+                QVideoFrame::Format_RGB32,
+                QVideoFrame::Format_ARGB32,
+                QVideoFrame::Format_RGB24,
+                QVideoFrame::Format_BGR32,
+                QVideoFrame::Format_BGR24};
+    }
+    return {};
+}
+
+bool VideoSurface::present(QVideoFrame const& frame) {
+    if (frame.isValid()) {
+        QVideoFrame cloneFrame(frame);
+        if (cloneFrame.map(QAbstractVideoBuffer::ReadOnly)) {
+            QImage image(cloneFrame.bits(), cloneFrame.width(), cloneFrame.height(),
+                         cloneFrame.bytesPerLine(), QVideoFrame::imageFormatFromPixelFormat(cloneFrame.pixelFormat()));
+            // image.copy() is required: cloneFrame.bits() becomes invalid after unmap
+            mWidget->updateFrame(image.copy());
+            cloneFrame.unmap();
+        }
+    }
+    return true;
+}
+
 // --------------------------------------------
 // GstVideoWidget
 // --------------------------------------------
 
-GstVideoWidget::GstVideoWidget(QWidget* parent) : QVideoWidget(parent) {
+GstVideoWidget::GstVideoWidget(QWidget* parent) : QWidget(parent) {
     mPlayer = new QMediaPlayer(this);
-    mPlayer->setVideoOutput(this);
+    mSurface = new VideoSurface(this, this);
+    mPlayer->setVideoOutput(mSurface);
+    setMouseTracking(true);
 }
 
 auto GstVideoWidget::setGstPipeline(std::string const& pipeline) -> void {
@@ -61,7 +96,7 @@ auto GstVideoWidget::setGstPipeline(std::string const& pipeline) -> void {
 }
 
 auto GstVideoWidget::applyPipeline() -> void {
-    std::string flipElement = "";
+    std::string flipElement;
     if (mRotation == 1) {
         flipElement = " ! videoflip method=clockwise";
     } else if (mRotation == 2) {
@@ -70,7 +105,9 @@ auto GstVideoWidget::applyPipeline() -> void {
         flipElement = " ! videoflip method=counterclockwise";
     }
 
-    mPlayer->setMedia(QUrl(std::format("gst-pipeline: {} ! videoconvert ! xvimagesink name=\"qtvideosink\" sync=false", mBasePipeline, flipElement).c_str()));
+    // qtvideosink is required (not xvimagesink) so VideoSurface::present receives frames for color sampling
+    mPlayer->setMedia(QUrl(QString::fromStdString(
+            std::format("gst-pipeline: {}{} ! videoconvert ! qtvideosink name=\"qtvideosink\" sync=false", mBasePipeline, flipElement))));
     play();
 }
 
@@ -103,6 +140,69 @@ auto GstVideoWidget::stop() -> void {
     mPlayer->stop();
 }
 
+void GstVideoWidget::sampleColorAtCursor() {
+    if (mLastFrame.isNull() || width() == 0 || height() == 0) return;
+
+    QPoint framePos(mLastCursorPos.x() * mLastFrame.width() / width(),
+                    mLastCursorPos.y() * mLastFrame.height() / height());
+    if (!mLastFrame.rect().contains(framePos)) return;
+
+    mLastPickedColor = mLastFrame.pixelColor(framePos);
+    emit colorPicked(mLastPickedColor);
+}
+
+void GstVideoWidget::updateFrame(QImage const& frame) {
+    mLastFrame = frame;
+    if (mMouseInWidget) sampleColorAtCursor();
+    update();
+}
+
+void GstVideoWidget::paintEvent(QPaintEvent* event) {
+    Q_UNUSED(event);
+    QPainter painter(this);
+
+    if (!mLastFrame.isNull()) {
+        painter.drawImage(rect(), mLastFrame);
+    } else {
+        painter.fillRect(rect(), Qt::black);
+        painter.setPen(Qt::white);
+        painter.drawText(rect(), Qt::AlignCenter, "No Video Frame Received");
+    }
+
+    if (mMouseInWidget) {
+        int x = mLastCursorPos.x();
+        int y = mLastCursorPos.y();
+        constexpr int arm = 8;
+
+        painter.setPen(QPen(QColor(0, 0, 0, 180), 3));
+        painter.drawLine(x - arm, y, x + arm, y);
+        painter.drawLine(x, y - arm, x, y + arm);
+
+        painter.setPen(QPen(Qt::white, 1));
+        painter.drawLine(x - arm, y, x + arm, y);
+        painter.drawLine(x, y - arm, x, y + arm);
+    }
+}
+
+void GstVideoWidget::mouseMoveEvent(QMouseEvent* event) {
+    mLastCursorPos = event->pos();
+    if (mMouseInWidget) sampleColorAtCursor();
+    update();
+    QWidget::mouseMoveEvent(event);
+}
+
+void GstVideoWidget::enterEvent(QEvent* event) {
+    mMouseInWidget = true;
+    update();
+    QWidget::enterEvent(event);
+}
+
+void GstVideoWidget::leaveEvent(QEvent* event) {
+    mMouseInWidget = false;
+    update();
+    QWidget::leaveEvent(event);
+}
+
 // --------------------------------------------
 // GstVideoGridWidget
 // --------------------------------------------
@@ -161,6 +261,11 @@ auto GstVideoGridWidget::addGstVideoWidget(std::string const& name, std::string
         return false;
     }
 
+    connect(gstVideoBoxGstVideoWidget, &GstVideoWidget::colorPicked,
+            this, [this, name](QColor const& color) {
+                emit colorPicked(QString::fromStdString(name), color);
+            });
+
     gstVideoBoxLayout->addWidget(gstVideoBoxLabel, 0);
     gstVideoBoxLayout->addWidget(gstVideoBoxGstVideoWidget, 1);
     gstVideoBoxWidget->setLayout(gstVideoBoxLayout);