diff --git a/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java b/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java index 14e40ba140..0b5bd3239d 100644 --- a/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java +++ b/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java @@ -44,7 +44,7 @@ public class CvSink extends ImageSink { * Create a sink for accepting OpenCV images. grabFrame() must be called on the created sink to * get each new image. * - * @param name Source name (arbitrary unique identifier) + * @param name Sink name (arbitrary unique identifier) * @param pixelFormat Source pixel format */ public CvSink(String name, PixelFormat pixelFormat) { diff --git a/cscore/src/main/native/cpp/RawSinkImpl.cpp b/cscore/src/main/native/cpp/RawSinkImpl.cpp index 3e38cb81c3..355b0913e4 100644 --- a/cscore/src/main/native/cpp/RawSinkImpl.cpp +++ b/cscore/src/main/native/cpp/RawSinkImpl.cpp @@ -63,6 +63,11 @@ uint64_t RawSinkImpl::GrabFrame(WPI_RawFrame& image) { } uint64_t RawSinkImpl::GrabFrame(WPI_RawFrame& image, double timeout) { + return GrabFrame(image, timeout, 0); +} + +uint64_t RawSinkImpl::GrabFrame(WPI_RawFrame& image, double timeout, + uint64_t lastFrameTime) { SetEnabled(true); auto source = GetSource(); @@ -72,7 +77,7 @@ uint64_t RawSinkImpl::GrabFrame(WPI_RawFrame& image, double timeout) { return 0; } - auto frame = source->GetNextFrame(timeout); // blocks + auto frame = source->GetNextFrame(timeout, lastFrameTime); // blocks if (!frame) { // Bad frame; sleep for 20 ms so we don't consume all processor time. std::this_thread::sleep_for(std::chrono::milliseconds(20)); @@ -183,6 +188,18 @@ uint64_t GrabSinkFrameTimeout(CS_Sink sink, WPI_RawFrame& image, double timeout, return static_cast(*data->sink).GrabFrame(image, timeout); } +uint64_t GrabSinkFrameTimeoutLastTime(CS_Sink sink, WPI_RawFrame& image, + double timeout, uint64_t lastFrameTime, + CS_Status* status) { + auto data = Instance::GetInstance().GetSink(sink); + if (!data || (data->kind & SinkMask) == 0) { + *status = CS_INVALID_HANDLE; + return 0; + } + return static_cast(*data->sink) + .GrabFrame(image, timeout, lastFrameTime); +} + } // namespace cs extern "C" { @@ -209,4 +226,13 @@ uint64_t CS_GrabRawSinkFrameTimeout(CS_Sink sink, struct WPI_RawFrame* image, return cs::GrabSinkFrameTimeout(sink, *image, timeout, status); } +uint64_t CS_GrabRawSinkFrameTimeoutWithFrameTime(CS_Sink sink, + struct WPI_RawFrame* image, + double timeout, + uint64_t lastFrameTime, + CS_Status* status) { + return cs::GrabSinkFrameTimeoutLastTime(sink, *image, timeout, lastFrameTime, + status); +} + } // extern "C" diff --git a/cscore/src/main/native/cpp/RawSinkImpl.h b/cscore/src/main/native/cpp/RawSinkImpl.h index ac719764ec..423f3b4b22 100644 --- a/cscore/src/main/native/cpp/RawSinkImpl.h +++ b/cscore/src/main/native/cpp/RawSinkImpl.h @@ -34,10 +34,15 @@ class RawSinkImpl : public SinkImpl { uint64_t GrabFrame(WPI_RawFrame& frame); uint64_t GrabFrame(WPI_RawFrame& frame, double timeout); + // Wait for a frame with a time other than lastFrameTime + uint64_t GrabFrame(WPI_RawFrame& frame, double timeout, + uint64_t lastFrameTime); private: void ThreadMain(); + // Copies the image from incomingFrame into rawFrame, converting where + // necessary to the resolution of rawFrame uint64_t GrabFrameImpl(WPI_RawFrame& rawFrame, Frame& incomingFrame); std::atomic_bool m_active; // set to false to terminate threads diff --git a/cscore/src/main/native/cpp/SourceImpl.cpp b/cscore/src/main/native/cpp/SourceImpl.cpp index f2cc0d8ccc..067ba15014 100644 --- a/cscore/src/main/native/cpp/SourceImpl.cpp +++ b/cscore/src/main/native/cpp/SourceImpl.cpp @@ -84,12 +84,17 @@ Frame SourceImpl::GetNextFrame() { return m_frame; } -Frame SourceImpl::GetNextFrame(double timeout) { +Frame SourceImpl::GetNextFrame(double timeout, Frame::Time lastFrameTime) { std::unique_lock lock{m_frameMutex}; - auto oldTime = m_frame.GetTime(); + + if (lastFrameTime == 0) { + lastFrameTime = m_frame.GetTime(); + } + + // Wait unitl m_frame has a timestamp other than lastFrameTime if (!m_frameCv.wait_for( lock, std::chrono::milliseconds(static_cast(timeout * 1000)), - [=, this] { return m_frame.GetTime() != oldTime; })) { + [=, this] { return m_frame.GetTime() != lastFrameTime; })) { m_frame = Frame{*this, "timed out getting frame", wpi::Now()}; } return m_frame; diff --git a/cscore/src/main/native/cpp/SourceImpl.h b/cscore/src/main/native/cpp/SourceImpl.h index 840fb5d7ed..f023e81f2d 100644 --- a/cscore/src/main/native/cpp/SourceImpl.h +++ b/cscore/src/main/native/cpp/SourceImpl.h @@ -98,7 +98,8 @@ class SourceImpl : public PropertyContainer { // Blocking function that waits for the next frame and returns it (with // timeout in seconds). If timeout expires, returns empty frame. - Frame GetNextFrame(double timeout); + // If lastFrameTime==0, uses m_frame.GetTime() for lastFrameTime + Frame GetNextFrame(double timeout, Frame::Time lastFrameTime = 0); // Force a wakeup of all GetNextFrame() callers by sending an empty frame. void Wakeup(); diff --git a/cscore/src/main/native/include/cscore_cv.h b/cscore/src/main/native/include/cscore_cv.h index 237c3946af..d703582c06 100644 --- a/cscore/src/main/native/include/cscore_cv.h +++ b/cscore/src/main/native/include/cscore_cv.h @@ -151,6 +151,27 @@ class CvSink : public ImageSink { [[nodiscard]] uint64_t GrabFrameNoTimeoutDirect(cv::Mat& image); + /** + * Wait for the next frame and get the image. + * Times out (returning 0) after timeout seconds. + * The provided image will have the pixelFormat this class was constructed + * with. The data is backed by data in the CvSink. It will be invalidated by + * any grabFrame*() call on the sink. + * + *

If lastFrameTime is provided and non-zero, the sink will fill image with + * the first frame from the source that is not equal to lastFrameTime. If + * lastFrameTime is zero, the time of the current frame owned by the CvSource + * is used, and this function will block until the connected CvSource provides + * a new frame. + * + * @return Frame time, or 0 on error (call GetError() to obtain the error + * message); the frame time is in the same time base as wpi::Now(), + * and is in 1 us increments. + */ + [[nodiscard]] + uint64_t GrabFrameDirectLastTime(cv::Mat& image, uint64_t lastFrameTime, + double timeout = 0.225); + private: constexpr int GetCvFormat(WPI_PixelFormat pixelFormat); @@ -365,6 +386,25 @@ inline uint64_t CvSink::GrabFrameNoTimeoutDirect(cv::Mat& image) { return timestamp; } +inline uint64_t CvSink::GrabFrameDirectLastTime(cv::Mat& image, + uint64_t lastFrameTime, + double timeout) { + rawFrame.height = 0; + rawFrame.width = 0; + rawFrame.stride = 0; + rawFrame.pixelFormat = pixelFormat; + auto timestamp = GrabSinkFrameTimeoutLastTime(m_handle, rawFrame, timeout, + lastFrameTime, &m_status); + if (m_status != CS_OK) { + return 0; + } + image = + cv::Mat{rawFrame.height, rawFrame.width, + GetCvFormat(static_cast(rawFrame.pixelFormat)), + rawFrame.data, static_cast(rawFrame.stride)}; + return timestamp; +} + } // namespace cs #endif // CSCORE_CSCORE_CV_H_ diff --git a/cscore/src/main/native/include/cscore_raw.h b/cscore/src/main/native/include/cscore_raw.h index 6333bca655..e7a47fc9ee 100644 --- a/cscore/src/main/native/include/cscore_raw.h +++ b/cscore/src/main/native/include/cscore_raw.h @@ -27,6 +27,11 @@ uint64_t CS_GrabRawSinkFrame(CS_Sink sink, struct WPI_RawFrame* rawImage, CS_Status* status); uint64_t CS_GrabRawSinkFrameTimeout(CS_Sink sink, struct WPI_RawFrame* rawImage, double timeout, CS_Status* status); +uint64_t CS_GrabRawSinkFrameTimeoutWithFrameTime(CS_Sink sink, + struct WPI_RawFrame* rawImage, + double timeout, + uint64_t lastFrameTime, + CS_Status* status); CS_Sink CS_CreateRawSink(const struct WPI_String* name, CS_Bool isCv, CS_Status* status); @@ -67,6 +72,9 @@ void PutSourceFrame(CS_Source source, const WPI_RawFrame& image, uint64_t GrabSinkFrame(CS_Sink sink, WPI_RawFrame& image, CS_Status* status); uint64_t GrabSinkFrameTimeout(CS_Sink sink, WPI_RawFrame& image, double timeout, CS_Status* status); +uint64_t GrabSinkFrameTimeoutLastTime(CS_Sink sink, WPI_RawFrame& image, + double timeout, uint64_t lastFrameTime, + CS_Status* status); /** * A source for user code to provide video frames as raw bytes. @@ -163,6 +171,24 @@ class RawSink : public ImageSink { */ [[nodiscard]] uint64_t GrabFrameNoTimeout(wpi::RawFrame& image) const; + + /** + * Wait for the next frame and get the image. May block forever. + * The provided image will have three 8-bit channels stored in BGR order. + * + *

If lastFrameTime is provided and non-zero, the sink will fill image with + * the first frame from the source that is not equal to lastFrameTime. If + * lastFrameTime is zero, the time of the current frame owned by the CvSource + * is used, and this function will block until the connected CvSource provides + * a new frame. + * + * @return Frame time, or 0 on error (call GetError() to obtain the error + * message); the frame time is in the same time base as wpi::Now(), + * and is in 1 us increments. + */ + [[nodiscard]] + uint64_t GrabFrameLastTime(wpi::RawFrame& image, uint64_t lastFrameTime, + double timeout = 0.225) const; }; inline RawSource::RawSource(std::string_view name, const VideoMode& mode) { @@ -199,6 +225,14 @@ inline uint64_t RawSink::GrabFrameNoTimeout(wpi::RawFrame& image) const { m_status = 0; return GrabSinkFrame(m_handle, image, &m_status); } + +inline uint64_t RawSink::GrabFrameLastTime(wpi::RawFrame& image, + uint64_t lastFrameTime, + double timeout) const { + m_status = 0; + return GrabSinkFrameTimeoutLastTime(m_handle, image, timeout, lastFrameTime, + &m_status); +} /** @} */ } // namespace cs