Add GetNextFrame timeout to CvSink and MjpegServer.

MjpegServer uses the timeout to generate keep-alives to any clients
(which helps detect disconnects and avoid stale client threads).

CvSink GrabFrame now defaults to a timeout, but the timeout can be
changed by the user, or the old no-timeout version is now available
as GrabFrameNoTimeout.

cscore-jni.def

@@ -85,6 +85,7 @@ CS_SetCameraExposureAuto @82
 CS_SetCameraExposureHoldCurrent @83
 CS_SetCameraExposureManual @84
 CS_SetDefaultLogger @85
+CS_GrabSinkFrameTimeout @86
 
 ; JNI functions
 JNI_OnLoad
@@ -153,6 +154,7 @@ Java_edu_wpi_cscore_CameraServerJNI_getMjpegServerListenAddress
 Java_edu_wpi_cscore_CameraServerJNI_getMjpegServerPort
 Java_edu_wpi_cscore_CameraServerJNI_setSinkDescription
 Java_edu_wpi_cscore_CameraServerJNI_grabSinkFrame
+Java_edu_wpi_cscore_CameraServerJNI_grabSinkFrameTimeout
 Java_edu_wpi_cscore_CameraServerJNI_getSinkError
 Java_edu_wpi_cscore_CameraServerJNI_setSinkEnabled
 Java_edu_wpi_cscore_CameraServerJNI_addListener

cscore.def

@@ -85,3 +85,4 @@ CS_SetCameraExposureAuto @82
 CS_SetCameraExposureHoldCurrent @83
 CS_SetCameraExposureManual @84
 CS_SetDefaultLogger @85
+CS_GrabSinkFrameTimeout @86

include/cscore_c.h

@@ -322,6 +322,8 @@ int CS_GetMjpegServerPort(CS_Sink sink, CS_Status* status);
 void CS_SetSinkDescription(CS_Sink sink, const char* description,
                            CS_Status* status);
 uint64_t CS_GrabSinkFrame(CS_Sink sink, struct CvMat* image, CS_Status* status);
+uint64_t CS_GrabSinkFrameTimeout(CS_Sink sink, struct CvMat* image,
+                                 double timeout, CS_Status* status);
 char* CS_GetSinkError(CS_Sink sink, CS_Status* status);
 void CS_SetSinkEnabled(CS_Sink sink, CS_Bool enabled, CS_Status* status);
 

include/cscore_cpp.h

@@ -286,6 +286,8 @@ int GetMjpegServerPort(CS_Sink sink, CS_Status* status);
 void SetSinkDescription(CS_Sink sink, llvm::StringRef description,
                         CS_Status* status);
 uint64_t GrabSinkFrame(CS_Sink sink, cv::Mat& image, CS_Status* status);
+uint64_t GrabSinkFrameTimeout(CS_Sink sink, cv::Mat& image, double timeout,
+                              CS_Status* status);
 std::string GetSinkError(CS_Sink sink, CS_Status* status);
 llvm::StringRef GetSinkError(CS_Sink sink, llvm::SmallVectorImpl<char>& buf,
                              CS_Status* status);
@@ -332,6 +334,8 @@ std::vector<std::string> GetNetworkInterfaces();
 // C functions taking a cv::Mat* for specific interop implementations
 extern "C" {
 uint64_t CS_GrabSinkFrameCpp(CS_Sink sink, cv::Mat* image, CS_Status* status);
+uint64_t CS_GrabSinkFrameTimeoutCpp(CS_Sink sink, cv::Mat* image,
+                                    double timeout, CS_Status* status);
 void CS_PutSourceFrameCpp(CS_Source source, cv::Mat* image, CS_Status* status);
 }
 

include/cscore_oo.h

