diff --git a/src/CvSinkImpl.cpp b/src/CvSinkImpl.cpp index de2fcc44a9..0b4eeb6e46 100644 --- a/src/CvSinkImpl.cpp +++ b/src/CvSinkImpl.cpp @@ -9,6 +9,7 @@ #include "llvm/SmallString.h" #include "opencv2/core/core.hpp" +#include "opencv2/imgproc/imgproc.hpp" #include "opencv2/highgui/highgui.hpp" #include "cscore_cpp.h" @@ -46,8 +47,26 @@ uint64_t CvSinkImpl::GrabFrame(cv::Mat& image) { if (!source) return 0; auto frame = source->GetNextFrame(); // blocks if (!frame) return 0; // signal error - cv::imdecode(cv::InputArray{frame.data(), static_cast(frame.size())}, - cv::IMREAD_COLOR, &image); + switch (frame.GetPixelFormat()) { + case VideoMode::kMJPEG: + cv::imdecode(cv::InputArray{frame.data(), static_cast(frame.size())}, + cv::IMREAD_COLOR, &image); + // Check to see if we successfully decoded + if (image.cols != frame.width() || image.rows != frame.height()) return 0; + break; + case VideoMode::kYUYV: + cv::cvtColor(cv::Mat{frame.height(), frame.width(), CV_8UC2, + frame.data()}, + image, cv::COLOR_YUV2BGR_YUYV); + break; + case VideoMode::kRGB565: + cv::cvtColor(cv::Mat{frame.height(), frame.width(), CV_8UC2, + frame.data()}, + image, cv::COLOR_BGR5652RGB); + break; + default: + return 0; + } return frame.time(); } diff --git a/src/CvSourceImpl.cpp b/src/CvSourceImpl.cpp index 72b309dba7..80b4577f08 100644 --- a/src/CvSourceImpl.cpp +++ b/src/CvSourceImpl.cpp @@ -93,7 +93,7 @@ void CvSourceImpl::PutFrame(cv::Mat& image) { } cv::imencode(".jpg", image, m_jpegBuf, m_compressionParams); SourceImpl::PutFrame( - VideoMode::kMJPEG, + VideoMode::kMJPEG, image.cols, image.rows, llvm::StringRef(reinterpret_cast(m_jpegBuf.data()), m_jpegBuf.size()), wpi::Now()); diff --git a/src/Frame.h b/src/Frame.h index 1a6e699337..9adf672ecb 100644 --- a/src/Frame.h +++ b/src/Frame.h @@ -37,6 +37,8 @@ class Frame { std::size_t size; std::size_t capacity; VideoMode::PixelFormat pixelFormat; + int width; + int height; }; public: @@ -81,11 +83,26 @@ class Frame { return m_data->data; } + char* data() { + if (!m_data) return nullptr; + return m_data->data; + } + VideoMode::PixelFormat GetPixelFormat() const { if (!m_data) return VideoMode::kUnknown; return m_data->pixelFormat; } + int width() const { + if (!m_data) return 0; + return m_data->width; + } + + int height() const { + if (!m_data) return 0; + return m_data->height; + } + Time time() const { if (!m_data) return Time{}; return m_data->time; diff --git a/src/MJPEGServerImpl.cpp b/src/MJPEGServerImpl.cpp index f07a7d8688..f40379a9cf 100644 --- a/src/MJPEGServerImpl.cpp +++ b/src/MJPEGServerImpl.cpp @@ -389,7 +389,21 @@ void MJPEGServerImpl::SendStream(wpi::raw_socket_ostream& os) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); continue; } - DEBUG4("HTTP: sending frame size=" << frame.size()); + + const char* data = frame.data(); + std::size_t size = frame.size(); + switch (frame.GetPixelFormat()) { + case VideoMode::kMJPEG: + break; + case VideoMode::kYUYV: + case VideoMode::kRGB565: + default: + // Bad frame; sleep for 10 ms so we don't consume all processor time. + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + continue; + } + + DEBUG4("HTTP: sending frame size=" << size); // print the individual mimetype and the length // sending the content-length fixes random stream disruption observed @@ -398,11 +412,11 @@ void MJPEGServerImpl::SendStream(wpi::raw_socket_ostream& os) { header.clear(); oss << "\r\n--" BOUNDARY "\r\n" << "Content-Type: image/jpeg\r\n" - << "Content-Length: " << frame.size() << "\r\n" + << "Content-Length: " << size << "\r\n" << "X-Timestamp: " << timestamp << "\r\n" << "\r\n"; os << oss.str(); - os << llvm::StringRef(frame.data(), frame.size()); + os << llvm::StringRef(data, size); // os.flush(); } Disable(); diff --git a/src/SourceImpl.cpp b/src/SourceImpl.cpp index 90ebec8163..dcad592c32 100644 --- a/src/SourceImpl.cpp +++ b/src/SourceImpl.cpp @@ -289,8 +289,8 @@ std::vector SourceImpl::EnumerateVideoModes( return m_videoModes; } -void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, - llvm::StringRef data, Frame::Time time) { +void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, int width, + int height, llvm::StringRef data, Frame::Time time) { std::size_t dataSize = data.size(); // If MJPEG, determine if we need to add DHT to it, and allocate enough space @@ -343,6 +343,8 @@ done: frameData->time = time; frameData->size = dataSize; frameData->pixelFormat = pixelFormat; + frameData->width = width; + frameData->height = height; // Copy in image data DEBUG4("Copying data to " << ((void*)frameData->data) << " from " diff --git a/src/SourceImpl.h b/src/SourceImpl.h index d44ffb3833..f82af58a79 100644 --- a/src/SourceImpl.h +++ b/src/SourceImpl.h @@ -116,10 +116,10 @@ class SourceImpl { std::vector EnumerateVideoModes(CS_Status* status) const; protected: - void PutFrame(VideoMode::PixelFormat pixelFormat, llvm::StringRef data, - Frame::Time time); + void PutFrame(VideoMode::PixelFormat pixelFormat, int width, int height, + llvm::StringRef data, Frame::Time time); void PutError(llvm::StringRef msg, Frame::Time time) { - PutFrame(VideoMode::kUnknown, msg, time); + PutFrame(VideoMode::kUnknown, 0, 0, msg, time); } // Notification functions for corresponding atomics diff --git a/src/USBCameraImpl.cpp b/src/USBCameraImpl.cpp index a43f52c493..17ba0e750b 100644 --- a/src/USBCameraImpl.cpp +++ b/src/USBCameraImpl.cpp @@ -59,12 +59,10 @@ static VideoMode::PixelFormat ToPixelFormat(__u32 pixelformat) { switch (pixelformat) { case V4L2_PIX_FMT_MJPEG: return VideoMode::kMJPEG; -#if 0 case V4L2_PIX_FMT_YUYV: return VideoMode::kYUYV; case V4L2_PIX_FMT_RGB565: return VideoMode::kRGB565; -#endif default: return VideoMode::kUnknown; } @@ -557,6 +555,7 @@ void USBCameraImpl::CameraThreadMain() { } PutFrame(static_cast(m_mode.pixelFormat), + m_mode.width, m_mode.height, llvm::StringRef( static_cast(m_buffers[buf.index].m_data), static_cast(buf.bytesused)),