[cscore] Sink: add ability to get most recent frame instead of waiting

cscore/src/main/java/edu/wpi/first/cscore/CvSink.java

@@ -44,7 +44,7 @@ private int getCVFormat(PixelFormat pixelFormat) {
    * 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) {

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<RawSinkImpl&>(*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<RawSinkImpl&>(*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"

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

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<int>(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;

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();

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.
+   *
+   * <p>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<WPI_PixelFormat>(rawFrame.pixelFormat)),
+              rawFrame.data, static_cast<size_t>(rawFrame.stride)};
+  return timestamp;
+}
+
 }  // namespace cs
 
 #endif  // CSCORE_CSCORE_CV_H_

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.
+   *
+   * <p>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