CameraServer: auto-increment startAutomaticCapture(). (#468)

Also add GetServer() functions so the automatically created VideoSink can
be retrieved by user code if desired.

hal/lib/athena/Solenoid.cpp

@@ -55,8 +55,7 @@ HAL_SolenoidHandle HAL_InitializeSolenoidPort(HAL_PortHandle portHandle,
 
   auto handle =
       solenoidHandles.Allocate(module * kNumSolenoidChannels + channel, status);
-  if (handle == HAL_kInvalidHandle) {  // out of resources
+  if (*status != 0) {
-    *status = NO_AVAILABLE_RESOURCES;
     return HAL_kInvalidHandle;
   }
   auto solenoidPort = solenoidHandles.Get(handle);

wpilibc/athena/include/CameraServer.h

@@ -7,6 +7,7 @@
 
 #pragma once
 
+#include <atomic>
 #include <memory>
 #include <mutex>
 #include <string>
@@ -44,8 +45,10 @@ class CameraServer : public ErrorBase {
    * If you also want to perform vision processing on the roboRIO, use
    * getVideo() to get access to the camera images.
    *
-   * This overload calls {@link #StartAutomaticCapture(int)} with device 0,
+   * The first time this overload is called, it calls
-   * creating a camera named "USB Camera 0".
+   * {@link #StartAutomaticCapture(int)} with device 0, creating a camera
+   * named "USB Camera 0".  Subsequent calls increment the device number
+   * (e.g. 1, 2, etc).
    */
   cs::UsbCamera StartAutomaticCapture();
 
@@ -241,6 +244,21 @@ class CameraServer : public ErrorBase {
    */
   void RemoveServer(llvm::StringRef name);
 
+  /**
+   * Get server for the primary camera feed.
+   *
+   * <p>This is only valid to call after a camera feed has been added
+   * with StartAutomaticCapture() or AddServer().
+   */
+  cs::VideoSink GetServer();
+
+  /**
+   * Gets a server by name.
+   *
+   * @param name Server name
+   */
+  cs::VideoSink GetServer(llvm::StringRef name);
+
   /**
    * Adds an already created camera.
    *
@@ -277,6 +295,7 @@ class CameraServer : public ErrorBase {
   static constexpr char const* kPublishName = "/CameraPublisher";
 
   std::mutex m_mutex;
+  std::atomic<int> m_defaultUsbDevice;
   std::string m_primarySourceName;
   llvm::StringMap<cs::VideoSource> m_sources;
   llvm::StringMap<cs::VideoSink> m_sinks;

wpilibc/athena/src/CameraServer.cpp

@@ -171,7 +171,7 @@ static std::string PixelFormatToString(int pixelFormat) {
       return "Unknown";
   }
 }
-
+#if 0
 static cs::VideoMode::PixelFormat PixelFormatFromString(llvm::StringRef str) {
   if (str == "MJPEG" || str == "mjpeg" || str == "JPEG" || str == "jpeg")
     return cs::VideoMode::PixelFormat::kMJPEG;
@@ -228,7 +228,7 @@ static cs::VideoMode VideoModeFromString(llvm::StringRef modeStr) {
 
   return mode;
 }
-
+#endif
 static std::string VideoModeToString(const cs::VideoMode& mode) {
   std::string rv;
   llvm::raw_string_ostream oss{rv};
@@ -261,9 +261,9 @@ static void PutSourcePropertyValue(ITable* table, const cs::VideoEvent& event,
   llvm::SmallString<64> infoName;
   if (llvm::StringRef{event.name}.startswith("raw_")) {
     name = "RawProperty/";
-    name += llvm::StringRef{event.name}.substr(4);
+    name += event.name;
     infoName = "RawPropertyInfo/";
-    infoName += llvm::StringRef{event.name}.substr(4);
+    infoName += event.name;
   } else {
     name = "Property/";
     name += event.name;
@@ -427,6 +427,9 @@ CameraServer::CameraServer()
       0x4fff, true};
 
   // Listener for NetworkTable events
+  // We don't currently support changing settings via NT due to
+  // synchronization issues, so just update to current setting if someone
+  // else tries to change it.
   llvm::SmallString<64> buf;
   m_tableListener = nt::AddEntryListener(
       Concatenate(kPublishName, "/", buf),
@@ -446,24 +449,16 @@ CameraServer::CameraServer()
         relativeKey = relativeKey.substr(subKeyIndex + 1);
 
         // handle standard names
-        llvm::SmallString<64> propNameBuf;
         llvm::StringRef propName;
         if (relativeKey == "mode") {
-          if (!value->IsString()) return;
+          // reset to current mode
-          auto mode = VideoModeFromString(value->GetString());
+          nt::SetEntryValue(key, nt::Value::MakeString(VideoModeToString(
-          if (mode.pixelFormat == cs::VideoMode::PixelFormat::kUnknown ||
+                                     sourceIt->second.GetVideoMode())));
-              !sourceIt->second.SetVideoMode(mode)) {
-            // reset to current mode
-            nt::SetEntryValue(key, nt::Value::MakeString(VideoModeToString(
-                                       sourceIt->second.GetVideoMode())));
-          }
           return;
         } else if (relativeKey.startswith("Property/")) {
           propName = relativeKey.substr(9);
         } else if (relativeKey.startswith("RawProperty/")) {
-          propNameBuf = "raw_";
+          propName = relativeKey.substr(12);
-          propNameBuf += relativeKey.substr(12);
-          propName = propNameBuf.str();
         } else {
           return;  // ignore
         }
@@ -474,17 +469,14 @@ CameraServer::CameraServer()
           case cs::VideoProperty::kNone:
             return;
           case cs::VideoProperty::kBoolean:
-            if (!value->IsBoolean()) return;
+            nt::SetEntryValue(key, nt::Value::MakeBoolean(property.Get() != 0));
-            property.Set(value->GetBoolean() ? 1 : 0);
             return;
           case cs::VideoProperty::kInteger:
           case cs::VideoProperty::kEnum:
-            if (!value->IsDouble()) return;
+            nt::SetEntryValue(key, nt::Value::MakeDouble(property.Get()));
-            property.Set(static_cast<int>(value->GetDouble()));
             return;
           case cs::VideoProperty::kString:
-            if (!value->IsString()) return;
+            nt::SetEntryValue(key, nt::Value::MakeString(property.GetString()));
-            property.SetString(value->GetString());
             return;
           default:
             return;
@@ -494,7 +486,7 @@ CameraServer::CameraServer()
 }
 
 cs::UsbCamera CameraServer::StartAutomaticCapture() {
-  return StartAutomaticCapture(0);
+  return StartAutomaticCapture(m_defaultUsbDevice++);
 }
 
 cs::UsbCamera CameraServer::StartAutomaticCapture(int dev) {
@@ -667,6 +659,33 @@ void CameraServer::RemoveServer(llvm::StringRef name) {
   m_sinks.erase(name);
 }
 
+cs::VideoSink CameraServer::GetServer() {
+  llvm::SmallString<64> name;
+  {
+    std::lock_guard<std::mutex> lock(m_mutex);
+    if (m_primarySourceName.empty()) {
+      wpi_setWPIErrorWithContext(CameraServerError, "no camera available");
+      return cs::VideoSink{};
+    }
+    name = "serve_";
+    name += m_primarySourceName;
+  }
+  return GetServer(name);
+}
+
+cs::VideoSink CameraServer::GetServer(llvm::StringRef name) {
+  std::lock_guard<std::mutex> lock(m_mutex);
+  auto it = m_sinks.find(name);
+  if (it == m_sinks.end()) {
+    llvm::SmallString<64> buf;
+    llvm::raw_svector_ostream err{buf};
+    err << "could not find server " << name;
+    wpi_setWPIErrorWithContext(CameraServerError, err.str());
+    return cs::VideoSink{};
+  }
+  return it->second;
+}
+
 void CameraServer::AddCamera(const cs::VideoSource& camera) {
   std::string name = camera.GetName();
   std::lock_guard<std::mutex> lock(m_mutex);

wpilibc/sim/src/DriverStation.cpp

@@ -230,7 +230,6 @@ bool DriverStation::IsTest() const {
 
 /**
  * Is the driver station attached to a Field Management System?
- * Note: This does not work with the Blue DS.
  * @return True if the robot is competing on a field being controlled by a Field
  *         Management System
  */

wpilibj/src/athena/java/edu/wpi/first/wpilibj/CameraServer.java

@@ -24,8 +24,9 @@ import edu.wpi.cscore.VideoSource;
 import edu.wpi.first.wpilibj.networktables.NetworkTable;
 import edu.wpi.first.wpilibj.networktables.NetworkTablesJNI;
 import edu.wpi.first.wpilibj.tables.ITable;
+import java.util.concurrent.atomic.AtomicInteger;
 import java.util.ArrayList;
-import java.util.HashMap;
+import java.util.Hashtable;
 import java.util.regex.Matcher;
 import java.util.regex.Pattern;
 
@@ -49,20 +50,21 @@ public class CameraServer {
   /**
    * Get the CameraServer instance.
    */
-  public static CameraServer getInstance() {
+  public static synchronized CameraServer getInstance() {
     if (server == null) {
       server = new CameraServer();
     }
     return server;
   }
 
+  private AtomicInteger m_defaultUsbDevice;
   private String m_primarySourceName;
-  private HashMap<String, VideoSource> m_sources;
+  private final Hashtable<String, VideoSource> m_sources;
-  private HashMap<String, VideoSink> m_sinks;
+  private final Hashtable<String, VideoSink> m_sinks;
-  private HashMap<Integer, ITable> m_tables;  // indexed by source handle
+  private final Hashtable<Integer, ITable> m_tables;  // indexed by source handle
-  private ITable m_publishTable;
+  private final ITable m_publishTable;
-  private VideoListener m_videoListener; //NOPMD
+  private final VideoListener m_videoListener; //NOPMD
-  private int m_tableListener; //NOPMD
+  private final int m_tableListener; //NOPMD
   private int m_nextPort;
   private String[] m_addresses;
 
@@ -93,11 +95,6 @@ public class CameraServer {
     return "mjpg:http://" + address + ":" + port + "/?action=stream";
   }
 
-  @SuppressWarnings("JavadocMethod")
-  private synchronized ITable getSourceTable(int source) {
-    return m_tables.get(source);
-  }
-
   @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP"})
   private synchronized String[] getSinkStreamValues(int sink) {
     // Ignore all but MjpegServer
@@ -290,8 +287,8 @@ public class CameraServer {
     String name;
     String infoName;
     if (event.name.startsWith("raw_")) {
-      name = "RawProperty/" + event.name.substring(4);
+      name = "RawProperty/" + event.name;
-      infoName = "RawPropertyInfo/" + event.name.substring(4);
+      infoName = "RawPropertyInfo/" + event.name;
     } else {
       name = "Property/" + event.name;
       infoName = "PropertyInfo/" + event.name;
@@ -335,9 +332,10 @@ public class CameraServer {
 
   @SuppressWarnings({"JavadocMethod", "PMD.UnusedLocalVariable"})
   private CameraServer() {
-    m_sources = new HashMap<String, VideoSource>();
+    m_defaultUsbDevice = new AtomicInteger();
-    m_sinks = new HashMap<String, VideoSink>();
+    m_sources = new Hashtable<String, VideoSource>();
-    m_tables = new HashMap<Integer, ITable>();
+    m_sinks = new Hashtable<String, VideoSink>();
+    m_tables = new Hashtable<Integer, ITable>();
     m_publishTable = NetworkTable.getTable(kPublishName);
     m_nextPort = kBasePort;
     m_addresses = new String[0];
@@ -359,21 +357,23 @@ public class CameraServer {
         case kSourceCreated: {
           // Create subtable for the camera
           ITable table = m_publishTable.getSubTable(event.name);
-          synchronized (this) {
+          m_tables.put(event.sourceHandle, table);
-            m_tables.put(event.sourceHandle, table);
-          }
           table.putString("source", makeSourceValue(event.sourceHandle));
           table.putString("description",
               CameraServerJNI.getSourceDescription(event.sourceHandle));
           table.putBoolean("connected", CameraServerJNI.isSourceConnected(event.sourceHandle));
           table.putStringArray("streams", getSourceStreamValues(event.sourceHandle));
-          VideoMode mode = CameraServerJNI.getSourceVideoMode(event.sourceHandle);
+          try {
-          table.setDefaultString("mode", videoModeToString(mode));
+            VideoMode mode = CameraServerJNI.getSourceVideoMode(event.sourceHandle);
-          table.putStringArray("modes", getSourceModeValues(event.sourceHandle));
+            table.setDefaultString("mode", videoModeToString(mode));
+            table.putStringArray("modes", getSourceModeValues(event.sourceHandle));
+          } catch (VideoException ex) {
+            // Do nothing. Let the other event handlers update this if there is an error.
+          }
           break;
         }
         case kSourceDestroyed: {
-          ITable table = getSourceTable(event.sourceHandle);
+          ITable table = m_tables.get(event.sourceHandle);
           if (table != null) {
             table.putString("source", "");
             table.putStringArray("streams", new String[0]);
@@ -382,7 +382,7 @@ public class CameraServer {
           break;
         }
         case kSourceConnected: {
-          ITable table = getSourceTable(event.sourceHandle);
+          ITable table = m_tables.get(event.sourceHandle);
           if (table != null) {
             // update the description too (as it may have changed)
             table.putString("description",
@@ -392,42 +392,42 @@ public class CameraServer {
           break;
         }
         case kSourceDisconnected: {
-          ITable table = getSourceTable(event.sourceHandle);
+          ITable table = m_tables.get(event.sourceHandle);
           if (table != null) {
             table.putBoolean("connected", false);
           }
           break;
         }
         case kSourceVideoModesUpdated: {
-          ITable table = getSourceTable(event.sourceHandle);
+          ITable table = m_tables.get(event.sourceHandle);
           if (table != null) {
             table.putStringArray("modes", getSourceModeValues(event.sourceHandle));
           }
           break;
         }
         case kSourceVideoModeChanged: {
-          ITable table = getSourceTable(event.sourceHandle);
+          ITable table = m_tables.get(event.sourceHandle);
           if (table != null) {
             table.putString("mode", videoModeToString(event.mode));
           }
           break;
         }
         case kSourcePropertyCreated: {
-          ITable table = getSourceTable(event.sourceHandle);
+          ITable table = m_tables.get(event.sourceHandle);
           if (table != null) {
             putSourcePropertyValue(table, event, true);
           }
           break;
         }
         case kSourcePropertyValueUpdated: {
-          ITable table = getSourceTable(event.sourceHandle);
+          ITable table = m_tables.get(event.sourceHandle);
           if (table != null) {
             putSourcePropertyValue(table, event, false);
           }
           break;
         }
         case kSourcePropertyChoicesUpdated: {
-          ITable table = getSourceTable(event.sourceHandle);
+          ITable table = m_tables.get(event.sourceHandle);
           if (table != null) {
             String[] choices = CameraServerJNI.getEnumPropertyChoices(event.propertyHandle);
             table.putStringArray("PropertyInfo/" + event.name + "/choices", choices);
@@ -450,8 +450,11 @@ public class CameraServer {
     }, 0x4fff, true);
 
     // Listener for NetworkTable events
+    // We don't currently support changing settings via NT due to
+    // synchronization issues, so just update to current setting if someone
+    // else tries to change it.
     m_tableListener = NetworkTablesJNI.addEntryListener(kPublishName + "/",
-      (uid, key, value, flags) -> {
+      (uid, key, eventValue, flags) -> {
         String relativeKey = key.substring(kPublishName.length() + 1);
 
         // get source (sourceName/...)
@@ -471,16 +474,13 @@ public class CameraServer {
         // handle standard names
         String propName;
         if (relativeKey.equals("mode")) {
-          VideoMode mode = videoModeFromString((String) value);
+          // reset to current mode
-          if (mode.pixelFormat == PixelFormat.kUnknown || !source.setVideoMode(mode)) {
+          NetworkTablesJNI.putString(key, videoModeToString(source.getVideoMode()));
-            // reset to current mode
-            NetworkTablesJNI.putString(key, videoModeToString(source.getVideoMode()));
-          }
           return;
         } else if (relativeKey.startsWith("Property/")) {
           propName = relativeKey.substring(9);
         } else if (relativeKey.startsWith("RawProperty/")) {
-          propName = "raw_" + relativeKey.substring(12);
+          propName = relativeKey.substring(12);
         } else {
           return;  // ignore
         }
@@ -491,14 +491,17 @@ public class CameraServer {
           case kNone:
             return;
           case kBoolean:
-            property.set(((Boolean) value).booleanValue() ? 1 : 0);
+            // reset to current setting
+            NetworkTablesJNI.putBoolean(key, property.get() != 0);
             return;
           case kInteger:
           case kEnum:
-            property.set(((Double) value).intValue());
+            // reset to current setting
+            NetworkTablesJNI.putDouble(key, property.get());
             return;
           case kString:
-            property.setString((String) value);
+            // reset to current setting
+            NetworkTablesJNI.putString(key, property.getString());
             return;
           default:
             return;
@@ -513,11 +516,13 @@ public class CameraServer {
    * If you also want to perform vision processing on the roboRIO, use
    * getVideo() to get access to the camera images.
    *
-   * <p>This overload calls {@link #startAutomaticCapture(int)} with device 0,
+   * <p>The first time this overload is called, it calls
-   * creating a camera named "USB Camera 0".
+   * {@link #startAutomaticCapture(int)} with device 0, creating a camera
+   * named "USB Camera 0".  Subsequent calls increment the device number
+   * (e.g. 1, 2, etc).
    */
   public UsbCamera startAutomaticCapture() {
-    return startAutomaticCapture(0);
+    return startAutomaticCapture(m_defaultUsbDevice.getAndIncrement());
   }
 
   /**
@@ -745,6 +750,32 @@ public class CameraServer {
     }
   }
 
+  /**
+   * Get server for the primary camera feed.
+   *
+   * <p>This is only valid to call after a camera feed has been added
+   * with startAutomaticCapture() or addServer().
+   */
+  public VideoSink getServer() {
+    synchronized (this) {
+      if (m_primarySourceName == null) {
+        throw new VideoException("no camera available");
+      }
+      return getServer("serve_" + m_primarySourceName);
+    }
+  }
+
+  /**
+   * Gets a server by name.
+   *
+   * @param name Server name
+   */
+  public VideoSink getServer(String name) {
+    synchronized (this) {
+      return m_sinks.get(name);
+    }
+  }
+
   /**
    * Adds an already created camera.
    *

wpilibj/src/athena/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -546,9 +546,9 @@ public class DriverStation implements RobotState.Interface {
     return m_newControlData.getAndSet(false);
   }
 
+  @SuppressWarnings({"SummaryJavadoc", "JavadocMethod"})
   /**
-   * Is the driver station attached to a Field Management System? Note: This does not work with the
+   * Is the driver station attached to a Field Management System?
-   * Blue DS.
    *
    * @return True if the robot is competing on a field being controlled by a Field Management System
    */

wpilibj/src/athena/java/edu/wpi/first/wpilibj/SafePWM.java

@@ -74,7 +74,7 @@ public class SafePWM extends PWM implements MotorSafety {
 
   /**
    * Feed the MotorSafety timer. This method is called by the subclass motor whenever it updates its
-   * speed, thereby reseting the timeout value.
+   * speed, thereby resetting the timeout value.
    */
   @SuppressWarnings("MethodName")
   public void Feed() {

wpilibj/src/sim/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -335,8 +335,7 @@ public class DriverStation implements RobotState.Interface {
   }
 
   /**
-   * Is the driver station attached to a Field Management System? Note: This does not work with the
+   * Is the driver station attached to a Field Management System?
-   * Blue DS.
    *
    * @return True if the robot is competing on a field being controlled by a Field Management System
    */