diff --git a/scripts/mock_camera_stream.sh b/scripts/mock_camera_stream.sh new file mode 100755 index 00000000..d06ed7f4 --- /dev/null +++ b/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}" diff --git a/teleoperation/camera_client/include/CameraClientMainWindow.hpp b/teleoperation/camera_client/include/CameraClientMainWindow.hpp index 0bffccb0..8fd12b84 100644 --- a/teleoperation/camera_client/include/CameraClientMainWindow.hpp +++ b/teleoperation/camera_client/include/CameraClientMainWindow.hpp @@ -25,6 +25,17 @@ namespace mrover { std::unordered_map> 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); diff --git a/teleoperation/camera_client/include/GstVideoWidgets.hpp b/teleoperation/camera_client/include/GstVideoWidgets.hpp index cf7a7977..fc4d6aab 100644 --- a/teleoperation/camera_client/include/GstVideoWidgets.hpp +++ b/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 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 diff --git a/teleoperation/camera_client/include/pch.hpp b/teleoperation/camera_client/include/pch.hpp index 71b9e36d..8b60e8e4 100644 --- a/teleoperation/camera_client/include/pch.hpp +++ b/teleoperation/camera_client/include/pch.hpp @@ -9,6 +9,7 @@ #include #include +#include #include #include #include @@ -20,7 +21,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -31,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -39,7 +43,8 @@ #include #include #include -#include +#include +#include #include #include diff --git a/teleoperation/camera_client/src/CameraClientMainWindow.cpp b/teleoperation/camera_client/src/CameraClientMainWindow.cpp index 92af756e..3a9fd220 100644 --- a/teleoperation/camera_client/src/CameraClientMainWindow.cpp +++ b/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 diff --git a/teleoperation/camera_client/src/GstVideoWidgets.cpp b/teleoperation/camera_client/src/GstVideoWidgets.cpp index 09a49e82..e69e979c 100644 --- a/teleoperation/camera_client/src/GstVideoWidgets.cpp +++ b/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 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);