@@ -544,10 +544,17 @@ class CvSink : public VideoSink {
   void SetDescription(llvm::StringRef description);
 
   /// Wait for the next frame and get the image.
+  /// Times out (returning 0) after timeout seconds.
   /// The provided image will have three 8-bit channels stored in BGR order.
   /// @return Frame time, or 0 on error (call GetError() to obtain the error
   ///         message);
-  uint64_t GrabFrame(cv::Mat& image) const;
+  uint64_t GrabFrame(cv::Mat& image, double timeout = 0.225) 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.
+  /// @return Frame time, or 0 on error (call GetError() to obtain the error
+  ///         message);
+  uint64_t GrabFrameNoTimeout(cv::Mat& image) const;
 
   /// Get error string.  Call this if WaitForFrame() returns 0 to determine
   /// what the error is.

include/cscore_oo.inl

@@ -465,7 +465,12 @@ inline void CvSink::SetDescription(llvm::StringRef description) {
   SetSinkDescription(m_handle, description, &m_status);
 }
 
-inline uint64_t CvSink::GrabFrame(cv::Mat& image) const {
+inline uint64_t CvSink::GrabFrame(cv::Mat& image, double timeout) const {
+  m_status = 0;
+  return GrabSinkFrameTimeout(m_handle, image, timeout, &m_status);
+}
+
+inline uint64_t CvSink::GrabFrameNoTimeout(cv::Mat& image) const {
   m_status = 0;
   return GrabSinkFrame(m_handle, image, &m_status);
 }

java/lib/CameraServerJNI.cpp

@@ -1261,6 +1261,21 @@ JNIEXPORT jlong JNICALL Java_edu_wpi_cscore_CameraServerJNI_grabSinkFrame
   return rv;
 }
 
+/*
+ * Class:     edu_wpi_cscore_CameraServerJNI
+ * Method:    grabSinkFrameTimeout
+ * Signature: (IJD)J
+ */
+JNIEXPORT jlong JNICALL Java_edu_wpi_cscore_CameraServerJNI_grabSinkFrameTimeout
+  (JNIEnv *env, jclass, jint sink, jlong imageNativeObj, jdouble timeout)
+{
+  cv::Mat& image = *((cv::Mat*)imageNativeObj);
+  CS_Status status = 0;
+  auto rv = cs::GrabSinkFrameTimeout(sink, image, timeout, &status);
+  CheckStatus(env, status);
+  return rv;
+}
+
 /*
  * Class:     edu_wpi_cscore_CameraServerJNI
  * Method:    getSinkError

java/src/edu/wpi/cscore/CameraServerJNI.java

@@ -193,6 +193,7 @@ public class CameraServerJNI {
   //
   public static native void setSinkDescription(int sink, String description);
   public static native long grabSinkFrame(int sink, long imageNativeObj);
+  public static native long grabSinkFrameTimeout(int sink, long imageNativeObj, double timeout);
   public static native String getSinkError(int sink);
   public static native void setSinkEnabled(int sink, boolean enabled);
 

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

@@ -39,10 +39,28 @@ public class CvSink extends VideoSink {
   }
 
   /// Wait for the next frame and get the image.
+  /// Times out (returning 0) after 0.225 seconds.
   /// The provided image will have three 3-bit channels stored in BGR order.
   /// @return Frame time, or 0 on error (call GetError() to obtain the error
   ///         message);
   public long grabFrame(Mat image) {
+    return grabFrame(image, 0.225);
+  }
+
+  /// Wait for the next frame and get the image.
+  /// Times out (returning 0) after timeout seconds.
+  /// The provided image will have three 3-bit channels stored in BGR order.
+  /// @return Frame time, or 0 on error (call GetError() to obtain the error
+  ///         message);
+  public long grabFrame(Mat image, double timeout) {
+    return CameraServerJNI.grabSinkFrameTimeout(m_handle, image.nativeObj, timeout);
+  }
+
+  /// Wait for the next frame and get the image.  May block forever.
+  /// The provided image will have three 3-bit channels stored in BGR order.
+  /// @return Frame time, or 0 on error (call GetError() to obtain the error
+  ///         message);
+  public long grabFrameNoTimeout(Mat image) {
     return CameraServerJNI.grabSinkFrame(m_handle, image.nativeObj);
   }
 

src/CvSinkImpl.cpp

@@ -68,6 +68,32 @@ uint64_t CvSinkImpl::GrabFrame(cv::Mat& image) {
   return frame.GetTime();
 }
 
+uint64_t CvSinkImpl::GrabFrame(cv::Mat& image, double timeout) {
+  SetEnabled(true);
+
+  auto source = GetSource();
+  if (!source) {
+    // Source disconnected; sleep for one second
+    std::this_thread::sleep_for(std::chrono::seconds(1));
+    return 0;
+  }
+
+  auto frame = source->GetNextFrame(timeout);  // 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));
+    return 0;  // signal error
+  }
+
+  if (!frame.GetCv(image)) {
+    // Shouldn't happen, but just in case...
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+    return 0;
+  }
+
+  return frame.GetTime();
+}
+
 // Send HTTP response and a stream of JPG-frames
 void CvSinkImpl::ThreadMain() {
   Enable();
@@ -128,6 +154,16 @@ uint64_t GrabSinkFrame(CS_Sink sink, cv::Mat& image, CS_Status* status) {
   return static_cast<CvSinkImpl&>(*data->sink).GrabFrame(image);
 }
 
+uint64_t GrabSinkFrameTimeout(CS_Sink sink, cv::Mat& image, double timeout,
+                              CS_Status* status) {
+  auto data = Sinks::GetInstance().Get(sink);
+  if (!data || data->kind != CS_SINK_CV) {
+    *status = CS_INVALID_HANDLE;
+    return 0;
+  }
+  return static_cast<CvSinkImpl&>(*data->sink).GrabFrame(image, timeout);
+}
+
 std::string GetSinkError(CS_Sink sink, CS_Status* status) {
   auto data = Sinks::GetInstance().Get(sink);
   if (!data || data->kind != CS_SINK_CV) {
@@ -182,10 +218,21 @@ uint64_t CS_GrabSinkFrame(CS_Sink sink, struct CvMat* image,
   return cs::GrabSinkFrame(sink, mat, status);
 }
 
+uint64_t CS_GrabSinkFrameTimeout(CS_Sink sink, struct CvMat* image,
+                                 double timeout, CS_Status* status) {
+  auto mat = cv::cvarrToMat(image);
+  return cs::GrabSinkFrameTimeout(sink, mat, timeout, status);
+}
+
 uint64_t CS_GrabSinkFrameCpp(CS_Sink sink, cv::Mat* image, CS_Status* status) {
    return cs::GrabSinkFrame(sink, *image, status);
 }
 
+uint64_t CS_GrabSinkFrameTimeoutCpp(CS_Sink sink, cv::Mat* image,
+                                    double timeout, CS_Status* status) {
+  return cs::GrabSinkFrameTimeout(sink, *image, timeout, status);
+}
+
 char* CS_GetSinkError(CS_Sink sink, CS_Status* status) {
   llvm::SmallString<128> buf;
   auto str = cs::GetSinkError(sink, buf, status);

src/CvSinkImpl.h

@@ -39,6 +39,7 @@ class CvSinkImpl : public SinkImpl {
   void Stop();
 
   uint64_t GrabFrame(cv::Mat& image);
+  uint64_t GrabFrame(cv::Mat& image, double timeout);
 
  private:
   void ThreadMain();

src/MjpegServerImpl.cpp

@@ -571,15 +571,17 @@ void MjpegServerImpl::ConnThread::SendStream(wpi::raw_socket_ostream& os) {
   while (m_active && !os.has_error()) {
     auto source = GetSource();
     if (!source) {
-      // Source disconnected; sleep for one second
-      std::this_thread::sleep_for(std::chrono::seconds(1));
+      // Source disconnected; sleep so we don't consume all processor time.
+      os << "\r\n";  // Keep connection alive
+      std::this_thread::sleep_for(std::chrono::milliseconds(200));
       continue;
     }
     SDEBUG4("waiting for frame");
-    Frame frame = source->GetNextFrame();  // blocks
+    Frame frame = source->GetNextFrame(0.225);  // blocks
     if (!m_active) break;
     if (!frame) {
       // Bad frame; sleep for 20 ms so we don't consume all processor time.
+      os << "\r\n";  // Keep connection alive
       std::this_thread::sleep_for(std::chrono::milliseconds(20));
       continue;
     }

src/SourceImpl.cpp

@@ -11,6 +11,7 @@
 #include <cstring>
 
 #include "llvm/STLExtras.h"
+#include "support/timestamp.h"
 
 #include "Log.h"
 #include "Notifier.h"
@@ -73,6 +74,17 @@ Frame SourceImpl::GetNextFrame() {
   return m_frame;
 }
 
+Frame SourceImpl::GetNextFrame(double timeout) {
+  std::unique_lock<std::mutex> lock{m_frameMutex};
+  auto oldTime = m_frame.GetTime();
+  if (!m_frameCv.wait_for(
+          lock, std::chrono::milliseconds(static_cast<int>(timeout * 1000)),
+          [=] { return m_frame.GetTime() != oldTime; })) {
+    m_frame = Frame{*this, "timed out getting frame", wpi::Now()};
+  }
+  return m_frame;
+}
+
 void SourceImpl::Wakeup() {
   {
     std::lock_guard<std::mutex> lock{m_frameMutex};

src/SourceImpl.h

@@ -82,6 +82,10 @@ class SourceImpl {
   // Blocking function that waits for the next frame and returns it.
   Frame GetNextFrame();
 
+  // 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);
+
   // Force a wakeup of all GetNextFrame() callers by sending an empty frame.
   void Wakeup();