CameraServer: Add switched camera support (#1600)

This adds a new function "addSwitchedCamera" that creates and publishes a
virtual camera where the published stream information is consistent even if
the mjpeg server source is switched to a different camera.

Previously, changing the source of the mjpeg server resulted in updating the
stream information published for that source.

cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java

@@ -65,6 +65,8 @@ public final class CameraServer {
   private final Map<String, VideoSource> m_sources;
   private final Map<String, VideoSink> m_sinks;
   private final Map<Integer, NetworkTable> m_tables;  // indexed by source handle
+  // source handle indexed by sink handle
+  private final Map<Integer, Integer> m_fixedSources;
   private final NetworkTable m_publishTable;
   private final VideoListener m_videoListener; //NOPMD
   private final int m_tableListener; //NOPMD
@@ -157,14 +159,20 @@ public final class CameraServer {
     return values;
   }
 
-  @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP"})
+  @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP", "PMD.CyclomaticComplexity"})
   private synchronized void updateStreamValues() {
     // Over all the sinks...
     for (VideoSink i : m_sinks.values()) {
       int sink = i.getHandle();
 
       // Get the source's subtable (if none exists, we're done)
-      int source = CameraServerJNI.getSinkSource(sink);
+      int source;
+      Integer fixedSource = m_fixedSources.get(sink);
+      if (fixedSource != null) {
+        source = fixedSource;
+      } else {
+        source = CameraServerJNI.getSinkSource(sink);
+      }
       if (source == 0) {
         continue;
       }
@@ -295,6 +303,7 @@ public final class CameraServer {
     m_defaultUsbDevice = new AtomicInteger();
     m_sources = new Hashtable<>();
     m_sinks = new Hashtable<>();
+    m_fixedSources = new Hashtable<>();
     m_tables = new Hashtable<>();
     m_publishTable = NetworkTableInstance.getDefault().getTable(kPublishName);
     m_nextPort = kBasePort;
@@ -596,6 +605,21 @@ public final class CameraServer {
     return camera;
   }
 
+  /**
+   * Adds a virtual camera for switching between two streams.  Unlike the
+   * other addCamera methods, this returns a VideoSink rather than a
+   * VideoSource.  Calling setSource() on the returned object can be used
+   * to switch the actual source of the stream.
+   */
+  public MjpegServer addSwitchedCamera(String name) {
+    // create a dummy CvSource
+    CvSource source = new CvSource(name, VideoMode.PixelFormat.kMJPEG, 160, 120, 30);
+    MjpegServer server = startAutomaticCapture(source);
+    m_fixedSources.put(server.getHandle(), source.getHandle());
+
+    return server;
+  }
+
   /**
    * Get OpenCV access to the primary camera feed.  This allows you to
    * get images from the camera for image processing on the roboRIO.

cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp

@@ -37,6 +37,7 @@ struct CameraServer::Impl {
   std::string m_primarySourceName;
   wpi::StringMap<cs::VideoSource> m_sources;
   wpi::StringMap<cs::VideoSink> m_sinks;
+  wpi::DenseMap<CS_Sink, CS_Source> m_fixedSources;
   wpi::DenseMap<CS_Source, std::shared_ptr<nt::NetworkTable>> m_tables;
   std::shared_ptr<nt::NetworkTable> m_publishTable;
   cs::VideoListener m_videoListener;
@@ -156,7 +157,8 @@ void CameraServer::Impl::UpdateStreamValues() {
     CS_Sink sink = i.second.GetHandle();
 
     // Get the source's subtable (if none exists, we're done)
-    CS_Source source = cs::GetSinkSource(sink, &status);
+    CS_Source source = m_fixedSources.lookup(sink);
+    if (source == 0) source = cs::GetSinkSource(sink, &status);
     if (source == 0) continue;
     auto table = m_tables.lookup(source);
     if (table) {
@@ -538,6 +540,15 @@ cs::AxisCamera CameraServer::AddAxisCamera(const wpi::Twine& name,
   return camera;
 }
 
+cs::MjpegServer CameraServer::AddSwitchedCamera(const wpi::Twine& name) {
+  // create a dummy CvSource
+  cs::CvSource source{name, cs::VideoMode::PixelFormat::kMJPEG, 160, 120, 30};
+  cs::MjpegServer server = StartAutomaticCapture(source);
+  m_impl->m_fixedSources[server.GetHandle()] = source.GetHandle();
+
+  return server;
+}
+
 cs::MjpegServer CameraServer::StartAutomaticCapture(
     const cs::VideoSource& camera) {
   AddCamera(camera);

cameraserver/src/main/native/include/cameraserver/CameraServer.h

@@ -173,6 +173,14 @@ class CameraServer {
   cs::AxisCamera AddAxisCamera(const wpi::Twine& name,
                                std::initializer_list<T> hosts);
 
+  /**
+   * Adds a virtual camera for switching between two streams.  Unlike the
+   * other addCamera methods, this returns a VideoSink rather than a
+   * VideoSource.  Calling SetSource() on the returned object can be used
+   * to switch the actual source of the stream.
+   */
+  cs::MjpegServer AddSwitchedCamera(const wpi::Twine& name);
+
   /**
    * Get OpenCV access to the primary camera feed.  This allows you to
    * get images from the camera for image processing on the roboRIO.