[wpilib] Allow SendableCameraWrappers to take arbitrary URLs (#3850)

Useful for adding cameras that are streamed from a coprocessor

Co-authored-by: Peter Johnson <johnson.peter@gmail.com>
Co-authored-by: Sam Carlberg <sam.carlberg@gmail.com>

wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp

@@ -8,19 +8,19 @@
 #include <memory>
 #include <string>
 
-#include <wpi/DenseMap.h>
+#include <wpi/StringMap.h>
 #include <wpi/sendable/SendableBuilder.h>
 #include <wpi/sendable/SendableRegistry.h>
 
 namespace frc {
 namespace detail {
 std::shared_ptr<SendableCameraWrapper>& GetSendableCameraWrapper(
-    CS_Source source) {
-  static wpi::DenseMap<int, std::shared_ptr<SendableCameraWrapper>> wrappers;
-  return wrappers[static_cast<int>(source)];
+    std::string_view cameraName) {
+  static wpi::StringMap<std::shared_ptr<SendableCameraWrapper>> wrappers;
+  return wrappers[cameraName];
 }
 
-void AddToSendableRegistry(wpi::Sendable* sendable, std::string name) {
+void AddToSendableRegistry(wpi::Sendable* sendable, std::string_view name) {
   wpi::SendableRegistry::Add(sendable, name);
 }
 }  // namespace detail

wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h

@@ -7,9 +7,13 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 
 #ifndef DYNAMIC_CAMERA_SERVER
 #include <cscore_oo.h>
+#include <fmt/format.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableInstance.h>
 #else
 namespace cs {
 class VideoSource;
@@ -20,6 +24,7 @@ using CS_Source = CS_Handle;  // NOLINT
 
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
+#include <wpi/span.h>
 
 namespace frc {
 
@@ -28,8 +33,8 @@ class SendableCameraWrapper;
 namespace detail {
 constexpr const char* kProtocol = "camera_server://";
 std::shared_ptr<SendableCameraWrapper>& GetSendableCameraWrapper(
-    CS_Source source);
-void AddToSendableRegistry(wpi::Sendable* sendable, std::string name);
+    std::string_view cameraName);
+void AddToSendableRegistry(wpi::Sendable* sendable, std::string_view name);
 }  // namespace detail
 
 /**
@@ -46,9 +51,9 @@ class SendableCameraWrapper
    * Creates a new sendable wrapper. Private constructor to avoid direct
    * instantiation with multiple wrappers floating around for the same camera.
    *
-   * @param source the source to wrap
+   * @param cameraName the name of the camera to wrap
    */
-  SendableCameraWrapper(CS_Source source, const private_init&);
+  SendableCameraWrapper(std::string_view cameraName, const private_init&);
 
   /**
    * Gets a sendable wrapper object for the given video source, creating the
@@ -61,6 +66,9 @@ class SendableCameraWrapper
   static SendableCameraWrapper& Wrap(const cs::VideoSource& source);
   static SendableCameraWrapper& Wrap(CS_Source source);
 
+  static SendableCameraWrapper& Wrap(std::string_view cameraName,
+                                     wpi::span<const std::string> cameraUrls);
+
   void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
@@ -68,11 +76,9 @@ class SendableCameraWrapper
 };
 
 #ifndef DYNAMIC_CAMERA_SERVER
-inline SendableCameraWrapper::SendableCameraWrapper(CS_Source source,
+inline SendableCameraWrapper::SendableCameraWrapper(std::string_view name,
                                                     const private_init&)
     : m_uri(detail::kProtocol) {
-  CS_Status status = 0;
-  auto name = cs::GetSourceName(source, &status);
   detail::AddToSendableRegistry(this, name);
   m_uri += name;
 }
@@ -83,12 +89,27 @@ inline SendableCameraWrapper& SendableCameraWrapper::Wrap(
 }
 
 inline SendableCameraWrapper& SendableCameraWrapper::Wrap(CS_Source source) {
-  auto& wrapper = detail::GetSendableCameraWrapper(source);
+  CS_Status status = 0;
+  auto name = cs::GetSourceName(source, &status);
+  auto& wrapper = detail::GetSendableCameraWrapper(name);
   if (!wrapper) {
-    wrapper = std::make_shared<SendableCameraWrapper>(source, private_init{});
+    wrapper = std::make_shared<SendableCameraWrapper>(name, private_init{});
   }
   return *wrapper;
 }
+
+inline SendableCameraWrapper& SendableCameraWrapper::Wrap(
+    std::string_view cameraName, wpi::span<const std::string> cameraUrls) {
+  auto& wrapper = detail::GetSendableCameraWrapper(cameraName);
+  if (!wrapper) {
+    wrapper =
+        std::make_shared<SendableCameraWrapper>(cameraName, private_init{});
+  }
+  auto streams = fmt::format("/CameraPublisher/{}/streams", cameraName);
+  nt::NetworkTableInstance::GetDefault().GetEntry(streams).SetStringArray(
+      cameraUrls);
+  return *wrapper;
+}
 #endif
 
 }  // namespace frc

wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h

@@ -127,6 +127,18 @@ class ShuffleboardContainer : public virtual ShuffleboardValue {
    */
   ComplexWidget& Add(std::string_view title, const cs::VideoSource& video);
 
+  /**
+   * Adds a widget to this container to display a video stream.
+   *
+   * @param title      the title of the widget
+   * @param cameraName the name of the streamed camera
+   * @param cameraUrls the URLs with which the dashboard can access the camera
+   * stream
+   * @return a widget to display the camera stream
+   */
+  ComplexWidget& AddCamera(std::string_view title, std::string_view cameraName,
+                           wpi::span<const std::string> cameraUrls);
+
   /**
    * Adds a widget to this container to display the given sendable.
    *
@@ -526,4 +538,10 @@ inline frc::ComplexWidget& frc::ShuffleboardContainer::Add(
     std::string_view title, const cs::VideoSource& video) {
   return Add(title, frc::SendableCameraWrapper::Wrap(video));
 }
+
+inline frc::ComplexWidget& frc::ShuffleboardContainer::AddCamera(
+    std::string_view title, std::string_view cameraName,
+    wpi::span<const std::string> cameraUrls) {
+  return Add(title, frc::SendableCameraWrapper::Wrap(cameraName, cameraUrls));
+}
 #endif

wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java

@@ -5,17 +5,19 @@
 package edu.wpi.first.wpilibj.shuffleboard;
 
 import edu.wpi.first.cscore.VideoSource;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
 import java.util.Map;
+import java.util.Objects;
 import java.util.WeakHashMap;
 
 /** A wrapper to make video sources sendable and usable from Shuffleboard. */
 public final class SendableCameraWrapper implements Sendable, AutoCloseable {
   private static final String kProtocol = "camera_server://";
 
-  private static Map<VideoSource, SendableCameraWrapper> m_wrappers = new WeakHashMap<>();
+  private static Map<String, SendableCameraWrapper> m_wrappers = new WeakHashMap<>();
 
   private final String m_uri;
 
@@ -26,9 +28,18 @@ public final class SendableCameraWrapper implements Sendable, AutoCloseable {
    * @param source the source to wrap
    */
   private SendableCameraWrapper(VideoSource source) {
-    String name = source.getName();
-    SendableRegistry.add(this, name);
-    m_uri = kProtocol + name;
+    this(source.getName());
+  }
+
+  private SendableCameraWrapper(String cameraName) {
+    SendableRegistry.add(this, cameraName);
+    m_uri = kProtocol + cameraName;
+  }
+
+  /** Clears all cached wrapper objects. This should only be used in tests. */
+  @SuppressWarnings("PMD.DefaultPackage")
+  static void clearWrappers() {
+    m_wrappers.clear();
   }
 
   @Override
@@ -45,7 +56,52 @@ public final class SendableCameraWrapper implements Sendable, AutoCloseable {
    *     ShuffleboardTab#add(Sendable)} and {@link ShuffleboardLayout#add(Sendable)}
    */
   public static SendableCameraWrapper wrap(VideoSource source) {
-    return m_wrappers.computeIfAbsent(source, SendableCameraWrapper::new);
+    return m_wrappers.computeIfAbsent(source.getName(), name -> new SendableCameraWrapper(source));
+  }
+
+  /**
+   * Creates a wrapper for an arbitrary camera stream. The stream URLs <i>must</i> be specified
+   * using a host resolvable by a program running on a different host (such as a dashboard); prefer
+   * using static IP addresses (if known) or DHCP identifiers such as {@code "raspberrypi.local"}.
+   *
+   * <p>If a wrapper already exists for the given camera, that wrapper is returned and the specified
+   * URLs are ignored.
+   *
+   * @param cameraName the name of the camera. Cannot be null or empty
+   * @param cameraUrls the URLs with which the camera stream may be accessed. At least one URL must
+   *     be specified
+   * @return a sendable wrapper object for the video source, usable in Shuffleboard via {@link
+   *     ShuffleboardTab#add(Sendable)} and {@link ShuffleboardLayout#add(Sendable)}
+   */
+  @SuppressWarnings("PMD.CyclomaticComplexity")
+  public static SendableCameraWrapper wrap(String cameraName, String... cameraUrls) {
+    if (m_wrappers.containsKey(cameraName)) {
+      return m_wrappers.get(cameraName);
+    }
+
+    Objects.requireNonNull(cameraName, "cameraName");
+    Objects.requireNonNull(cameraUrls, "cameraUrls");
+    if (cameraName.isEmpty()) {
+      throw new IllegalArgumentException("Camera name not specified");
+    }
+    if (cameraUrls.length == 0) {
+      throw new IllegalArgumentException("No camera URLs specified");
+    }
+    for (int i = 0; i < cameraUrls.length; i++) {
+      Objects.requireNonNull(cameraUrls[i], "Camera URL at index " + i + " was null");
+    }
+
+    String streams = "/CameraPublisher/" + cameraName + "/streams";
+    if (NetworkTableInstance.getDefault().getEntries(streams, 0).length != 0) {
+      throw new IllegalStateException(
+          "A camera is already being streamed with the name '" + cameraName + "'");
+    }
+
+    NetworkTableInstance.getDefault().getEntry(streams).setStringArray(cameraUrls);
+
+    SendableCameraWrapper wrapper = new SendableCameraWrapper(cameraName);
+    m_wrappers.put(cameraName, wrapper);
+    return wrapper;
   }
 
   @Override

wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java

@@ -122,6 +122,20 @@ public interface ShuffleboardContainer extends ShuffleboardValue {
    */
   SimpleWidget add(String title, Object defaultValue);
 
+  /**
+   * Adds a widget to this container to display a video stream.
+   *
+   * @param title the title of the widget
+   * @param cameraName the name of the streamed camera
+   * @param cameraUrls the URLs with which the dashboard can access the camera stream
+   * @return a widget to display the camera stream
+   * @throws IllegalArgumentException if a widget already exists in this container with the given
+   *     title
+   */
+  default ComplexWidget addCamera(String title, String cameraName, String... cameraUrls) {
+    return add(title, SendableCameraWrapper.wrap(cameraName, cameraUrls));
+  }
+
   /**
    * Adds a widget to this container. The widget will display the data provided by the value
    * supplier. Changes made on the dashboard will not propagate to the widget object, and will be

wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapperTest.java

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.shuffleboard;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import org.junit.jupiter.api.AfterAll;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class SendableCameraWrapperTest {
+  @BeforeEach
+  void setup() {
+    NetworkTableInstance.getDefault().deleteAllEntries();
+    SendableCameraWrapper.clearWrappers();
+  }
+
+  @AfterAll
+  static void tearDown() {
+    NetworkTableInstance.getDefault().deleteAllEntries();
+  }
+
+  @Test
+  void testNullCameraName() {
+    assertThrows(NullPointerException.class, () -> SendableCameraWrapper.wrap(null, ""));
+  }
+
+  @Test
+  void testEmptyCameraName() {
+    assertThrows(IllegalArgumentException.class, () -> SendableCameraWrapper.wrap("", ""));
+  }
+
+  @Test
+  void testNullUrlArray() {
+    assertThrows(
+        NullPointerException.class, () -> SendableCameraWrapper.wrap("name", (String[]) null));
+  }
+
+  @Test
+  void testNullUrlInArray() {
+    assertThrows(NullPointerException.class, () -> SendableCameraWrapper.wrap("name", "url", null));
+  }
+
+  @Test
+  void testEmptyUrlArray() {
+    assertThrows(IllegalArgumentException.class, () -> SendableCameraWrapper.wrap("name"));
+  }
+
+  @Test
+  void testUrlsAddedToNt() {
+    SendableCameraWrapper.wrap("name", "url1", "url2");
+    assertArrayEquals(
+        new String[] {"url1", "url2"},
+        NetworkTableInstance.getDefault()
+            .getEntry("/CameraPublisher/name/streams")
+            .getValue()
+            .getStringArray());
+  }
+}