[cameraserver] Add getVideo() pixelFormat overload (wpilibsuite#5966)

KangarooKoala · Nov 27, 2023 · a74db52 · a74db52
1 parent a7eb422
commit a74db52
Show file tree

Hide file tree

Showing 3 changed files with 94 additions and 0 deletions.
diff --git a/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java b/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
@@ -745,6 +745,34 @@ public static CvSink getVideo(VideoSource camera) {
     return newsink;
   }
 
+  /**
+   * Get OpenCV access to the specified camera. This allows you to get images from the camera for
+   * image processing on the roboRIO.
+   *
+   * @param camera Camera (e.g. as returned by startAutomaticCapture).
+   * @param pixelFormat Desired pixelFormat of the camera
+   * @return OpenCV sink for the specified camera
+   */
+  public static CvSink getVideo(VideoSource camera, PixelFormat pixelFormat) {
+    String name = "opencv_" + camera.getName();
+
+    synchronized (CameraServer.class) {
+      VideoSink sink = m_sinks.get(name);
+      if (sink != null) {
+        VideoSink.Kind kind = sink.getKind();
+        if (kind != VideoSink.Kind.kCv) {
+          throw new VideoException("expected OpenCV sink, but got " + kind);
+        }
+        return (CvSink) sink;
+      }
+    }
+
+    CvSink newsink = new CvSink(name, pixelFormat);
+    newsink.setSource(camera);
+    addServer(newsink);
+    return newsink;
+  }
+
   /**
    * Get OpenCV access to the specified camera. This allows you to get images from the camera for
    * image processing on the roboRIO.

diff --git a/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp b/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp
@@ -622,6 +622,33 @@ cs::CvSink CameraServer::GetVideo(const cs::VideoSource& camera) {
   return newsink;
 }
 
+cs::CvSink CameraServer::GetVideo(const cs::VideoSource& camera,
+                                  cs::VideoMode::PixelFormat pixelFormat) {
+  auto& inst = ::GetInstance();
+  wpi::SmallString<64> name{"opencv_"};
+  name += camera.GetName();
+
+  {
+    std::scoped_lock lock(inst.m_mutex);
+    auto it = inst.m_sinks.find(name);
+    if (it != inst.m_sinks.end()) {
+      auto kind = it->second.GetKind();
+      if (kind != cs::VideoSink::kCv) {
+        auto csShared = GetCameraServerShared();
+        csShared->SetCameraServerError("expected OpenCV sink, but got {}",
+                                       static_cast<int>(kind));
+        return cs::CvSink{};
+      }
+      return *static_cast<cs::CvSink*>(&it->second);
+    }
+  }
+
+  cs::CvSink newsink{name.str(), pixelFormat};
+  newsink.SetSource(camera);
+  AddServer(newsink);
+  return newsink;
+}
+
 cs::CvSink CameraServer::GetVideo(std::string_view name) {
   auto& inst = ::GetInstance();
   cs::VideoSource source;
@@ -638,6 +665,23 @@ cs::CvSink CameraServer::GetVideo(std::string_view name) {
   return GetVideo(source);
 }
 
+cs::CvSink CameraServer::GetVideo(std::string_view name,
+                                  cs::VideoMode::PixelFormat pixelFormat) {
+  auto& inst = ::GetInstance();
+  cs::VideoSource source;
+  {
+    std::scoped_lock lock(inst.m_mutex);
+    auto it = inst.m_sources.find(name);
+    if (it == inst.m_sources.end()) {
+      auto csShared = GetCameraServerShared();
+      csShared->SetCameraServerError("could not find camera {}", name);
+      return cs::CvSink{};
+    }
+    source = it->second;
+  }
+  return GetVideo(source, pixelFormat);
+}
+
 cs::CvSource CameraServer::PutVideo(std::string_view name, int width,
                                     int height) {
   ::GetInstance();

diff --git a/cameraserver/src/main/native/include/cameraserver/CameraServer.h b/cameraserver/src/main/native/include/cameraserver/CameraServer.h
@@ -191,6 +191,17 @@ class CameraServer {
    */
   static cs::CvSink GetVideo(const cs::VideoSource& camera);
 
+  /**
+   * Get OpenCV access to the specified camera.  This allows you to get
+   * images from the camera for image processing on the roboRIO.
+   *
+   * @param camera Camera (e.g. as returned by startAutomaticCapture).
+   * @param pixelFormat The desired pixelFormat of captured frames from the
+   * camera
+   */
+  static cs::CvSink GetVideo(const cs::VideoSource& camera,
+                             cs::VideoMode::PixelFormat pixelFormat);
+
   /**
    * Get OpenCV access to the specified camera.  This allows you to get
    * images from the camera for image processing on the roboRIO.
@@ -199,6 +210,17 @@ class CameraServer {
    */
   static cs::CvSink GetVideo(std::string_view name);
 
+  /**
+   * Get OpenCV access to the specified camera.  This allows you to get
+   * images from the camera for image processing on the roboRIO.
+   *
+   * @param name Camera name
+   * @param pixelFormat The desired pixelFormat of captured frames from the
+   * camera
+   */
+  static cs::CvSink GetVideo(std::string_view name,
+                             cs::VideoMode::PixelFormat pixelFormat);
+
   /**
    * Create a MJPEG stream with OpenCV input. This can be called to pass custom
    * annotated images to the dashboard.