[templates] Add Java template for educational robot (#3309)

Educational robot is a very minimal template designed for educational use
(rather than competition).

.github/workflows/documentation.yml

@@ -9,7 +9,7 @@ jobs:
   publish:
     name: "Documentation - Publish"
     runs-on: ubuntu-latest
-    if: github.repository_owner == 'wpilibsuite' && (github.ref == 'refs/heads/master' || startsWith(github.ref, 'refs/tags/v'))
+    if: github.repository_owner == 'wpilibsuite' && (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
     steps:
       - uses: actions/checkout@v2
         with:
@@ -21,7 +21,7 @@ jobs:
       - name: Set environment variables (Development)
         run: |
           echo "TARGET_FOLDER=$BASE_PATH/development" >> $GITHUB_ENV
-        if: github.ref == 'refs/heads/master'
+        if: github.ref == 'refs/heads/main'
       - name: Set environment variables (Tag)
         run: |
           echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV

.github/workflows/gradle.yml

@@ -71,12 +71,12 @@ jobs:
           keychain-password: ${{ secrets.APPLE_KEYCHAIN_PASSWORD }}
         if: |
           matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
-          (github.ref == 'refs/heads/master' || startsWith(github.ref, 'refs/tags/v')))
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v')))
       - name: Set Keychain Lock Timeout
         run: security set-keychain-settings -lut 3600
         if: |
           matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
-          (github.ref == 'refs/heads/master' || startsWith(github.ref, 'refs/tags/v')))
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v')))
       - name: Set release environment variable
         run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
         shell: bash
@@ -87,7 +87,7 @@ jobs:
         run: ./gradlew build -PbuildServer -PdeveloperID=${{ secrets.APPLE_DEVELOPER_ID }} ${{ env.EXTRA_GRADLE_ARGS }}
         if: |
           matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
-          (github.ref == 'refs/heads/master' || startsWith(github.ref, 'refs/tags/v')))
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v')))
       - uses: actions/upload-artifact@v2
         with:
           name: ${{ matrix.artifact-name }}
@@ -136,12 +136,12 @@ jobs:
       - name: Combine
         if: |
           !startsWith(github.ref, 'refs/tags/v') &&
-          github.ref != 'refs/heads/master'
+          github.ref != 'refs/heads/main'
         run: cd combiner && ./gradlew publish -Pallwpilib
       - name: Combine (Master)
         if: |
           github.repository_owner == 'wpilibsuite' &&
-          github.ref == 'refs/heads/master'
+          github.ref == 'refs/heads/main'
         run: cd combiner && ./gradlew publish -Pallwpilib
         env:
           RUN_AZURE_ARTIFACTORY_RELEASE: "TRUE"

.github/workflows/lint-format.yml

@@ -12,7 +12,7 @@ jobs:
         run: |
           git fetch --prune --unshallow
           git checkout -b pr
-          git branch -f master origin/master
+          git branch -f main origin/main
       - name: Set up Python 3.8
         uses: actions/setup-python@v2
         with:
@@ -35,13 +35,9 @@ jobs:
         run: |
           git fetch --prune --unshallow
           git checkout -b pr
-          git branch -f master origin/master
-      - name: Set up Python 3.8
-        uses: actions/setup-python@v2
-        with:
-          python-version: 3.8
+          git branch -f main origin/main
       - name: Install clang-format and clang-tidy
-        run: sudo apt-get update -q && sudo apt-get install -y clang-format-10 clang-tidy-10
+        run: sudo apt-get update -q && sudo apt-get install -y clang-format-10 clang-tidy-10 python3.8
       - name: Install wpiformat
         run: pip3 install wpiformat
       - name: Create compile_commands.json

.gitignore

@@ -222,5 +222,6 @@ compile_commands.json
 # clang configuration and clangd cache
 .clang
 .clangd/
+.cache/
 
 imgui.ini

CONTRIBUTING.md

@@ -1,6 +1,6 @@
 # Contributing to WPILib
 
-So you want to contribute your changes back to WPILib. Great! We have a few contributing rules that will help you make sure your changes will be accepted into the project. Please remember to follow the rules in the [code of conduct](https://github.com/wpilibsuite/allwpilib/blob/master/CODE_OF_CONDUCT.md), and behave with Gracious Professionalism.
+So you want to contribute your changes back to WPILib. Great! We have a few contributing rules that will help you make sure your changes will be accepted into the project. Please remember to follow the rules in the [code of conduct](https://github.com/wpilibsuite/allwpilib/blob/main/CODE_OF_CONDUCT.md), and behave with Gracious Professionalism.
 
 - [General Contribution Rules](#general-contribution-rules)
 - [What to Contribute](#what-to-contribute)
@@ -45,7 +45,7 @@ While the library should be fully formatted according to the styles, additional
 
 ### Pull Request Format
 
-Changes should be submitted as a Pull Request against the master branch of WPILib. For most changes, commits will be squashed upon merge. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. We may ask you to break a PR into multiple standalone PRs or commits for rebase within one PR to separate unrelated changes. No change will be merged unless it is up to date with the current master. We do this to make sure that the git history isn't too cluttered.
+Changes should be submitted as a Pull Request against the main branch of WPILib. For most changes, commits will be squashed upon merge. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. We may ask you to break a PR into multiple standalone PRs or commits for rebase within one PR to separate unrelated changes. No change will be merged unless it is up to date with the current main branch. We do this to make sure that the git history isn't too cluttered.
 
 ### Merge Process
 

MAINTAINERS.md

@@ -11,10 +11,10 @@ For Java dependencies, there is likely a file related to the specific dependency
 Note, changing artifact locations (This includes changing the artifact year currently, I have an issue open to change this) requires updating the `native-utils` plugin
 
 ## Publishing allwpilib
-allwpilib publishes to the development repo on every push to master. To publish a release build, upload a new tag, and a release will automatically be built and published.
+allwpilib publishes to the development repo on every push to main. To publish a release build, upload a new tag, and a release will automatically be built and published.
 
 ## Publishing desktop tools
-Desktop tools publish to the development repo on every push to master. To publish a release build, upload a new tag, and a release will automatically be built and published.
+Desktop tools publish to the development repo on every push to main. To publish a release build, upload a new tag, and a release will automatically be built and published.
 
 ## Publishing VS Code
 Before publishing, make sure to update the gradlerio version in `vscode-wpilib/resources/gradle/version.txt` Also make sure the gradle wrapper version matches the wrapper required by gradlerio.

MavenArtifacts.md

@@ -9,7 +9,7 @@ We provide two repositories. These repositories are:
 * (Development) https://frcmaven.wpi.edu/artifactory/development/
 
 The release repository is where official WPILib releases are pushed.
-The development repository is where development releases of every commit to [master](https://github.com/wpilibsuite/allwpilib/tree/master) is pushed.
+The development repository is where development releases of every commit to [main](https://github.com/wpilibsuite/allwpilib/tree/main) is pushed.
 
 ## Artifact classifiers
 We provide two base types of artifacts.

README.md

@@ -51,7 +51,7 @@ Using Gradle makes building WPILib very straightforward. It only has a few depen
 
 Clone the WPILib repository and follow the instructions above for installing any required tooling.
 
-See the [styleguide README](https://github.com/wpilibsuite/styleguide/blob/master/README.md) for wpiformat setup instructions.
+See the [styleguide README](https://github.com/wpilibsuite/styleguide/blob/main/README.md) for wpiformat setup instructions.
 
 ## Building
 

cscore/CMakeLists.txt

@@ -87,11 +87,11 @@ if (WITH_JAVA)
         set(OPENCV_JAVA_INSTALL_DIR ${OpenCV_INSTALL_PATH}/share/OpenCV/java/)
     endif()
 
-    find_file(OPENCV_JAR_FILE NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin NO_DEFAULT_PATH)
+    find_file(OPENCV_JAR_FILE NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin ${OpenCV_INSTALL_PATH}/share/java NO_DEFAULT_PATH)
     find_file(OPENCV_JNI_FILE NAMES libopencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.so
                                     libopencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.dylib
                                     opencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.dll
-                                    PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin ${OpenCV_INSTALL_PATH}/bin/Release ${OpenCV_INSTALL_PATH}/bin/Debug ${OpenCV_INSTALL_PATH}/lib NO_DEFAULT_PATH)
+                                    PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin ${OpenCV_INSTALL_PATH}/bin/Release ${OpenCV_INSTALL_PATH}/bin/Debug ${OpenCV_INSTALL_PATH}/lib ${OpenCV_INSTALL_PATH}/lib/jni NO_DEFAULT_PATH)
 
     file(GLOB
         cscore_jni_src src/main/native/cpp/jni/CameraServerJNI.cpp)

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

@@ -313,6 +313,19 @@ public class CameraServerJNI {
 
   public static native void removeListener(int handle);
 
+  public static native int createListenerPoller();
+
+  public static native void destroyListenerPoller(int poller);
+
+  public static native int addPolledListener(int poller, int eventMask, boolean immediateNotify);
+
+  public static native VideoEvent[] pollListener(int poller) throws InterruptedException;
+
+  public static native VideoEvent[] pollListenerTimeout(int poller, double timeout)
+      throws InterruptedException;
+
+  public static native void cancelPollListener(int poller);
+
   //
   // Telemetry Functions
   //

cscore/src/main/java/edu/wpi/cscore/VideoEvent.java

@@ -26,7 +26,8 @@ public class VideoEvent {
     kTelemetryUpdated(0x8000),
     kSinkPropertyCreated(0x10000),
     kSinkPropertyValueUpdated(0x20000),
-    kSinkPropertyChoicesUpdated(0x40000);
+    kSinkPropertyChoicesUpdated(0x40000),
+    kUsbCamerasChanged(0x80000);
 
     private final int value;
 
@@ -84,6 +85,8 @@ public class VideoEvent {
         return Kind.kSinkPropertyValueUpdated;
       case 0x40000:
         return Kind.kSinkPropertyChoicesUpdated;
+      case 0x80000:
+        return Kind.kUsbCamerasChanged;
       default:
         return Kind.kUnknown;
     }
@@ -102,7 +105,8 @@ public class VideoEvent {
       int property,
       int propertyKind,
       int value,
-      String valueStr) {
+      String valueStr,
+      int listener) {
     this.kind = getKindFromInt(kind);
     this.sourceHandle = source;
     this.sinkHandle = sink;
@@ -112,6 +116,7 @@ public class VideoEvent {
     this.propertyKind = VideoProperty.getKindFromInt(propertyKind);
     this.value = value;
     this.valueStr = valueStr;
+    this.listener = listener;
   }
 
   @SuppressWarnings("MemberName")
@@ -145,6 +150,10 @@ public class VideoEvent {
   @SuppressWarnings("MemberName")
   public String valueStr;
 
+  // Listener that was triggered
+  @SuppressWarnings("MemberName")
+  public int listener;
+
   public VideoSource getSource() {
     return new VideoSource(CameraServerJNI.copySource(sourceHandle));
   }

cscore/src/main/java/edu/wpi/cscore/VideoListener.java

@@ -4,6 +4,10 @@
 
 package edu.wpi.cscore;
 
+import java.util.HashMap;
+import java.util.Map;
+import java.util.concurrent.locks.Condition;
+import java.util.concurrent.locks.ReentrantLock;
 import java.util.function.Consumer;
 
 /**
@@ -20,15 +24,31 @@ public class VideoListener implements AutoCloseable {
    *     of events for the current library state.
    */
   public VideoListener(Consumer<VideoEvent> listener, int eventMask, boolean immediateNotify) {
-    m_handle = CameraServerJNI.addListener(listener, eventMask, immediateNotify);
+    s_lock.lock();
+    try {
+      if (s_poller == 0) {
+        s_poller = CameraServerJNI.createListenerPoller();
+        startThread();
+      }
+      m_handle = CameraServerJNI.addPolledListener(s_poller, eventMask, immediateNotify);
+      s_listeners.put(m_handle, listener);
+    } finally {
+      s_lock.unlock();
+    }
   }
 
   @Override
   public synchronized void close() {
     if (m_handle != 0) {
+      s_lock.lock();
+      try {
+        s_listeners.remove(m_handle);
+      } finally {
+        s_lock.unlock();
+      }
       CameraServerJNI.removeListener(m_handle);
+      m_handle = 0;
     }
-    m_handle = 0;
   }
 
   public boolean isValid() {
@@ -36,4 +56,71 @@ public class VideoListener implements AutoCloseable {
   }
 
   private int m_handle;
+
+  private static final ReentrantLock s_lock = new ReentrantLock();
+  private static final Map<Integer, Consumer<VideoEvent>> s_listeners = new HashMap<>();
+  private static Thread s_thread;
+  private static int s_poller;
+  private static boolean s_waitQueue;
+  private static final Condition s_waitQueueCond = s_lock.newCondition();
+
+  @SuppressWarnings("PMD.AvoidCatchingThrowable")
+  private static void startThread() {
+    s_thread =
+        new Thread(
+            () -> {
+              boolean wasInterrupted = false;
+              while (!Thread.interrupted()) {
+                VideoEvent[] events;
+                try {
+                  events = CameraServerJNI.pollListener(s_poller);
+                } catch (InterruptedException ex) {
+                  s_lock.lock();
+                  try {
+                    if (s_waitQueue) {
+                      s_waitQueue = false;
+                      s_waitQueueCond.signalAll();
+                      continue;
+                    }
+                  } finally {
+                    s_lock.unlock();
+                  }
+                  Thread.currentThread().interrupt();
+                  // don't try to destroy poller, as its handle is likely no longer valid
+                  wasInterrupted = true;
+                  break;
+                }
+                for (VideoEvent event : events) {
+                  Consumer<VideoEvent> listener;
+                  s_lock.lock();
+                  try {
+                    listener = s_listeners.get(event.listener);
+                  } finally {
+                    s_lock.unlock();
+                  }
+                  if (listener != null) {
+                    try {
+                      listener.accept(event);
+                    } catch (Throwable throwable) {
+                      System.err.println(
+                          "Unhandled exception during listener callback: " + throwable.toString());
+                      throwable.printStackTrace();
+                    }
+                  }
+                }
+              }
+              s_lock.lock();
+              try {
+                if (!wasInterrupted) {
+                  CameraServerJNI.destroyListenerPoller(s_poller);
+                }
+                s_poller = 0;
+              } finally {
+                s_lock.unlock();
+              }
+            },
+            "VideoListener");
+    s_thread.setDaemon(true);
+    s_thread.start();
+  }
 }

cscore/src/main/native/cpp/Handle.h

@@ -22,7 +22,8 @@ class Handle {
     kSource,
     kSink,
     kListener,
-    kSinkProperty
+    kSinkProperty,
+    kListenerPoller
   };
   enum { kIndexMax = 0xffff };
 

cscore/src/main/native/cpp/Instance.cpp

@@ -36,7 +36,10 @@ static void def_log_func(unsigned int level, const char* file,
   wpi::errs() << oss.str();
 }
 
-Instance::Instance() : telemetry(notifier), networkListener(logger, notifier) {
+Instance::Instance()
+    : telemetry(notifier),
+      networkListener(logger, notifier),
+      usbCameraListener(logger, notifier) {
   SetDefaultLogger();
 }
 
@@ -52,6 +55,7 @@ void Instance::Shutdown() {
   m_sinks.FreeAll();
   m_sources.FreeAll();
   networkListener.Stop();
+  usbCameraListener.Stop();
   telemetry.Stop();
   notifier.Stop();
 }

cscore/src/main/native/cpp/Instance.h

@@ -18,6 +18,7 @@
 #include "SourceImpl.h"
 #include "Telemetry.h"
 #include "UnlimitedHandleResource.h"
+#include "UsbCameraListener.h"
 
 namespace cs {
 
@@ -54,6 +55,7 @@ class Instance {
   Notifier notifier;
   Telemetry telemetry;
   NetworkListener networkListener;
+  UsbCameraListener usbCameraListener;
 
  private:
   UnlimitedHandleResource<Handle, SourceData, Handle::kSource> m_sources;

cscore/src/main/native/cpp/Notifier.cpp

@@ -15,170 +15,26 @@
 
 using namespace cs;
 
-bool Notifier::s_destroyed = false;
+Notifier::Notifier() {}
 
-namespace {
-// Vector which provides an integrated freelist for removal and reuse of
-// individual elements.
-template <typename T>
-class UidVector {
- public:
-  using size_type = typename std::vector<T>::size_type;
-
-  size_type size() const { return m_vector.size(); }
-  T& operator[](size_type i) { return m_vector[i]; }
-  const T& operator[](size_type i) const { return m_vector[i]; }
-
-  // Add a new T to the vector.  If there are elements on the freelist,
-  // reuses the last one; otherwise adds to the end of the vector.
-  // Returns the resulting element index (+1).
-  template <class... Args>
-  unsigned int emplace_back(Args&&... args) {
-    unsigned int uid;
-    if (m_free.empty()) {
-      uid = m_vector.size();
-      m_vector.emplace_back(std::forward<Args>(args)...);
-    } else {
-      uid = m_free.back();
-      m_free.pop_back();
-      m_vector[uid] = T(std::forward<Args>(args)...);
-    }
-    return uid + 1;
-  }
-
-  // Removes the identified element by replacing it with a default-constructed
-  // one.  The element is added to the freelist for later reuse.
-  void erase(unsigned int uid) {
-    --uid;
-    if (uid >= m_vector.size() || !m_vector[uid]) {
-      return;
-    }
-    m_free.push_back(uid);
-    m_vector[uid] = T();
-  }
-
- private:
-  std::vector<T> m_vector;
-  std::vector<unsigned int> m_free;
-};
-
-}  // namespace
-
-class Notifier::Thread : public wpi::SafeThread {
- public:
-  Thread(std::function<void()> on_start, std::function<void()> on_exit)
-      : m_on_start(std::move(on_start)), m_on_exit(std::move(on_exit)) {}
-
-  void Main() override;
-
-  struct Listener {
-    Listener() = default;
-    Listener(std::function<void(const RawEvent& event)> callback_,
-             int eventMask_)
-        : callback(std::move(callback_)), eventMask(eventMask_) {}
-
-    explicit operator bool() const { return static_cast<bool>(callback); }
-
-    std::string prefix;
-    std::function<void(const RawEvent& event)> callback;
-    int eventMask;
-  };
-  UidVector<Listener> m_listeners;
-
-  std::queue<RawEvent> m_notifications;
-
-  std::function<void()> m_on_start;
-  std::function<void()> m_on_exit;
-};
-
-Notifier::Notifier() {
-  s_destroyed = false;
-}
-
-Notifier::~Notifier() {
-  s_destroyed = true;
-}
+Notifier::~Notifier() {}
 
 void Notifier::Start() {
-  m_owner.Start(m_on_start, m_on_exit);
+  DoStart();
 }
 
-void Notifier::Stop() {
-  m_owner.Stop();
+unsigned int Notifier::Add(std::function<void(const RawEvent& event)> callback,
+                           int eventMask) {
+  return DoAdd(callback, eventMask);
 }
 
-void Notifier::Thread::Main() {
-  if (m_on_start) {
-    m_on_start();
-  }
-
-  std::unique_lock lock(m_mutex);
-  while (m_active) {
-    while (m_notifications.empty()) {
-      m_cond.wait(lock);
-      if (!m_active) {
-        goto done;
-      }
-    }
-
-    while (!m_notifications.empty()) {
-      if (!m_active) {
-        goto done;
-      }
-      auto item = std::move(m_notifications.front());
-      m_notifications.pop();
-
-      // Use index because iterator might get invalidated.
-      for (size_t i = 0; i < m_listeners.size(); ++i) {
-        if (!m_listeners[i]) {
-          continue;  // removed
-        }
-
-        // Event type must be within requested set for this listener.
-        if ((item.kind & m_listeners[i].eventMask) == 0) {
-          continue;
-        }
-
-        // make a copy of the callback so we can safely release the mutex
-        auto callback = m_listeners[i].callback;
-
-        // Don't hold mutex during callback execution!
-        lock.unlock();
-        callback(item);
-        lock.lock();
-      }
-    }
-  }
-
-done:
-  if (m_on_exit) {
-    m_on_exit();
-  }
-}
-
-int Notifier::AddListener(std::function<void(const RawEvent& event)> callback,
-                          int eventMask) {
-  Start();
-  auto thr = m_owner.GetThread();
-  return thr->m_listeners.emplace_back(callback, eventMask);
-}
-
-void Notifier::RemoveListener(int uid) {
-  auto thr = m_owner.GetThread();
-  if (!thr) {
-    return;
-  }
-  thr->m_listeners.erase(uid);
+unsigned int Notifier::AddPolled(unsigned int pollerUid, int eventMask) {
+  return DoAdd(pollerUid, eventMask);
 }
 
 void Notifier::NotifySource(const wpi::Twine& name, CS_Source source,
                             CS_EventKind kind) {
-  auto thr = m_owner.GetThread();
-  if (!thr) {
-    return;
-  }
-  thr->m_notifications.emplace(name, source, static_cast<RawEvent::Kind>(kind));
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, name, source, static_cast<RawEvent::Kind>(kind));
 }
 
 void Notifier::NotifySource(const SourceImpl& source, CS_EventKind kind) {
@@ -188,44 +44,24 @@ void Notifier::NotifySource(const SourceImpl& source, CS_EventKind kind) {
 
 void Notifier::NotifySourceVideoMode(const SourceImpl& source,
                                      const VideoMode& mode) {
-  auto thr = m_owner.GetThread();
-  if (!thr) {
-    return;
-  }
-
   auto handleData = Instance::GetInstance().FindSource(source);
-
-  thr->m_notifications.emplace(source.GetName(), handleData.first, mode);
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, source.GetName(), handleData.first, mode);
 }
 
 void Notifier::NotifySourceProperty(const SourceImpl& source, CS_EventKind kind,
                                     const wpi::Twine& propertyName,
                                     int property, CS_PropertyKind propertyKind,
                                     int value, const wpi::Twine& valueStr) {
-  auto thr = m_owner.GetThread();
-  if (!thr) {
-    return;
-  }
-
   auto handleData = Instance::GetInstance().FindSource(source);
-
-  thr->m_notifications.emplace(
-      propertyName, handleData.first, static_cast<RawEvent::Kind>(kind),
-      Handle{handleData.first, property, Handle::kProperty}, propertyKind,
-      value, valueStr);
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, propertyName, handleData.first,
+       static_cast<RawEvent::Kind>(kind),
+       Handle{handleData.first, property, Handle::kProperty}, propertyKind,
+       value, valueStr);
 }
 
 void Notifier::NotifySink(const wpi::Twine& name, CS_Sink sink,
                           CS_EventKind kind) {
-  auto thr = m_owner.GetThread();
-  if (!thr) {
-    return;
-  }
-
-  thr->m_notifications.emplace(name, sink, static_cast<RawEvent::Kind>(kind));
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, name, sink, static_cast<RawEvent::Kind>(kind));
 }
 
 void Notifier::NotifySink(const SinkImpl& sink, CS_EventKind kind) {
@@ -235,52 +71,30 @@ void Notifier::NotifySink(const SinkImpl& sink, CS_EventKind kind) {
 
 void Notifier::NotifySinkSourceChanged(const wpi::Twine& name, CS_Sink sink,
                                        CS_Source source) {
-  auto thr = m_owner.GetThread();
-  if (!thr) {
-    return;
-  }
-
   RawEvent event{name, sink, RawEvent::kSinkSourceChanged};
   event.sourceHandle = source;
-
-  thr->m_notifications.emplace(std::move(event));
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, std::move(event));
 }
 
 void Notifier::NotifySinkProperty(const SinkImpl& sink, CS_EventKind kind,
                                   const wpi::Twine& propertyName, int property,
                                   CS_PropertyKind propertyKind, int value,
                                   const wpi::Twine& valueStr) {
-  auto thr = m_owner.GetThread();
-  if (!thr) {
-    return;
-  }
-
   auto handleData = Instance::GetInstance().FindSink(sink);
-
-  thr->m_notifications.emplace(
-      propertyName, handleData.first, static_cast<RawEvent::Kind>(kind),
-      Handle{handleData.first, property, Handle::kSinkProperty}, propertyKind,
-      value, valueStr);
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, propertyName, handleData.first,
+       static_cast<RawEvent::Kind>(kind),
+       Handle{handleData.first, property, Handle::kSinkProperty}, propertyKind,
+       value, valueStr);
 }
 
 void Notifier::NotifyNetworkInterfacesChanged() {
-  auto thr = m_owner.GetThread();
-  if (!thr) {
-    return;
-  }
-
-  thr->m_notifications.emplace(RawEvent::kNetworkInterfacesChanged);
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, RawEvent::kNetworkInterfacesChanged);
 }
 
 void Notifier::NotifyTelemetryUpdated() {
-  auto thr = m_owner.GetThread();
-  if (!thr) {
-    return;
-  }
-
-  thr->m_notifications.emplace(RawEvent::kTelemetryUpdated);
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, RawEvent::kTelemetryUpdated);
+}
+
+void Notifier::NotifyUsbCamerasChanged() {
+  Send(UINT_MAX, RawEvent::kUsbCamerasChanged);
 }

cscore/src/main/native/cpp/Notifier.h

@@ -6,9 +6,11 @@
 #define CSCORE_NOTIFIER_H_
 
 #include <functional>
+#include <utility>
 
-#include <wpi/SafeThread.h>
+#include <wpi/CallbackManager.h>
 
+#include "Handle.h"
 #include "cscore_cpp.h"
 
 namespace cs {
@@ -16,7 +18,40 @@ namespace cs {
 class SinkImpl;
 class SourceImpl;
 
-class Notifier {
+namespace impl {
+
+struct ListenerData : public wpi::CallbackListenerData<
+                          std::function<void(const RawEvent& event)>> {
+  ListenerData() = default;
+  ListenerData(std::function<void(const RawEvent& event)> callback_,
+               int eventMask_)
+      : CallbackListenerData(std::move(callback_)), eventMask(eventMask_) {}
+  ListenerData(unsigned int pollerUid_, int eventMask_)
+      : CallbackListenerData(pollerUid_), eventMask(eventMask_) {}
+
+  int eventMask;
+};
+
+class NotifierThread
+    : public wpi::CallbackThread<NotifierThread, RawEvent, ListenerData> {
+ public:
+  bool Matches(const ListenerData& /*listener*/, const RawEvent& /*data*/) {
+    return true;
+  }
+
+  void SetListener(RawEvent* data, unsigned int listener_uid) {
+    data->listener = Handle(listener_uid, Handle::kListener);
+  }
+
+  void DoCallback(std::function<void(const RawEvent& event)> callback,
+                  const RawEvent& data) {
+    callback(data);
+  }
+};
+
+}  // namespace impl
+
+class Notifier : public wpi::CallbackManager<Notifier, impl::NotifierThread> {
   friend class NotifierTest;
 
  public:
@@ -24,16 +59,10 @@ class Notifier {
   ~Notifier();
 
   void Start();
-  void Stop();
 
-  static bool destroyed() { return s_destroyed; }
-
-  void SetOnStart(std::function<void()> on_start) { m_on_start = on_start; }
-  void SetOnExit(std::function<void()> on_exit) { m_on_exit = on_exit; }
-
-  int AddListener(std::function<void(const RawEvent& event)> callback,
-                  int eventMask);
-  void RemoveListener(int uid);
+  unsigned int Add(std::function<void(const RawEvent& event)> callback,
+                   int eventMask);
+  unsigned int AddPolled(unsigned int pollerUid, int eventMask);
 
   // Notification events
   void NotifySource(const wpi::Twine& name, CS_Source source,
@@ -54,14 +83,7 @@ class Notifier {
                           const wpi::Twine& valueStr);
   void NotifyNetworkInterfacesChanged();
   void NotifyTelemetryUpdated();
-
- private:
-  class Thread;
-  wpi::SafeThreadOwner<Thread> m_owner;
-
-  std::function<void()> m_on_start;
-  std::function<void()> m_on_exit;
-  static bool s_destroyed;
+  void NotifyUsbCamerasChanged();
 };
 
 }  // namespace cs

cscore/src/main/native/cpp/UsbCameraListener.h

@@ -0,0 +1,31 @@
+// 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.
+
+#ifndef CSCORE_USBCAMERALISTENER_H_
+#define CSCORE_USBCAMERALISTENER_H_
+
+#include <memory>
+
+#include <wpi/Logger.h>
+
+namespace cs {
+
+class Notifier;
+
+class UsbCameraListener {
+ public:
+  UsbCameraListener(wpi::Logger& logger, Notifier& notifier);
+  ~UsbCameraListener();
+
+  void Start();
+  void Stop();
+
+ private:
+  class Impl;
+  std::unique_ptr<Impl> m_impl;
+};
+
+}  // namespace cs
+
+#endif  // CSCORE_USBCAMERALISTENER_H_

cscore/src/main/native/cpp/cscore_c.cpp

@@ -15,6 +15,39 @@
 #include "cscore_cpp.h"
 #include "cscore_raw.h"
 
+static CS_Event ConvertToC(const cs::RawEvent& rawEvent) {
+  CS_Event event;
+  event.kind = static_cast<CS_EventKind>(static_cast<int>(rawEvent.kind));
+  event.source = rawEvent.sourceHandle;
+  event.sink = rawEvent.sinkHandle;
+  event.name = rawEvent.name.c_str();
+  event.mode = rawEvent.mode;
+  event.property = rawEvent.propertyHandle;
+  event.propertyKind = rawEvent.propertyKind;
+  event.value = rawEvent.value;
+  event.valueStr = rawEvent.valueStr.c_str();
+  event.listener = rawEvent.listener;
+  return event;
+}
+
+template <typename O, typename I>
+static O* ConvertToC(std::vector<I>&& in, int* count) {
+  using T = std::vector<I>;
+  size_t size = in.size();
+  O* out = static_cast<O*>(wpi::safe_malloc(size * sizeof(O) + sizeof(T)));
+  *count = size;
+  for (size_t i = 0; i < size; ++i) {
+    out[i] = ConvertToC(in[i]);
+  }
+
+  // retain vector at end of returned array
+  alignas(T) unsigned char buf[sizeof(T)];
+  new (buf) T(std::move(in));
+  std::memcpy(out + size * sizeof(O), buf, sizeof(T));
+
+  return out;
+}
+
 extern "C" {
 
 CS_PropertyKind CS_GetPropertyKind(CS_Property property, CS_Status* status) {
@@ -332,16 +365,7 @@ CS_Listener CS_AddListener(void* data,
                            CS_Status* status) {
   return cs::AddListener(
       [=](const cs::RawEvent& rawEvent) {
-        CS_Event event;
-        event.kind = static_cast<CS_EventKind>(static_cast<int>(rawEvent.kind));
-        event.source = rawEvent.sourceHandle;
-        event.sink = rawEvent.sinkHandle;
-        event.name = rawEvent.name.c_str();
-        event.mode = rawEvent.mode;
-        event.property = rawEvent.propertyHandle;
-        event.propertyKind = rawEvent.propertyKind;
-        event.value = rawEvent.value;
-        event.valueStr = rawEvent.valueStr.c_str();
+        CS_Event event = ConvertToC(rawEvent);
         callback(data, &event);
       },
       eventMask, immediateNotify, status);
@@ -351,6 +375,45 @@ void CS_RemoveListener(CS_Listener handle, CS_Status* status) {
   return cs::RemoveListener(handle, status);
 }
 
+CS_ListenerPoller CS_CreateListenerPoller(void) {
+  return cs::CreateListenerPoller();
+}
+
+void CS_DestroyListenerPoller(CS_ListenerPoller poller) {
+  cs::DestroyListenerPoller(poller);
+}
+
+CS_Listener CS_AddPolledListener(CS_ListenerPoller poller, int eventMask,
+                                 CS_Bool immediateNotify, CS_Status* status) {
+  return cs::AddPolledListener(poller, eventMask, immediateNotify, status);
+}
+
+struct CS_Event* CS_PollListener(CS_ListenerPoller poller, int* count) {
+  return ConvertToC<CS_Event>(cs::PollListener(poller), count);
+}
+
+struct CS_Event* CS_PollListenerTimeout(CS_ListenerPoller poller, int* count,
+                                        double timeout, CS_Bool* timedOut) {
+  bool cppTimedOut = false;
+  auto arrCpp = cs::PollListener(poller, timeout, &cppTimedOut);
+  *timedOut = cppTimedOut;
+  return ConvertToC<CS_Event>(std::move(arrCpp), count);
+}
+
+void CS_CancelPollListener(CS_ListenerPoller poller) {
+  cs::CancelPollListener(poller);
+}
+
+void CS_FreeEvents(CS_Event* arr, int count) {
+  // destroy vector saved at end of array
+  using T = std::vector<cs::RawEvent>;
+  alignas(T) unsigned char buf[sizeof(T)];
+  std::memcpy(buf, arr + count * sizeof(CS_Event), sizeof(T));
+  reinterpret_cast<T*>(buf)->~T();
+
+  std::free(arr);
+}
+
 int CS_NotifierDestroyed(void) {
   return cs::NotifierDestroyed();
 }

cscore/src/main/native/cpp/cscore_cpp.cpp

@@ -705,19 +705,12 @@ void ReleaseSink(CS_Sink sink, CS_Status* status) {
 // Listener Functions
 //
 
-void SetListenerOnStart(std::function<void()> onStart) {
-  Instance::GetInstance().notifier.SetOnStart(onStart);
-}
+void SetListenerOnStart(std::function<void()> onStart) {}
 
-void SetListenerOnExit(std::function<void()> onExit) {
-  Instance::GetInstance().notifier.SetOnExit(onExit);
-}
+void SetListenerOnExit(std::function<void()> onExit) {}
 
-CS_Listener AddListener(std::function<void(const RawEvent& event)> callback,
-                        int eventMask, bool immediateNotify,
-                        CS_Status* status) {
+static void StartBackground(int eventMask, bool immediateNotify) {
   auto& inst = Instance::GetInstance();
-  int uid = inst.notifier.AddListener(callback, eventMask);
   if ((eventMask & CS_NETWORK_INTERFACES_CHANGED) != 0) {
     // start network interface event listener
     inst.networkListener.Start();
@@ -725,6 +718,21 @@ CS_Listener AddListener(std::function<void(const RawEvent& event)> callback,
       inst.notifier.NotifyNetworkInterfacesChanged();
     }
   }
+  if ((eventMask & CS_USB_CAMERAS_CHANGED) != 0) {
+    // start network interface event listener
+    inst.usbCameraListener.Start();
+    if (immediateNotify) {
+      inst.notifier.NotifyUsbCamerasChanged();
+    }
+  }
+}
+
+CS_Listener AddListener(std::function<void(const RawEvent& event)> callback,
+                        int eventMask, bool immediateNotify,
+                        CS_Status* status) {
+  auto& inst = Instance::GetInstance();
+  int uid = inst.notifier.Add(callback, eventMask);
+  StartBackground(eventMask, immediateNotify);
   if (immediateNotify) {
     // TODO
   }
@@ -737,11 +745,67 @@ void RemoveListener(CS_Listener handle, CS_Status* status) {
     *status = CS_INVALID_HANDLE;
     return;
   }
-  Instance::GetInstance().notifier.RemoveListener(uid);
+  Instance::GetInstance().notifier.Remove(uid);
+}
+
+CS_ListenerPoller CreateListenerPoller() {
+  auto& inst = Instance::GetInstance();
+  return Handle(inst.notifier.CreatePoller(), Handle::kListenerPoller);
+}
+
+void DestroyListenerPoller(CS_ListenerPoller poller) {
+  int uid = Handle{poller}.GetTypedIndex(Handle::kListenerPoller);
+  if (uid < 0) {
+    return;
+  }
+  Instance::GetInstance().notifier.RemovePoller(uid);
+}
+
+CS_Listener AddPolledListener(CS_ListenerPoller poller, int eventMask,
+                              bool immediateNotify, CS_Status* status) {
+  Handle handle{poller};
+  int id = handle.GetTypedIndex(Handle::kListenerPoller);
+  if (id < 0) {
+    *status = CS_INVALID_HANDLE;
+    return 0;
+  }
+
+  auto& inst = Instance::GetInstance();
+  int uid = inst.notifier.AddPolled(id, eventMask);
+  StartBackground(eventMask, immediateNotify);
+  return Handle{uid, Handle::kListener};
+}
+
+std::vector<RawEvent> PollListener(CS_ListenerPoller poller) {
+  Handle handle{poller};
+  int id = handle.GetTypedIndex(Handle::kListenerPoller);
+  if (id < 0) {
+    return {};
+  }
+  return Instance::GetInstance().notifier.Poll(id);
+}
+
+std::vector<RawEvent> PollListener(CS_ListenerPoller poller, double timeout,
+                                   bool* timedOut) {
+  Handle handle{poller};
+  int id = handle.GetTypedIndex(Handle::kListenerPoller);
+  if (id < 0) {
+    return {};
+  }
+  return Instance::GetInstance().notifier.Poll(id, timeout, timedOut);
+}
+
+void CancelPollListener(CS_ListenerPoller poller) {
+  Handle handle{poller};
+  int id = handle.GetTypedIndex(Handle::kListenerPoller);
+  if (id < 0) {
+    return;
+  }
+  return Instance::GetInstance().notifier.CancelPoll(id);
 }
 
 bool NotifierDestroyed() {
-  return Notifier::destroyed();
+  return false;
 }
 
 //

cscore/src/main/native/cpp/jni/CameraServerJNI.cpp

@@ -31,6 +31,7 @@ static JClass videoModeCls;
 static JClass videoEventCls;
 static JClass rawFrameCls;
 static JException videoEx;
+static JException interruptedEx;
 static JException nullPointerEx;
 static JException unsupportedEx;
 static JException exceptionEx;
@@ -45,6 +46,7 @@ static const JClassInit classes[] = {
 
 static const JExceptionInit exceptions[] = {
     {"edu/wpi/cscore/VideoException", &videoEx},
+    {"java/lang/InterruptedException", &interruptedEx},
     {"java/lang/NullPointerException", &nullPointerEx},
     {"java/lang/UnsupportedOperationException", &unsupportedEx},
     {"java/lang/Exception", &exceptionEx}};
@@ -268,7 +270,7 @@ static jobject MakeJObject(JNIEnv* env, const cs::VideoMode& videoMode) {
 static jobject MakeJObject(JNIEnv* env, const cs::RawEvent& event) {
   static jmethodID constructor =
       env->GetMethodID(videoEventCls, "<init>",
-                       "(IIILjava/lang/String;IIIIIIILjava/lang/String;)V");
+                       "(IIILjava/lang/String;IIIIIIILjava/lang/String;I)V");
   JLocal<jstring> name(env, MakeJString(env, event.name));
   JLocal<jstring> valueStr(env, MakeJString(env, event.valueStr));
   // clang-format off
@@ -286,10 +288,23 @@ static jobject MakeJObject(JNIEnv* env, const cs::RawEvent& event) {
       static_cast<jint>(event.propertyHandle),
       static_cast<jint>(event.propertyKind),
       static_cast<jint>(event.value),
-      valueStr.obj());
+      valueStr.obj(),
+      static_cast<jint>(event.listener));
   // clang-format on
 }
 
+static jobjectArray MakeJObject(JNIEnv* env, wpi::ArrayRef<cs::RawEvent> arr) {
+  jobjectArray jarr = env->NewObjectArray(arr.size(), videoEventCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
 extern "C" {
 
 /*
@@ -1881,6 +1896,92 @@ Java_edu_wpi_cscore_CameraServerJNI_removeListener
   CheckStatus(env, status);
 }
 
+/*
+ * Class:     edu_wpi_cscore_CameraServerJNI
+ * Method:    createListenerPoller
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_cscore_CameraServerJNI_createListenerPoller
+  (JNIEnv*, jclass)
+{
+  return cs::CreateListenerPoller();
+}
+
+/*
+ * Class:     edu_wpi_cscore_CameraServerJNI
+ * Method:    destroyListenerPoller
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_cscore_CameraServerJNI_destroyListenerPoller
+  (JNIEnv*, jclass, jint poller)
+{
+  cs::DestroyListenerPoller(poller);
+}
+
+/*
+ * Class:     edu_wpi_cscore_CameraServerJNI
+ * Method:    addPolledListener
+ * Signature: (IIZ)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_cscore_CameraServerJNI_addPolledListener
+  (JNIEnv* env, jclass, jint poller, jint eventMask, jboolean immediateNotify)
+{
+  CS_Status status = 0;
+  auto rv = cs::AddPolledListener(poller, eventMask, immediateNotify, &status);
+  CheckStatus(env, status);
+  return rv;
+}
+
+/*
+ * Class:     edu_wpi_cscore_CameraServerJNI
+ * Method:    pollListener
+ * Signature: (I)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_cscore_CameraServerJNI_pollListener
+  (JNIEnv* env, jclass, jint poller)
+{
+  auto events = cs::PollListener(poller);
+  if (events.empty()) {
+    interruptedEx.Throw(env, "PollListener interrupted");
+    return nullptr;
+  }
+  return MakeJObject(env, events);
+}
+
+/*
+ * Class:     edu_wpi_cscore_CameraServerJNI
+ * Method:    pollListenerTimeout
+ * Signature: (ID)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_cscore_CameraServerJNI_pollListenerTimeout
+  (JNIEnv* env, jclass, jint poller, jdouble timeout)
+{
+  bool timed_out = false;
+  auto events = cs::PollListener(poller, timeout, &timed_out);
+  if (events.empty() && !timed_out) {
+    interruptedEx.Throw(env, "PollListener interrupted");
+    return nullptr;
+  }
+  return MakeJObject(env, events);
+}
+
+/*
+ * Class:     edu_wpi_cscore_CameraServerJNI
+ * Method:    cancelPollListener
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_cscore_CameraServerJNI_cancelPollListener
+  (JNIEnv*, jclass, jint poller)
+{
+  cs::CancelPollListener(poller);
+}
+
 /*
  * Class:     edu_wpi_cscore_CameraServerJNI
  * Method:    setTelemetryPeriod

cscore/src/main/native/include/cscore_c.h

@@ -46,6 +46,7 @@ typedef int CS_Status;
 typedef int CS_Handle;
 typedef CS_Handle CS_Property;
 typedef CS_Handle CS_Listener;
+typedef CS_Handle CS_ListenerPoller;
 typedef CS_Handle CS_Sink;
 typedef CS_Handle CS_Source;
 /** @} */
@@ -169,7 +170,8 @@ enum CS_EventKind {
   CS_TELEMETRY_UPDATED = 0x8000,
   CS_SINK_PROPERTY_CREATED = 0x10000,
   CS_SINK_PROPERTY_VALUE_UPDATED = 0x20000,
-  CS_SINK_PROPERTY_CHOICES_UPDATED = 0x40000
+  CS_SINK_PROPERTY_CHOICES_UPDATED = 0x40000,
+  CS_USB_CAMERAS_CHANGED = 0x80000
 };
 
 /**
@@ -222,6 +224,9 @@ struct CS_Event {
   enum CS_PropertyKind propertyKind;
   int value;
   const char* valueStr;
+
+  // Listener that was triggered
+  CS_Listener listener;
 };
 
 /**
@@ -429,9 +434,19 @@ void CS_SetListenerOnStart(void (*onStart)(void* data), void* data);
 void CS_SetListenerOnExit(void (*onExit)(void* data), void* data);
 CS_Listener CS_AddListener(
     void* data, void (*callback)(void* data, const struct CS_Event* event),
-    int eventMask, int immediateNotify, CS_Status* status);
+    int eventMask, CS_Bool immediateNotify, CS_Status* status);
 
 void CS_RemoveListener(CS_Listener handle, CS_Status* status);
+
+CS_ListenerPoller CS_CreateListenerPoller(void);
+void CS_DestroyListenerPoller(CS_ListenerPoller poller);
+CS_Listener CS_AddPolledListener(CS_ListenerPoller poller, int eventMask,
+                                 CS_Bool immediateNotify, CS_Status* status);
+struct CS_Event* CS_PollListener(CS_ListenerPoller poller, int* count);
+struct CS_Event* CS_PollListenerTimeout(CS_ListenerPoller poller, int* count,
+                                        double timeout, CS_Bool* timedOut);
+void CS_FreeEvents(struct CS_Event* arr, int count);
+void CS_CancelPollListener(CS_ListenerPoller poller);
 /** @} */
 
 int CS_NotifierDestroyed(void);

cscore/src/main/native/include/cscore_cpp.h

@@ -116,7 +116,8 @@ struct RawEvent {
     kTelemetryUpdated = CS_TELEMETRY_UPDATED,
     kSinkPropertyCreated = CS_SINK_PROPERTY_CREATED,
     kSinkPropertyValueUpdated = CS_SINK_PROPERTY_VALUE_UPDATED,
-    kSinkPropertyChoicesUpdated = CS_SINK_PROPERTY_CHOICES_UPDATED
+    kSinkPropertyChoicesUpdated = CS_SINK_PROPERTY_CHOICES_UPDATED,
+    kUsbCamerasChanged = CS_USB_CAMERAS_CHANGED
   };
 
   RawEvent() = default;
@@ -163,6 +164,9 @@ struct RawEvent {
   CS_PropertyKind propertyKind;
   int value;
   std::string valueStr;
+
+  // Listener that was triggered
+  CS_Listener listener{0};
 };
 
 /**
@@ -378,6 +382,15 @@ CS_Listener AddListener(std::function<void(const RawEvent& event)> callback,
                         int eventMask, bool immediateNotify, CS_Status* status);
 
 void RemoveListener(CS_Listener handle, CS_Status* status);
+
+CS_ListenerPoller CreateListenerPoller();
+void DestroyListenerPoller(CS_ListenerPoller poller);
+CS_Listener AddPolledListener(CS_ListenerPoller poller, int eventMask,
+                              bool immediateNotify, CS_Status* status);
+std::vector<RawEvent> PollListener(CS_ListenerPoller poller);
+std::vector<RawEvent> PollListener(CS_ListenerPoller poller, double timeout,
+                                   bool* timedOut);
+void CancelPollListener(CS_ListenerPoller poller);
 /** @} */
 
 bool NotifierDestroyed();

cscore/src/main/native/linux/UsbCameraListener.cpp

@@ -0,0 +1,55 @@
+// 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.
+
+#include "UsbCameraListener.h"
+
+#include <wpi/EventLoopRunner.h>
+#include <wpi/uv/FsEvent.h>
+#include <wpi/uv/Timer.h>
+
+#include "Notifier.h"
+
+using namespace cs;
+
+class UsbCameraListener::Impl {
+ public:
+  explicit Impl(Notifier& notifier) : m_notifier(notifier) {}
+
+  Notifier& m_notifier;
+
+  std::unique_ptr<wpi::EventLoopRunner> m_runner;
+};
+
+UsbCameraListener::UsbCameraListener(wpi::Logger& logger, Notifier& notifier)
+    : m_impl(std::make_unique<Impl>(notifier)) {}
+
+UsbCameraListener::~UsbCameraListener() = default;
+
+void UsbCameraListener::Start() {
+  if (!m_impl->m_runner) {
+    m_impl->m_runner = std::make_unique<wpi::EventLoopRunner>();
+    m_impl->m_runner->ExecAsync([impl = m_impl.get()](wpi::uv::Loop& loop) {
+      auto refreshTimer = wpi::uv::Timer::Create(loop);
+      refreshTimer->timeout.connect([notifier = &impl->m_notifier] {
+        notifier->NotifyUsbCamerasChanged();
+      });
+      refreshTimer->Unreference();
+
+      auto devEvents = wpi::uv::FsEvent::Create(loop);
+      devEvents->fsEvent.connect([refreshTimer](const char* fn, int flags) {
+        if (wpi::StringRef(fn).startswith("video")) {
+          refreshTimer->Start(wpi::uv::Timer::Time(200));
+        }
+      });
+      devEvents->Start("/dev");
+      devEvents->Unreference();
+    });
+  }
+}
+
+void UsbCameraListener::Stop() {
+  if (m_impl->m_runner) {
+    m_impl->m_runner.reset();
+  }
+}

cscore/src/main/native/osx/UsbCameraListener.cpp

@@ -0,0 +1,17 @@
+// 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.
+
+#include "UsbCameraListener.h"
+
+using namespace cs;
+
+class UsbCameraListener::Impl {};
+
+UsbCameraListener::UsbCameraListener(wpi::Logger& logger, Notifier& notifier) {}
+
+UsbCameraListener::~UsbCameraListener() = default;
+
+void UsbCameraListener::Start() {}
+
+void UsbCameraListener::Stop() {}

cscore/src/main/native/windows/UsbCameraListener.cpp

@@ -0,0 +1,72 @@
+// 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.
+
+#include "UsbCameraListener.h"
+
+#include "Notifier.h"
+#include "WindowsMessagePump.h"
+
+#include <dbt.h>  // NOLINT(build/include_order)
+
+#define IDT_TIMER1 1001
+
+using namespace cs;
+
+class UsbCameraListener::Impl {
+ public:
+  explicit Impl(Notifier& notifier) : m_notifier{notifier} {}
+
+  void Start() {
+    m_messagePump = std::make_unique<WindowsMessagePump>(
+        [this](HWND hwnd, UINT uiMsg, WPARAM wParam, LPARAM lParam) {
+          return this->PumpMain(hwnd, uiMsg, wParam, lParam);
+        });
+  }
+
+  void Stop() { m_messagePump = nullptr; }
+
+  LRESULT PumpMain(HWND hwnd, UINT uiMsg, WPARAM wParam, LPARAM lParam) {
+    switch (uiMsg) {
+      case WM_CLOSE:
+        KillTimer(hwnd, IDT_TIMER1);
+        break;
+      case WM_TIMER:
+        if (wParam == IDT_TIMER1) {
+          KillTimer(hwnd, IDT_TIMER1);
+          m_notifier.NotifyUsbCamerasChanged();
+        }
+        break;
+      case WM_DEVICECHANGE:
+        PDEV_BROADCAST_HDR parameter =
+            reinterpret_cast<PDEV_BROADCAST_HDR>(lParam);
+        if (wParam == DBT_DEVICEARRIVAL &&
+            parameter->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE) {
+          SetTimer(hwnd, IDT_TIMER1, 200, nullptr);
+        } else if (wParam == DBT_DEVICEREMOVECOMPLETE &&
+                   parameter->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE) {
+          SetTimer(hwnd, IDT_TIMER1, 200, nullptr);
+        }
+        break;
+    }
+    return 0;
+  }
+
+  Notifier& m_notifier;
+  std::unique_ptr<cs::WindowsMessagePump> m_messagePump;
+};
+
+UsbCameraListener::UsbCameraListener(wpi::Logger& logger, Notifier& notifier)
+    : m_impl{std::make_unique<Impl>(notifier)} {}
+
+UsbCameraListener::~UsbCameraListener() {
+  Stop();
+}
+
+void UsbCameraListener::Start() {
+  m_impl->Start();
+}
+
+void UsbCameraListener::Stop() {
+  m_impl->Stop();
+}

docs/build.gradle

@@ -25,6 +25,7 @@ def zipBaseNameJava = '_GROUP_edu_wpi_first_wpilibj_ID_documentation_CLS'
 def outputsFolder = file("$project.buildDir/outputs")
 
 def cppProjectZips = []
+def cppIncludeRoots = []
 
 cppProjectZips.add(project(':hal').cppHeadersZip)
 cppProjectZips.add(project(':wpiutil').cppHeadersZip)
@@ -49,6 +50,9 @@ doxygen {
     cppProjectZips.each {
         dependsOn it
         source it.source
+        it.ext.includeDirs.each {
+            cppIncludeRoots.add(it.absolutePath)
+        }
     }
 
     exclude 'Eigen/**'
@@ -57,7 +61,6 @@ doxygen {
     exclude 'uv.h'
     exclude 'uv/**'
 
-
     extension_mapping 'inc=C++'
     project_name 'WPILibC++'
     project_number wpilibVersioning.version.get()
@@ -75,6 +78,9 @@ doxygen {
     html_timestamp true
     generate_treeview true
     extract_static true
+    full_path_names true
+    strip_from_inc_path cppIncludeRoots as String[]
+    strip_from_path cppIncludeRoots as String[]
 }
 
 tasks.register("zipCppDocs", Zip) {

glass/.styleguide

@@ -21,6 +21,7 @@ repoRootNameOverride {
 includeOtherLibs {
   ^GLFW
   ^cscore
+  ^frc/
   ^imgui
   ^ntcore
   ^wpi/

glass/src/app/native/cpp/NetworkTablesSettings.cpp

@@ -1,73 +0,0 @@
-// 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.
-
-#include "NetworkTablesSettings.h"
-
-#include <utility>
-
-#include <imgui.h>
-#include <imgui_stdlib.h>
-#include <ntcore_cpp.h>
-#include <wpi/SmallVector.h>
-#include <wpi/StringRef.h>
-#include <wpi/raw_ostream.h>
-
-#include "glass/Context.h"
-
-NetworkTablesSettings::NetworkTablesSettings(NT_Inst inst,
-                                             const char* storageName)
-    : m_inst{inst} {
-  auto& storage = glass::GetStorage(storageName);
-  m_pMode = storage.GetIntRef("mode");
-  m_pIniName = storage.GetStringRef("iniName", "networktables.ini");
-  m_pServerTeam = storage.GetStringRef("serverTeam");
-  m_pListenAddress = storage.GetStringRef("listenAddress");
-}
-
-void NetworkTablesSettings::Update() {
-  if (!m_restart) {
-    return;
-  }
-  m_restart = false;
-  nt::StopClient(m_inst);
-  nt::StopServer(m_inst);
-  nt::StopLocal(m_inst);
-  if (*m_pMode == 1) {
-    wpi::StringRef serverTeam{*m_pServerTeam};
-    unsigned int team;
-    if (!serverTeam.contains('.') && !serverTeam.getAsInteger(10, team)) {
-      nt::StartClientTeam(m_inst, team, NT_DEFAULT_PORT);
-    } else {
-      wpi::SmallVector<wpi::StringRef, 4> serverNames;
-      wpi::SmallVector<std::pair<wpi::StringRef, unsigned int>, 4> servers;
-      serverTeam.split(serverNames, ',', -1, false);
-      for (auto&& serverName : serverNames) {
-        servers.emplace_back(serverName, NT_DEFAULT_PORT);
-      }
-      nt::StartClient(m_inst, servers);
-    }
-  } else if (*m_pMode == 2) {
-    nt::StartServer(m_inst, m_pIniName->c_str(), m_pListenAddress->c_str(),
-                    NT_DEFAULT_PORT);
-  }
-}
-
-void NetworkTablesSettings::Display() {
-  static const char* modeOptions[] = {"Disabled", "Client", "Server"};
-  ImGui::Combo("Mode", m_pMode, modeOptions, 3);
-  switch (*m_pMode) {
-    case 1:
-      ImGui::InputText("Team/IP", m_pServerTeam);
-      break;
-    case 2:
-      ImGui::InputText("Listen Address", m_pListenAddress);
-      ImGui::InputText("ini Filename", m_pIniName);
-      break;
-    default:
-      break;
-  }
-  if (ImGui::Button("Apply")) {
-    m_restart = true;
-  }
-}

glass/src/app/native/cpp/main.cpp

@@ -10,12 +10,12 @@
 #include <wpi/SmallString.h>
 #include <wpigui.h>
 
-#include "NetworkTablesSettings.h"
 #include "glass/Context.h"
 #include "glass/Model.h"
 #include "glass/View.h"
 #include "glass/networktables/NetworkTables.h"
 #include "glass/networktables/NetworkTablesProvider.h"
+#include "glass/networktables/NetworkTablesSettings.h"
 #include "glass/other/Log.h"
 #include "glass/other/Plot.h"
 
@@ -37,7 +37,7 @@ static std::unique_ptr<glass::PlotProvider> gPlotProvider;
 static std::unique_ptr<glass::NetworkTablesProvider> gNtProvider;
 
 static std::unique_ptr<glass::NetworkTablesModel> gNetworkTablesModel;
-static std::unique_ptr<NetworkTablesSettings> gNetworkTablesSettings;
+static std::unique_ptr<glass::NetworkTablesSettings> gNetworkTablesSettings;
 static glass::LogData gNetworkTablesLog;
 static glass::Window* gNetworkTablesWindow;
 static glass::Window* gNetworkTablesSettingsWindow;
@@ -111,7 +111,7 @@ static void NtInitialize() {
   }
 
   // NetworkTables settings window
-  gNetworkTablesSettings = std::make_unique<NetworkTablesSettings>();
+  gNetworkTablesSettings = std::make_unique<glass::NetworkTablesSettings>();
   gui::AddEarlyExecute([] { gNetworkTablesSettings->Update(); });
 
   gNetworkTablesSettingsWindow = gNtProvider->AddWindow(

glass/src/lib/native/cpp/other/Log.cpp

@@ -23,7 +23,7 @@ void LogData::Append(const wpi::Twine& msg) {
   }
 
   size_t oldSize = m_buf.size();
-  wpi::raw_vector_ostream os{m_buf};
+  wpi::raw_string_ostream os{m_buf};
   msg.print(os);
   for (size_t newSize = m_buf.size(); oldSize < newSize; ++oldSize) {
     if (m_buf[oldSize] == '\n') {
@@ -32,6 +32,10 @@ void LogData::Append(const wpi::Twine& msg) {
   }
 }
 
+const std::string& LogData::GetBuffer() {
+  return m_buf;
+}
+
 void glass::DisplayLog(LogData* data, bool autoScroll) {
   if (data->m_buf.empty()) {
     return;
@@ -65,6 +69,11 @@ void LogView::Display() {
     if (ImGui::Selectable("Clear")) {
       m_data->Clear();
     }
+    const auto& buf = m_data->GetBuffer();
+    if (ImGui::Selectable("Copy to Clipboard", false,
+                          buf.empty() ? ImGuiSelectableFlags_Disabled : 0)) {
+      ImGui::SetClipboardText(buf.c_str());
+    }
     ImGui::EndPopup();
   }
 

glass/src/lib/native/cpp/other/Plot.cpp

@@ -113,6 +113,15 @@ class Plot {
 
   std::vector<std::unique_ptr<PlotSeries>> m_series;
 
+  // Returns base height; does not include actual plot height if auto-sized.
+  int GetAutoBaseHeight(bool* isAuto, size_t i);
+
+  void SetAutoHeight(int height) {
+    if (m_autoHeight) {
+      m_height = height;
+    }
+  }
+
  private:
   void EmitSettingsLimits(int axis);
 
@@ -123,6 +132,7 @@ class Plot {
   bool m_lockPrevX = false;
   bool m_paused = false;
   float m_viewTime = 10;
+  bool m_autoHeight = true;
   int m_height = 300;
   struct PlotRange {
     double min = 0;
@@ -164,6 +174,7 @@ PlotSeries::PlotSeries(wpi::StringRef id) : m_id(id) {
 
 PlotSeries::PlotSeries(DataSource* source, int yAxis) : m_yAxis(yAxis) {
   SetSource(source);
+  m_id = source->GetId();
 }
 
 void PlotSeries::CheckSource() {
@@ -181,7 +192,6 @@ void PlotSeries::CheckSource() {
 
 void PlotSeries::SetSource(DataSource* source) {
   m_source = source;
-  m_id = source->GetId();
 
   // add initial value
   m_data[m_size++] = ImPlotPoint{wpi::Now() * 1.0e-6, source->GetValue()};
@@ -364,9 +374,15 @@ PlotSeries::Action PlotSeries::EmitPlot(PlotView& view, double now, size_t i,
     ImPlot::EndLegendDragDropSource();
   }
 
+  // Show full source name tooltip
+  if (!m_name.empty() && ImPlot::IsLegendEntryHovered(label)) {
+    ImGui::SetTooltip("%s", m_id.c_str());
+  }
+
   // Edit settings via popup
   Action rv = kNone;
   if (ImPlot::BeginLegendPopup(label)) {
+    ImGui::TextUnformatted(m_id.c_str());
     if (ImGui::Button("Close")) {
       ImGui::CloseCurrentPopup();
     }
@@ -527,6 +543,13 @@ bool Plot::ReadIni(wpi::StringRef name, wpi::StringRef value) {
     }
     m_viewTime = num / 1000.0;
     return true;
+  } else if (name == "autoHeight") {
+    int num;
+    if (value.getAsInteger(10, num)) {
+      return true;
+    }
+    m_autoHeight = num != 0;
+    return true;
   } else if (name == "height") {
     int num;
     if (value.getAsInteger(10, num)) {
@@ -582,12 +605,12 @@ bool Plot::ReadIni(wpi::StringRef name, wpi::StringRef value) {
 void Plot::WriteIni(ImGuiTextBuffer* out) {
   out->appendf(
       "name=%s\nvisible=%d\nshowPause=%d\nlockPrevX=%d\nlegend=%d\n"
-      "yaxis2=%d\nyaxis3=%d\nviewTime=%d\nheight=%d\n",
+      "yaxis2=%d\nyaxis3=%d\nviewTime=%d\nautoHeight=%d\nheight=%d\n",
       m_name.c_str(), m_visible ? 1 : 0, m_showPause ? 1 : 0,
       m_lockPrevX ? 1 : 0, (m_plotFlags & ImPlotFlags_NoLegend) ? 0 : 1,
       (m_plotFlags & ImPlotFlags_YAxis2) ? 1 : 0,
       (m_plotFlags & ImPlotFlags_YAxis3) ? 1 : 0,
-      static_cast<int>(m_viewTime * 1000), m_height);
+      static_cast<int>(m_viewTime * 1000), m_autoHeight ? 1 : 0, m_height);
   for (int i = 0; i < 3; ++i) {
     out->appendf(
         "y%d_min=%d\ny%d_max=%d\ny%d_lockMin=%d\ny%d_lockMax=%d\n"
@@ -761,14 +784,34 @@ void Plot::EmitSettings(size_t i) {
   }
   ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
   ImGui::InputFloat("View Time (s)", &m_viewTime, 0.1f, 1.0f, "%.1f");
-  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
-  if (ImGui::InputInt("Height", &m_height, 10)) {
-    if (m_height < 0) {
-      m_height = 0;
+  ImGui::Checkbox("Auto Height", &m_autoHeight);
+  if (!m_autoHeight) {
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+    if (ImGui::InputInt("Height", &m_height, 10)) {
+      if (m_height < 0) {
+        m_height = 0;
+      }
     }
   }
 }
 
+int Plot::GetAutoBaseHeight(bool* isAuto, size_t i) {
+  *isAuto = m_autoHeight;
+
+  if (!m_visible) {
+    return 0;
+  }
+
+  int height = m_autoHeight ? 0 : m_height;
+
+  // Pause button
+  if ((i == 0 || !m_lockPrevX) && m_showPause) {
+    height += ImGui::GetFrameHeightWithSpacing();
+  }
+
+  return height;
+}
+
 void PlotView::Display() {
   if (ImGui::BeginPopupContextItem()) {
     if (ImGui::Button("Add plot")) {
@@ -844,6 +887,27 @@ void PlotView::Display() {
       auto ref = static_cast<const PlotSeriesRef*>(payload->Data);
       MovePlot(ref->view, ref->plotIndex, 0);
     }
+    return;
+  }
+
+  // Auto-size plots.  This requires two passes: the first pass to get the
+  // total height, the second to actually set the height after averaging it
+  // across all auto-sized heights.
+  int availHeight = ImGui::GetContentRegionAvail().y;
+  int numAuto = 0;
+  for (size_t i = 0; i < m_plots.size(); ++i) {
+    bool isAuto;
+    availHeight -= m_plots[i]->GetAutoBaseHeight(&isAuto, i);
+    availHeight -= ImGui::GetStyle().ItemSpacing.y;
+    if (isAuto) {
+      ++numAuto;
+    }
+  }
+  if (numAuto > 0) {
+    availHeight /= numAuto;
+    for (size_t i = 0; i < m_plots.size(); ++i) {
+      m_plots[i]->SetAutoHeight(availHeight);
+    }
   }
 
   double now = wpi::Now() * 1.0e-6;
@@ -925,10 +989,25 @@ void PlotProvider::DisplayMenu() {
   }
 
   if (ImGui::MenuItem("New Plot Window")) {
+    // this is an inefficient algorithm, but the number of windows is small
     char id[32];
-    std::snprintf(id, sizeof(id), "Plot <%d>",
-                  static_cast<int>(m_windows.size()));
-    AddWindow(id, std::make_unique<PlotView>(this));
+    size_t numWindows = m_windows.size();
+    for (size_t i = 0; i <= numWindows; ++i) {
+      std::snprintf(id, sizeof(id), "Plot <%d>", static_cast<int>(i));
+      bool match = false;
+      for (size_t j = i; j < numWindows; ++j) {
+        if (m_windows[j]->GetId() == id) {
+          match = true;
+          break;
+        }
+      }
+      if (!match) {
+        break;
+      }
+    }
+    if (auto win = AddWindow(id, std::make_unique<PlotView>(this))) {
+      win->SetDefaultSize(700, 400);
+    }
   }
 }
 

glass/src/lib/native/cpp/other/StringChooser.cpp

@@ -38,6 +38,4 @@ void glass::DisplayStringChooser(StringChooserModel* model) {
     }
     ImGui::EndCombo();
   }
-
-  ImGui::SameLine();
 }

glass/src/lib/native/include/glass/other/Field2D.h

@@ -4,38 +4,37 @@
 
 #pragma once
 
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
 #include <imgui.h>
+#include <wpi/ArrayRef.h>
 #include <wpi/STLExtras.h>
 #include <wpi/StringRef.h>
+#include <wpi/Twine.h>
 
 #include "glass/Model.h"
 #include "glass/View.h"
 
 namespace glass {
 
-class DataSource;
-
 class FieldObjectModel : public Model {
  public:
-  virtual DataSource* GetXData() = 0;
-  virtual DataSource* GetYData() = 0;
-  virtual DataSource* GetRotationData() = 0;
+  virtual const char* GetName() const = 0;
 
-  virtual void SetPose(double x, double y, double rot) = 0;
-  virtual void SetPosition(double x, double y) = 0;
-  virtual void SetRotation(double rot) = 0;
-};
-
-class FieldObjectGroupModel : public Model {
- public:
-  virtual void ForEachFieldObject(
-      wpi::function_ref<void(FieldObjectModel& model)> func) = 0;
+  virtual wpi::ArrayRef<frc::Pose2d> GetPoses() = 0;
+  virtual void SetPoses(wpi::ArrayRef<frc::Pose2d> poses) = 0;
+  virtual void SetPose(size_t i, frc::Pose2d pose) = 0;
+  virtual void SetPosition(size_t i, frc::Translation2d pos) = 0;
+  virtual void SetRotation(size_t i, frc::Rotation2d rot) = 0;
 };
 
 class Field2DModel : public Model {
  public:
-  virtual void ForEachFieldObjectGroup(
-      wpi::function_ref<void(FieldObjectGroupModel& model, wpi::StringRef name)>
+  virtual FieldObjectModel* AddFieldObject(const wpi::Twine& name) = 0;
+  virtual void RemoveFieldObject(const wpi::Twine& name) = 0;
+  virtual void ForEachFieldObject(
+      wpi::function_ref<void(FieldObjectModel& model, wpi::StringRef name)>
           func) = 0;
 };
 

glass/src/lib/native/include/glass/other/Log.h

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <string>
 #include <vector>
 
 #include <wpi/Twine.h>
@@ -20,10 +21,11 @@ class LogData {
 
   void Clear();
   void Append(const wpi::Twine& msg);
+  const std::string& GetBuffer();
 
  private:
   size_t m_maxLines;
-  std::vector<char> m_buf;
+  std::string m_buf;
   std::vector<size_t> m_lineOffsets{0};
 };
 

glass/src/libnt/native/cpp/NTField2D.cpp

@@ -5,20 +5,22 @@
 #include "glass/networktables/NTField2D.h"
 
 #include <algorithm>
+#include <vector>
 
 #include <ntcore_cpp.h>
+#include <wpi/Endian.h>
+#include <wpi/MathExtras.h>
+#include <wpi/SmallString.h>
 #include <wpi/SmallVector.h>
 
-#include "glass/DataSource.h"
-
 using namespace glass;
 
-class NTField2DModel::GroupModel : public FieldObjectGroupModel {
+class NTField2DModel::ObjectModel : public FieldObjectModel {
  public:
-  GroupModel(wpi::StringRef name, NT_Entry entry)
+  ObjectModel(wpi::StringRef name, NT_Entry entry)
       : m_name{name}, m_entry{entry} {}
 
-  wpi::StringRef GetName() const { return m_name; }
+  const char* GetName() const override { return m_name.c_str(); }
   NT_Entry GetEntry() const { return m_entry; }
 
   void NTUpdate(const nt::Value& value);
@@ -31,146 +33,118 @@ class NTField2DModel::GroupModel : public FieldObjectGroupModel {
   bool Exists() override { return nt::GetEntryType(m_entry) != NT_UNASSIGNED; }
   bool IsReadOnly() override { return false; }
 
-  void ForEachFieldObject(
-      wpi::function_ref<void(FieldObjectModel& model)> func) override;
+  wpi::ArrayRef<frc::Pose2d> GetPoses() override { return m_poses; }
+  void SetPoses(wpi::ArrayRef<frc::Pose2d> poses) override;
+  void SetPose(size_t i, frc::Pose2d pose) override;
+  void SetPosition(size_t i, frc::Translation2d pos) override;
+  void SetRotation(size_t i, frc::Rotation2d rot) override;
 
  private:
+  void UpdateNT();
+
   std::string m_name;
   NT_Entry m_entry;
 
-  // keep count of objects rather than resizing vector, as there is a fair
-  // amount of overhead associated with the latter (DataSource record keeping)
-  size_t m_count = 0;
-  class ObjectModel;
-  std::vector<std::unique_ptr<ObjectModel>> m_objects;
+  std::vector<frc::Pose2d> m_poses;
 };
 
-class NTField2DModel::GroupModel::ObjectModel : public FieldObjectModel {
- public:
-  ObjectModel(wpi::StringRef name, NT_Entry entry, int index)
-      : m_entry{entry},
-        m_index{index},
-        m_x{name + "[" + wpi::Twine{index} + "]/x"},
-        m_y{name + "[" + wpi::Twine{index} + "]/y"},
-        m_rot{name + "[" + wpi::Twine{index} + "]/rot"} {}
+void NTField2DModel::ObjectModel::NTUpdate(const nt::Value& value) {
+  if (value.IsDoubleArray()) {
+    auto arr = value.GetDoubleArray();
+    auto size = arr.size();
+    if ((size % 3) != 0) {
+      return;
+    }
+    m_poses.resize(size / 3);
+    for (size_t i = 0; i < size / 3; ++i) {
+      m_poses[i] = frc::Pose2d{
+          units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
+          frc::Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
+    }
+  } else if (value.IsRaw()) {
+    // treat it simply as an array of doubles
+    wpi::StringRef data = value.GetRaw();
 
-  void SetExists(bool exists) { m_exists = exists; }
-
-  void Update() override {}
-  bool Exists() override { return m_exists; }
-  bool IsReadOnly() override { return false; }
-
-  DataSource* GetXData() override { return &m_x; }
-  DataSource* GetYData() override { return &m_y; }
-  DataSource* GetRotationData() override { return &m_rot; }
-
-  void SetPose(double x, double y, double rot) override;
-  void SetPosition(double x, double y) override;
-  void SetRotation(double rot) override;
-
- private:
-  void SetPoseImpl(double x, double y, double rot, bool setX, bool setY,
-                   bool setRot);
-
-  NT_Entry m_entry;
-  int m_index;
-  bool m_exists = true;
-
- public:
-  DataSource m_x;
-  DataSource m_y;
-  DataSource m_rot;
-};
-
-void NTField2DModel::GroupModel::NTUpdate(const nt::Value& value) {
-  if (!value.IsDoubleArray()) {
-    m_count = 0;
-    return;
-  }
-
-  auto arr = value.GetDoubleArray();
-  // must be triples
-  if ((arr.size() % 3) != 0) {
-    m_count = 0;
-    return;
-  }
-
-  m_count = arr.size() / 3;
-  if (m_count > m_objects.size()) {
-    m_objects.reserve(m_count);
-    for (size_t i = m_objects.size(); i < m_count; ++i) {
-      m_objects.emplace_back(std::make_unique<ObjectModel>(m_name, m_entry, i));
+    // must be triples of doubles
+    auto size = data.size();
+    if ((size % (3 * 8)) != 0) {
+      return;
+    }
+    m_poses.resize(size / (3 * 8));
+    const char* p = data.begin();
+    for (size_t i = 0; i < size / (3 * 8); ++i) {
+      double x = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      double y = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      double rot = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      m_poses[i] = frc::Pose2d{units::meter_t{x}, units::meter_t{y},
+                               frc::Rotation2d{units::degree_t{rot}}};
     }
   }
-  if (m_count < m_objects.size()) {
-    for (size_t i = m_count; i < m_objects.size(); ++i) {
-      m_objects[i]->SetExists(false);
+}
+
+void NTField2DModel::ObjectModel::UpdateNT() {
+  if (m_poses.size() < (255 / 3)) {
+    wpi::SmallVector<double, 9> arr;
+    for (auto&& pose : m_poses) {
+      auto& translation = pose.Translation();
+      arr.push_back(translation.X().to<double>());
+      arr.push_back(translation.Y().to<double>());
+      arr.push_back(pose.Rotation().Degrees().to<double>());
     }
-  }
-
-  for (size_t i = 0; i < m_count; ++i) {
-    auto& obj = m_objects[i];
-    obj->SetExists(true);
-    obj->m_x.SetValue(arr[i * 3], value.last_change());
-    obj->m_y.SetValue(arr[i * 3 + 1], value.last_change());
-    obj->m_rot.SetValue(arr[i * 3 + 2], value.last_change());
+    nt::SetEntryTypeValue(m_entry, nt::Value::MakeDoubleArray(arr));
+  } else {
+    // send as raw array of doubles if too big for NT array
+    std::vector<char> arr;
+    arr.resize(m_poses.size() * 3 * 8);
+    char* p = arr.data();
+    for (auto&& pose : m_poses) {
+      auto& translation = pose.Translation();
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(translation.X().to<double>()));
+      p += 8;
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(translation.Y().to<double>()));
+      p += 8;
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(pose.Rotation().Degrees().to<double>()));
+      p += 8;
+    }
+    nt::SetEntryTypeValue(
+        m_entry, nt::Value::MakeRaw(wpi::StringRef{arr.data(), arr.size()}));
   }
 }
 
-void NTField2DModel::GroupModel::ForEachFieldObject(
-    wpi::function_ref<void(FieldObjectModel& model)> func) {
-  for (size_t i = 0; i < m_count; ++i) {
-    func(*m_objects[i]);
+void NTField2DModel::ObjectModel::SetPoses(wpi::ArrayRef<frc::Pose2d> poses) {
+  m_poses = poses;
+  UpdateNT();
+}
+
+void NTField2DModel::ObjectModel::SetPose(size_t i, frc::Pose2d pose) {
+  if (i < m_poses.size()) {
+    m_poses[i] = pose;
+    UpdateNT();
   }
 }
 
-void NTField2DModel::GroupModel::ObjectModel::SetPose(double x, double y,
-                                                      double rot) {
-  SetPoseImpl(x, y, rot, true, true, true);
+void NTField2DModel::ObjectModel::SetPosition(size_t i,
+                                              frc::Translation2d pos) {
+  if (i < m_poses.size()) {
+    m_poses[i] = frc::Pose2d{pos, m_poses[i].Rotation()};
+    UpdateNT();
+  }
 }
 
-void NTField2DModel::GroupModel::ObjectModel::SetPosition(double x, double y) {
-  SetPoseImpl(x, y, 0, true, true, false);
-}
-
-void NTField2DModel::GroupModel::ObjectModel::SetRotation(double rot) {
-  SetPoseImpl(0, 0, rot, false, false, true);
-}
-
-void NTField2DModel::GroupModel::ObjectModel::SetPoseImpl(double x, double y,
-                                                          double rot, bool setX,
-                                                          bool setY,
-                                                          bool setRot) {
-  // get from NT, validate type and size
-  auto value = nt::GetEntryValue(m_entry);
-  if (!value || !value->IsDoubleArray()) {
-    return;
+void NTField2DModel::ObjectModel::SetRotation(size_t i, frc::Rotation2d rot) {
+  if (i < m_poses.size()) {
+    m_poses[i] = frc::Pose2d{m_poses[i].Translation(), rot};
+    UpdateNT();
   }
-  auto origArr = value->GetDoubleArray();
-  if (static_cast<int>(origArr.size()) < ((m_index + 1) * 3)) {
-    return;
-  }
-
-  // copy existing array
-  wpi::SmallVector<double, 8> arr;
-  arr.reserve(origArr.size());
-  for (auto&& elem : origArr) {
-    arr.emplace_back(elem);
-  }
-
-  // update value
-  if (setX) {
-    arr[m_index * 3 + 0] = x;
-  }
-  if (setY) {
-    arr[m_index * 3 + 1] = y;
-  }
-  if (setRot) {
-    arr[m_index * 3 + 2] = rot;
-  }
-
-  // set back to NT
-  nt::SetEntryValue(m_entry, nt::Value::MakeDoubleArray(arr));
 }
 
 NTField2DModel::NTField2DModel(wpi::StringRef path)
@@ -199,9 +173,9 @@ void NTField2DModel::Update() {
     // common case: update of existing entry; search by entry
     if (event.flags & NT_NOTIFY_UPDATE) {
       auto it = std::find_if(
-          m_groups.begin(), m_groups.end(),
+          m_objects.begin(), m_objects.end(),
           [&](const auto& e) { return e->GetEntry() == event.entry; });
-      if (it != m_groups.end()) {
+      if (it != m_objects.end()) {
         (*it)->NTUpdate(*event.value);
         continue;
       }
@@ -213,20 +187,16 @@ void NTField2DModel::Update() {
       if (name.empty() || name[0] == '.') {
         continue;
       }
-      auto it = std::lower_bound(m_groups.begin(), m_groups.end(), name,
-                                 [](const auto& e, wpi::StringRef name) {
-                                   return e->GetName() < name;
-                                 });
-      bool match = (it != m_groups.end() && (*it)->GetName() == name);
+      auto [it, match] = Find(event.name);
       if (event.flags & NT_NOTIFY_DELETE) {
         if (match) {
-          m_groups.erase(it);
+          m_objects.erase(it);
         }
         continue;
       } else if (event.flags & NT_NOTIFY_NEW) {
         if (!match) {
-          it = m_groups.emplace(
-              it, std::make_unique<GroupModel>(event.name, event.entry));
+          it = m_objects.emplace(
+              it, std::make_unique<ObjectModel>(event.name, event.entry));
         }
       } else if (!match) {
         continue;
@@ -246,12 +216,40 @@ bool NTField2DModel::IsReadOnly() {
   return false;
 }
 
-void NTField2DModel::ForEachFieldObjectGroup(
-    wpi::function_ref<void(FieldObjectGroupModel& model, wpi::StringRef name)>
+FieldObjectModel* NTField2DModel::AddFieldObject(const wpi::Twine& name) {
+  wpi::SmallString<128> fullNameBuf;
+  wpi::StringRef fullName = (m_path + name).toStringRef(fullNameBuf);
+  auto [it, match] = Find(fullName);
+  if (!match) {
+    it = m_objects.emplace(
+        it, std::make_unique<ObjectModel>(fullName, m_nt.GetEntry(fullName)));
+  }
+  return it->get();
+}
+
+void NTField2DModel::RemoveFieldObject(const wpi::Twine& name) {
+  wpi::SmallString<128> fullNameBuf;
+  auto [it, match] = Find((m_path + name).toStringRef(fullNameBuf));
+  if (match) {
+    nt::DeleteEntry((*it)->GetEntry());
+    m_objects.erase(it);
+  }
+}
+
+void NTField2DModel::ForEachFieldObject(
+    wpi::function_ref<void(FieldObjectModel& model, wpi::StringRef name)>
         func) {
-  for (auto&& group : m_groups) {
-    if (group->Exists()) {
-      func(*group, wpi::StringRef{group->GetName()}.drop_front(m_path.size()));
+  for (auto&& obj : m_objects) {
+    if (obj->Exists()) {
+      func(*obj, wpi::StringRef{obj->GetName()}.drop_front(m_path.size()));
     }
   }
 }
+
+std::pair<NTField2DModel::Objects::iterator, bool> NTField2DModel::Find(
+    wpi::StringRef fullName) {
+  auto it = std::lower_bound(
+      m_objects.begin(), m_objects.end(), fullName,
+      [](const auto& e, wpi::StringRef name) { return e->GetName() < name; });
+  return {it, it != m_objects.end() && (*it)->GetName() == fullName};
+}

glass/src/libnt/native/cpp/NTStringChooser.cpp

@@ -39,17 +39,30 @@ void NTStringChooserModel::SetOptions(wpi::ArrayRef<std::string> val) {
 
 void NTStringChooserModel::Update() {
   for (auto&& event : m_nt.PollListener()) {
-    if (event.entry == m_default && event.value && event.value->IsString()) {
-      m_defaultValue = event.value->GetString();
-    } else if (event.entry == m_selected && event.value &&
-               event.value->IsString()) {
-      m_selectedValue = event.value->GetString();
-    } else if (event.entry == m_active && event.value &&
-               event.value->IsString()) {
-      m_activeValue = event.value->GetString();
-    } else if (event.entry == m_options && event.value &&
-               event.value->IsStringArray()) {
-      m_optionsValue = event.value->GetStringArray();
+    if (event.entry == m_default) {
+      if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+        m_defaultValue.clear();
+      } else if (event.value && event.value->IsString()) {
+        m_defaultValue = event.value->GetString();
+      }
+    } else if (event.entry == m_selected) {
+      if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+        m_selectedValue.clear();
+      } else if (event.value && event.value->IsString()) {
+        m_selectedValue = event.value->GetString();
+      }
+    } else if (event.entry == m_active) {
+      if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+        m_activeValue.clear();
+      } else if (event.value && event.value->IsString()) {
+        m_activeValue = event.value->GetString();
+      }
+    } else if (event.entry == m_options) {
+      if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+        m_optionsValue.clear();
+      } else if (event.value && event.value->IsStringArray()) {
+        m_optionsValue = event.value->GetStringArray();
+      }
     }
   }
 }

glass/src/libnt/native/cpp/NetworkTables.cpp

@@ -266,8 +266,8 @@ static std::shared_ptr<nt::Value> StringToBooleanArray(wpi::StringRef in) {
 static std::shared_ptr<nt::Value> StringToDoubleArray(wpi::StringRef in) {
   in = in.trim();
   if (in.empty()) {
-    return nt::NetworkTableValue::MakeBooleanArray(
-        std::initializer_list<bool>{});
+    return nt::NetworkTableValue::MakeDoubleArray(
+        std::initializer_list<double>{});
   }
   if (in.front() == '[') {
     in = in.drop_front();

glass/src/libnt/native/cpp/NetworkTablesSettings.cpp

@@ -0,0 +1,131 @@
+// 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.
+
+#include "glass/networktables/NetworkTablesSettings.h"
+
+#include <utility>
+
+#include <imgui.h>
+#include <imgui_stdlib.h>
+#include <ntcore_cpp.h>
+#include <wpi/SmallVector.h>
+#include <wpi/StringRef.h>
+#include <wpi/raw_ostream.h>
+
+#include "glass/Context.h"
+
+using namespace glass;
+
+void NetworkTablesSettings::Thread::Main() {
+  while (m_active) {
+    // wait to be woken up
+    std::unique_lock lock(m_mutex);
+    m_cond.wait(lock, [&] { return !m_active || m_restart; });
+    if (!m_active) {
+      break;
+    }
+
+    // clear restart flag
+    m_restart = false;
+
+    int mode;
+    bool dsClient;
+
+    do {
+      mode = m_mode;
+      dsClient = m_dsClient;
+
+      // release lock while stopping to avoid blocking GUI
+      lock.unlock();
+
+      // if just changing servers in client mode, no need to stop and restart
+      unsigned int curMode = nt::GetNetworkMode(m_inst);
+      if (mode != 1 || (curMode & NT_NET_MODE_SERVER) != 0) {
+        nt::StopClient(m_inst);
+        nt::StopServer(m_inst);
+        nt::StopLocal(m_inst);
+      }
+
+      if (m_mode != 1 || !dsClient) {
+        nt::StopDSClient(m_inst);
+      }
+
+      lock.lock();
+    } while (mode != m_mode || dsClient != m_dsClient);
+
+    if (m_mode == 1) {
+      wpi::StringRef serverTeam{m_serverTeam};
+      unsigned int team;
+      if (!serverTeam.contains('.') && !serverTeam.getAsInteger(10, team)) {
+        nt::StartClientTeam(m_inst, team, NT_DEFAULT_PORT);
+      } else {
+        wpi::SmallVector<wpi::StringRef, 4> serverNames;
+        wpi::SmallVector<std::pair<wpi::StringRef, unsigned int>, 4> servers;
+        serverTeam.split(serverNames, ',', -1, false);
+        for (auto&& serverName : serverNames) {
+          servers.emplace_back(serverName, NT_DEFAULT_PORT);
+        }
+        nt::StartClient(m_inst, servers);
+      }
+
+      if (m_dsClient) {
+        nt::StartDSClient(m_inst, NT_DEFAULT_PORT);
+      }
+    } else if (m_mode == 2) {
+      nt::StartServer(m_inst, m_iniName.c_str(), m_listenAddress.c_str(),
+                      NT_DEFAULT_PORT);
+    }
+  }
+}
+
+NetworkTablesSettings::NetworkTablesSettings(NT_Inst inst,
+                                             const char* storageName) {
+  auto& storage = glass::GetStorage(storageName);
+  m_pMode = storage.GetIntRef("mode");
+  m_pIniName = storage.GetStringRef("iniName", "networktables.ini");
+  m_pServerTeam = storage.GetStringRef("serverTeam");
+  m_pListenAddress = storage.GetStringRef("listenAddress");
+  m_pDsClient = storage.GetBoolRef("dsClient", true);
+
+  m_thread.Start(inst);
+}
+
+void NetworkTablesSettings::Update() {
+  if (!m_restart) {
+    return;
+  }
+  m_restart = false;
+
+  // do actual operation on thread
+  auto thr = m_thread.GetThread();
+  thr->m_restart = true;
+  thr->m_mode = *m_pMode;
+  thr->m_iniName = *m_pIniName;
+  thr->m_serverTeam = *m_pServerTeam;
+  thr->m_listenAddress = *m_pListenAddress;
+  thr->m_dsClient = *m_pDsClient;
+  thr->m_cond.notify_one();
+}
+
+bool NetworkTablesSettings::Display() {
+  static const char* modeOptions[] = {"Disabled", "Client", "Server"};
+  ImGui::Combo("Mode", m_pMode, modeOptions, m_serverOption ? 3 : 2);
+  switch (*m_pMode) {
+    case 1:
+      ImGui::InputText("Team/IP", m_pServerTeam);
+      ImGui::Checkbox("Get Address from DS", m_pDsClient);
+      break;
+    case 2:
+      ImGui::InputText("Listen Address", m_pListenAddress);
+      ImGui::InputText("ini Filename", m_pIniName);
+      break;
+    default:
+      break;
+  }
+  if (ImGui::Button("Apply")) {
+    m_restart = true;
+    return true;
+  }
+  return false;
+}

glass/src/libnt/native/include/glass/networktables/NTField2D.h

@@ -6,6 +6,7 @@
 
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include <ntcore_cpp.h>
@@ -32,8 +33,10 @@ class NTField2DModel : public Field2DModel {
   bool Exists() override;
   bool IsReadOnly() override;
 
-  void ForEachFieldObjectGroup(
-      wpi::function_ref<void(FieldObjectGroupModel& model, wpi::StringRef name)>
+  FieldObjectModel* AddFieldObject(const wpi::Twine& name) override;
+  void RemoveFieldObject(const wpi::Twine& name) override;
+  void ForEachFieldObject(
+      wpi::function_ref<void(FieldObjectModel& model, wpi::StringRef name)>
           func) override;
 
  private:
@@ -42,8 +45,11 @@ class NTField2DModel : public Field2DModel {
   NT_Entry m_name;
   std::string m_nameValue;
 
-  class GroupModel;
-  std::vector<std::unique_ptr<GroupModel>> m_groups;
+  class ObjectModel;
+  using Objects = std::vector<std::unique_ptr<ObjectModel>>;
+  Objects m_objects;
+
+  std::pair<Objects::iterator, bool> Find(wpi::StringRef fullName);
 };
 
 }  // namespace glass

glass/src/libnt/native/include/glass/networktables/NetworkTablesHelper.h

@@ -28,7 +28,8 @@ class NetworkTablesHelper {
   }
 
   static constexpr int kDefaultListenerFlags =
-      NT_NOTIFY_LOCAL | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE | NT_NOTIFY_IMMEDIATE;
+      NT_NOTIFY_LOCAL | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE | NT_NOTIFY_DELETE |
+      NT_NOTIFY_IMMEDIATE;
 
   NT_EntryListener AddListener(NT_Entry entry,
                                unsigned int flags = kDefaultListenerFlags) {

glass/src/libnt/native/include/glass/networktables/NetworkTablesSettings.h

@@ -7,26 +7,53 @@
 #include <string>
 
 #include <ntcore_cpp.h>
+#include <wpi/SafeThread.h>
 
 namespace wpi {
 template <typename T>
 class SmallVectorImpl;
 }  // namespace wpi
 
+namespace glass {
+
 class NetworkTablesSettings {
  public:
   explicit NetworkTablesSettings(
       NT_Inst inst = nt::GetDefaultInstance(),
       const char* storageName = "NetworkTables Settings");
 
+  /**
+   * Enables or disables the server option.  Default is enabled.
+   */
+  void EnableServerOption(bool enable) { m_serverOption = enable; }
+
   void Update();
-  void Display();
+  bool Display();
 
  private:
-  NT_Inst m_inst;
   bool m_restart = true;
+  bool m_serverOption = true;
   int* m_pMode;
   std::string* m_pIniName;
   std::string* m_pServerTeam;
   std::string* m_pListenAddress;
+  bool* m_pDsClient;
+
+  class Thread : public wpi::SafeThread {
+   public:
+    explicit Thread(NT_Inst inst) : m_inst{inst} {}
+
+    void Main() override;
+
+    NT_Inst m_inst;
+    bool m_restart = false;
+    int m_mode;
+    std::string m_iniName;
+    std::string m_serverTeam;
+    std::string m_listenAddress;
+    bool m_dsClient;
+  };
+  wpi::SafeThreadOwner<Thread> m_thread;
 };
+
+}  // namespace glass

hal/src/main/java/edu/wpi/first/hal/NotifierJNI.java

@@ -14,6 +14,9 @@ public class NotifierJNI extends JNIWrapper {
   /** Initializes the notifier. */
   public static native int initializeNotifier();
 
+  /** Sets the HAL notifier thread priority. */
+  public static native boolean setHALThreadPriority(boolean realTime, int priority);
+
   /** Sets the name of the notifier. */
   public static native void setNotifierName(int notifierHandle, String name);
 

hal/src/main/native/athena/Notifier.cpp

@@ -17,6 +17,7 @@
 #include "hal/ChipObject.h"
 #include "hal/Errors.h"
 #include "hal/HAL.h"
+#include "hal/Threads.h"
 #include "hal/handles/UnlimitedHandleResource.h"
 
 using namespace hal;
@@ -156,6 +157,12 @@ HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) {
   return handle;
 }
 
+HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
+                                       int32_t* status) {
+  auto native = notifierThread.native_handle();
+  return HAL_SetThreadPriority(&native, realTime, priority, status);
+}
+
 void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
                          int32_t* status) {}
 

hal/src/main/native/cpp/jni/NotifierJNI.cpp

@@ -36,6 +36,19 @@ Java_edu_wpi_first_hal_NotifierJNI_initializeNotifier
   return (jint)notifierHandle;
 }
 
+/*
+ * Class:     edu_wpi_first_hal_NotifierJNI
+ * Method:    setHALThreadPriority
+ * Signature: (ZI)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_NotifierJNI_setHALThreadPriority
+  (JNIEnv* env, jclass, jboolean realTime, jint priority)
+{
+  int32_t status = 0;
+  return HAL_SetNotifierThreadPriority(realTime, priority, &status);
+}
+
 /*
  * Class:     edu_wpi_first_hal_NotifierJNI
  * Method:    setNotifierName

hal/src/main/native/include/hal/Notifier.h

@@ -30,6 +30,25 @@ extern "C" {
  */
 HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status);
 
+/**
+ * Sets the HAL notifier thread priority.
+ *
+ * The HAL notifier thread is responsible for managing the FPGA's notifier
+ * interrupt and waking up user's Notifiers when it's their time to run.
+ * Giving the HAL notifier thread real-time priority helps ensure the user's
+ * real-time Notifiers, if any, are notified to run in a timely manner.
+ *
+ * @param realTime Set to true to set a real-time priority, false for standard
+ *                 priority.
+ * @param priority Priority to set the thread to. For real-time, this is 1-99
+ *                 with 99 being highest. For non-real-time, this is forced to
+ *                 0. See "man 7 sched" for more details.
+ * @param status   Error status variable. 0 on success.
+ * @return         True on success.
+ */
+HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
+                                       int32_t* status);
+
 /**
  * Sets the name of a notifier.
  *

hal/src/main/native/sim/Notifier.cpp

@@ -174,6 +174,11 @@ HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) {
   return handle;
 }
 
+HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
+                                       int32_t* status) {
+  return true;
+}
+
 void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
                          int32_t* status) {
   auto notifier = notifierHandles->Get(notifierHandle);

ntcore/src/main/native/cpp/ConnectionNotifier.h

@@ -5,7 +5,8 @@
 #ifndef NTCORE_CONNECTIONNOTIFIER_H_
 #define NTCORE_CONNECTIONNOTIFIER_H_
 
-#include "CallbackManager.h"
+#include <wpi/CallbackManager.h>
+
 #include "Handle.h"
 #include "IConnectionNotifier.h"
 #include "ntcore_cpp.h"
@@ -15,7 +16,8 @@ namespace nt {
 namespace impl {
 
 class ConnectionNotifierThread
-    : public CallbackThread<ConnectionNotifierThread, ConnectionNotification> {
+    : public wpi::CallbackThread<ConnectionNotifierThread,
+                                 ConnectionNotification> {
  public:
   explicit ConnectionNotifierThread(int inst) : m_inst(inst) {}
 
@@ -42,11 +44,11 @@ class ConnectionNotifierThread
 
 class ConnectionNotifier
     : public IConnectionNotifier,
-      public CallbackManager<ConnectionNotifier,
-                             impl::ConnectionNotifierThread> {
+      public wpi::CallbackManager<ConnectionNotifier,
+                                  impl::ConnectionNotifierThread> {
   friend class ConnectionNotifierTest;
-  friend class CallbackManager<ConnectionNotifier,
-                               impl::ConnectionNotifierThread>;
+  friend class wpi::CallbackManager<ConnectionNotifier,
+                                    impl::ConnectionNotifierThread>;
 
  public:
   explicit ConnectionNotifier(int inst);

ntcore/src/main/native/cpp/EntryNotifier.h

@@ -9,7 +9,8 @@
 #include <memory>
 #include <string>
 
-#include "CallbackManager.h"
+#include <wpi/CallbackManager.h>
+
 #include "Handle.h"
 #include "IEntryNotifier.h"
 #include "ntcore_cpp.h"
@@ -23,22 +24,23 @@ namespace nt {
 namespace impl {
 
 struct EntryListenerData
-    : public ListenerData<std::function<void(const EntryNotification& event)>> {
+    : public wpi::CallbackListenerData<
+          std::function<void(const EntryNotification& event)>> {
   EntryListenerData() = default;
   EntryListenerData(
       std::function<void(const EntryNotification& event)> callback_,
       StringRef prefix_, unsigned int flags_)
-      : ListenerData(callback_), prefix(prefix_), flags(flags_) {}
+      : CallbackListenerData(callback_), prefix(prefix_), flags(flags_) {}
   EntryListenerData(
       std::function<void(const EntryNotification& event)> callback_,
       NT_Entry entry_, unsigned int flags_)
-      : ListenerData(callback_), entry(entry_), flags(flags_) {}
+      : CallbackListenerData(callback_), entry(entry_), flags(flags_) {}
   EntryListenerData(unsigned int poller_uid_, StringRef prefix_,
                     unsigned int flags_)
-      : ListenerData(poller_uid_), prefix(prefix_), flags(flags_) {}
+      : CallbackListenerData(poller_uid_), prefix(prefix_), flags(flags_) {}
   EntryListenerData(unsigned int poller_uid_, NT_Entry entry_,
                     unsigned int flags_)
-      : ListenerData(poller_uid_), entry(entry_), flags(flags_) {}
+      : CallbackListenerData(poller_uid_), entry(entry_), flags(flags_) {}
 
   std::string prefix;
   NT_Entry entry = 0;
@@ -46,8 +48,8 @@ struct EntryListenerData
 };
 
 class EntryNotifierThread
-    : public CallbackThread<EntryNotifierThread, EntryNotification,
-                            EntryListenerData> {
+    : public wpi::CallbackThread<EntryNotifierThread, EntryNotification,
+                                 EntryListenerData> {
  public:
   explicit EntryNotifierThread(int inst) : m_inst(inst) {}
 
@@ -71,9 +73,9 @@ class EntryNotifierThread
 
 class EntryNotifier
     : public IEntryNotifier,
-      public CallbackManager<EntryNotifier, impl::EntryNotifierThread> {
+      public wpi::CallbackManager<EntryNotifier, impl::EntryNotifierThread> {
   friend class EntryNotifierTest;
-  friend class CallbackManager<EntryNotifier, impl::EntryNotifierThread>;
+  friend class wpi::CallbackManager<EntryNotifier, impl::EntryNotifierThread>;
 
  public:
   explicit EntryNotifier(int inst, wpi::Logger& logger);

ntcore/src/main/native/cpp/LoggerImpl.h

@@ -5,7 +5,8 @@
 #ifndef NTCORE_LOGGERIMPL_H_
 #define NTCORE_LOGGERIMPL_H_
 
-#include "CallbackManager.h"
+#include <wpi/CallbackManager.h>
+
 #include "Handle.h"
 #include "ntcore_cpp.h"
 
@@ -13,15 +14,17 @@ namespace nt {
 
 namespace impl {
 
-struct LoggerListenerData
-    : public ListenerData<std::function<void(const LogMessage& msg)>> {
+struct LoggerListenerData : public wpi::CallbackListenerData<
+                                std::function<void(const LogMessage& msg)>> {
   LoggerListenerData() = default;
   LoggerListenerData(std::function<void(const LogMessage& msg)> callback_,
                      unsigned int min_level_, unsigned int max_level_)
-      : ListenerData(callback_), min_level(min_level_), max_level(max_level_) {}
+      : CallbackListenerData(callback_),
+        min_level(min_level_),
+        max_level(max_level_) {}
   LoggerListenerData(unsigned int poller_uid_, unsigned int min_level_,
                      unsigned int max_level_)
-      : ListenerData(poller_uid_),
+      : CallbackListenerData(poller_uid_),
         min_level(min_level_),
         max_level(max_level_) {}
 
@@ -30,7 +33,7 @@ struct LoggerListenerData
 };
 
 class LoggerThread
-    : public CallbackThread<LoggerThread, LogMessage, LoggerListenerData> {
+    : public wpi::CallbackThread<LoggerThread, LogMessage, LoggerListenerData> {
  public:
   explicit LoggerThread(int inst) : m_inst(inst) {}
 
@@ -52,9 +55,9 @@ class LoggerThread
 
 }  // namespace impl
 
-class LoggerImpl : public CallbackManager<LoggerImpl, impl::LoggerThread> {
+class LoggerImpl : public wpi::CallbackManager<LoggerImpl, impl::LoggerThread> {
   friend class LoggerTest;
-  friend class CallbackManager<LoggerImpl, impl::LoggerThread>;
+  friend class wpi::CallbackManager<LoggerImpl, impl::LoggerThread>;
 
  public:
   explicit LoggerImpl(int inst);

ntcore/src/main/native/cpp/RpcServer.h

@@ -7,10 +7,10 @@
 
 #include <utility>
 
+#include <wpi/CallbackManager.h>
 #include <wpi/DenseMap.h>
 #include <wpi/mutex.h>
 
-#include "CallbackManager.h"
 #include "Handle.h"
 #include "IRpcServer.h"
 #include "Log.h"
@@ -32,11 +32,11 @@ struct RpcNotifierData : public RpcAnswer {
 };
 
 using RpcListenerData =
-    ListenerData<std::function<void(const RpcAnswer& answer)>>;
+    wpi::CallbackListenerData<std::function<void(const RpcAnswer& answer)>>;
 
 class RpcServerThread
-    : public CallbackThread<RpcServerThread, RpcAnswer, RpcListenerData,
-                            RpcNotifierData> {
+    : public wpi::CallbackThread<RpcServerThread, RpcAnswer, RpcListenerData,
+                                 RpcNotifierData> {
  public:
   RpcServerThread(int inst, wpi::Logger& logger)
       : m_inst(inst), m_logger(logger) {}
@@ -78,10 +78,11 @@ class RpcServerThread
 
 }  // namespace impl
 
-class RpcServer : public IRpcServer,
-                  public CallbackManager<RpcServer, impl::RpcServerThread> {
+class RpcServer
+    : public IRpcServer,
+      public wpi::CallbackManager<RpcServer, impl::RpcServerThread> {
   friend class RpcServerTest;
-  friend class CallbackManager<RpcServer, impl::RpcServerThread>;
+  friend class wpi::CallbackManager<RpcServer, impl::RpcServerThread>;
 
  public:
   RpcServer(int inst, wpi::Logger& logger);

shared/javacpp/publish.gradle

@@ -31,8 +31,14 @@ task cppHeadersZip(type: Zip) {
         into '/'
     }
 
-    from('src/main/native/include') {
-        into '/'
+    ext.includeDirs = [
+        project.file('src/main/native/include')
+    ]
+
+    ext.includeDirs.each {
+        from(it) {
+            into '/'
+        }
     }
 }
 

shared/jni/publish.gradle

@@ -50,8 +50,14 @@ task cppHeadersZip(type: Zip) {
         into '/'
     }
 
-    from('src/main/native/include') {
-        into '/'
+    ext.includeDirs = [
+        project.file('src/main/native/include')
+    ]
+
+    ext.includeDirs.each {
+        from(it) {
+            into '/'
+        }
     }
 }
 

shared/plugins/publish.gradle

@@ -43,18 +43,7 @@ addTaskToCopyAllOutputs(cppHeadersZip)
 
 model {
     publishing {
-        def pluginTaskList = createComponentZipTasks($.components, [pluginName], zipBaseName, Zip, project, { task, value ->
-            value.each { binary ->
-                if (binary.buildable) {
-                    if (binary instanceof SharedLibraryBinarySpec) {
-                        task.dependsOn binary.buildTask
-                        task.from(binary.sharedLibraryFile) {
-                            into nativeUtils.getPlatformPath(binary) + '/shared'
-                        }
-                    }
-                }
-            }
-        })
+        def pluginTaskList = createComponentZipTasks($.components, [pluginName], zipBaseName, Zip, project, includeStandardZipFormat)
 
         publications {
             cpp(MavenPublication) {

simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp

@@ -779,6 +779,10 @@ void KeyboardJoystick::SettingsDisplay() {
   ImGui::PopItemWidth();
 }
 
+static inline bool IsKeyDown(ImGuiIO& io, int key) {
+  return key >= 0 && key < IM_ARRAYSIZE(ImGuiIO::KeysDown) && io.KeysDown[key];
+}
+
 void KeyboardJoystick::Update() {
   ImGuiIO& io = ImGui::GetIO();
 
@@ -792,7 +796,7 @@ void KeyboardJoystick::Update() {
     auto& config = m_axisConfig[i];
     float& axisValue = m_data.axes.axes[i];
     // increase/decrease while key held down (to saturation); decay back to 0
-    if (config.incKey >= 0 && io.KeysDown[config.incKey]) {
+    if (IsKeyDown(io, config.incKey)) {
       axisValue += config.keyRate;
       if (axisValue > config.maxAbsValue) {
         axisValue = config.maxAbsValue;
@@ -805,7 +809,7 @@ void KeyboardJoystick::Update() {
       }
     }
 
-    if (config.decKey >= 0 && io.KeysDown[config.decKey]) {
+    if (IsKeyDown(io, config.decKey)) {
       axisValue -= config.keyRate;
       if (axisValue < -config.maxAbsValue) {
         axisValue = -config.maxAbsValue;
@@ -823,7 +827,7 @@ void KeyboardJoystick::Update() {
   m_data.buttons.buttons = 0;
   m_anyButtonPressed = false;
   for (int i = 0; i < m_data.buttons.count; ++i) {
-    if (m_buttonKey[i] >= 0 && io.KeysDown[m_buttonKey[i]]) {
+    if (IsKeyDown(io, m_buttonKey[i])) {
       m_data.buttons.buttons |= 1u << i;
       m_anyButtonPressed = true;
     }
@@ -834,21 +838,21 @@ void KeyboardJoystick::Update() {
     auto& config = m_povConfig[i];
     auto& povValue = m_data.povs.povs[i];
     povValue = -1;
-    if (config.key0 >= 0 && io.KeysDown[config.key0]) {
+    if (IsKeyDown(io, config.key0)) {
       povValue = 0;
-    } else if (config.key45 >= 0 && io.KeysDown[config.key45]) {
+    } else if (IsKeyDown(io, config.key45)) {
       povValue = 45;
-    } else if (config.key90 >= 0 && io.KeysDown[config.key90]) {
+    } else if (IsKeyDown(io, config.key90)) {
       povValue = 90;
-    } else if (config.key135 >= 0 && io.KeysDown[config.key135]) {
+    } else if (IsKeyDown(io, config.key135)) {
       povValue = 135;
-    } else if (config.key180 >= 0 && io.KeysDown[config.key180]) {
+    } else if (IsKeyDown(io, config.key180)) {
       povValue = 180;
-    } else if (config.key225 >= 0 && io.KeysDown[config.key225]) {
+    } else if (IsKeyDown(io, config.key225)) {
       povValue = 225;
-    } else if (config.key270 >= 0 && io.KeysDown[config.key270]) {
+    } else if (IsKeyDown(io, config.key270)) {
       povValue = 270;
-    } else if (config.key315 >= 0 && io.KeysDown[config.key315]) {
+    } else if (IsKeyDown(io, config.key315)) {
       povValue = 315;
     }
   }
@@ -970,6 +974,9 @@ void KeyboardJoystick::ReadIni(wpi::StringRef name, wpi::StringRef value) {
     if (value.getAsInteger(10, v)) {
       return;
     }
+    if (v < 0 || v >= IM_ARRAYSIZE(ImGuiIO::KeysDown)) {
+      return;
+    }
     m_buttonKey[index] = v;
   } else if (name.startswith("pov")) {
     name = name.drop_front(3);
@@ -993,6 +1000,9 @@ void KeyboardJoystick::ReadIni(wpi::StringRef name, wpi::StringRef value) {
     if (value.getAsInteger(10, v)) {
       return;
     }
+    if (v < 0 || v >= IM_ARRAYSIZE(ImGuiIO::KeysDown)) {
+      return;
+    }
     if (name == "key0") {
       m_povConfig[index].key0 = v;
     } else if (name == "key45") {

simulation/halsim_gui/src/main/native/cpp/SimDeviceGui.cpp

@@ -61,6 +61,7 @@ class SimDevicesModel : public glass::Model {
 }  // namespace
 
 static SimDevicesModel* gSimDevicesModel;
+static bool gSimDevicesShowPrefix = false;
 
 void SimDevicesModel::Update() {
   HALSIM_EnumerateSimDevices(
@@ -136,10 +137,14 @@ static void DisplaySimValue(const char* name, void* data,
 
 static void DisplaySimDevice(const char* name, void* data,
                              HAL_SimDeviceHandle handle) {
-  // only show "Foo" portion of "Accel:Foo"
-  auto [type, id] = wpi::StringRef{name}.split(':');
-  if (id.empty()) {
-    id = type;
+  wpi::StringRef id{name};
+  if (!gSimDevicesShowPrefix) {
+    // only show "Foo" portion of "Accel:Foo"
+    wpi::StringRef type;
+    std::tie(type, id) = id.split(':');
+    if (id.empty()) {
+      id = type;
+    }
   }
   if (glass::BeginDevice(id.data())) {
     HALSIM_EnumerateSimValues(handle, data, DisplaySimValue);
@@ -154,8 +159,14 @@ void SimDeviceGui::Initialize() {
       [](glass::Window* win, glass::Model* model) {
         win->SetDefaultPos(1025, 20);
         win->SetDefaultSize(250, 695);
-        return glass::MakeFunctionView(
-            [=] { static_cast<glass::DeviceTreeModel*>(model)->Display(); });
+        win->DisableRenamePopup();
+        return glass::MakeFunctionView([=] {
+          if (ImGui::BeginPopupContextItem()) {
+            ImGui::Checkbox("Show prefix", &gSimDevicesShowPrefix);
+            ImGui::EndPopup();
+          }
+          static_cast<glass::DeviceTreeModel*>(model)->Display();
+        });
       });
   HALSimGui::halProvider.ShowDefault("Other Devices");
 

wpigui/src/main/native/cpp/wpigui.cpp

@@ -213,7 +213,23 @@ bool gui::Initialize(const char* title, int width, int height) {
 
   // Update window settings
   if (gContext->xPos != -1 && gContext->yPos != -1) {
-    glfwSetWindowPos(gContext->window, gContext->xPos, gContext->yPos);
+    // check to make sure the position isn't off-screen
+    bool found = false;
+    int monCount;
+    GLFWmonitor** monitors = glfwGetMonitors(&monCount);
+    for (int i = 0; i < monCount; ++i) {
+      int monXPos, monYPos, monWidth, monHeight;
+      glfwGetMonitorWorkarea(monitors[i], &monXPos, &monYPos, &monWidth,
+                             &monHeight);
+      if (gContext->xPos >= monXPos && gContext->xPos < (monXPos + monWidth) &&
+          gContext->yPos >= monYPos && gContext->yPos < (monYPos + monHeight)) {
+        found = true;
+        break;
+      }
+    }
+    if (found) {
+      glfwSetWindowPos(gContext->window, gContext->xPos, gContext->yPos);
+    }
     glfwShowWindow(gContext->window);
   }
 

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java

@@ -56,7 +56,7 @@ public abstract class CommandBase implements Sendable, Command {
    * @param name name
    * @return the decorated Command
    */
-  public Command withName(String name) {
+  public CommandBase withName(String name) {
     this.setName(name);
     return this;
   }

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java

@@ -14,7 +14,7 @@ import java.util.WeakHashMap;
  * those commands are not also used independently, which can result in inconsistent command state
  * and unpredictable execution.
  */
-public abstract class CommandGroupBase extends CommandBase implements Command {
+public abstract class CommandGroupBase extends CommandBase {
   private static final Set<Command> m_groupedCommands =
       Collections.newSetFromMap(new WeakHashMap<>());
 

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java

@@ -450,7 +450,8 @@ public final class CommandScheduler implements Sendable, AutoCloseable {
    * requiring the subsystem
    *
    * @param subsystem the subsystem to be inquired about
-   * @return the command currently requiring the subsystem
+   * @return the command currently requiring the subsystem, or null if no command is currently
+   *     scheduled
    */
   public Command requiring(Subsystem subsystem) {
     return m_requirements.get(subsystem);

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java

@@ -200,6 +200,10 @@ public class RamseteCommand extends CommandBase {
   @Override
   public void end(boolean interrupted) {
     m_timer.stop();
+
+    if (interrupted) {
+      m_output.accept(0.0, 0.0);
+    }
   }
 
   @Override

wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp

@@ -146,6 +146,14 @@ void RamseteCommand::Execute() {
 
 void RamseteCommand::End(bool interrupted) {
   m_timer.Stop();
+
+  if (interrupted) {
+    if (m_usePID) {
+      m_outputVolts(0_V, 0_V);
+    } else {
+      m_outputVel(0_mps, 0_mps);
+    }
+  }
 }
 
 bool RamseteCommand::IsFinished() {

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java

@@ -353,7 +353,7 @@ public abstract class Command implements Sendable, AutoCloseable {
    * @return the requirements (as an {@link Enumeration Enumeration} of {@link Subsystem
    *     Subsystems}) of this command
    */
-  synchronized Enumeration getRequirements() {
+  synchronized Enumeration<?> getRequirements() {
     return m_requirements.getElements();
   }
 

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java

@@ -81,7 +81,7 @@ public class CommandGroup extends Command {
     command.setParent(this);
 
     m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE));
-    for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
+    for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
       requires((Subsystem) e.nextElement());
     }
   }
@@ -119,7 +119,7 @@ public class CommandGroup extends Command {
     command.setParent(this);
 
     m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE, timeout));
-    for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
+    for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
       requires((Subsystem) e.nextElement());
     }
   }
@@ -152,7 +152,7 @@ public class CommandGroup extends Command {
     command.setParent(this);
 
     m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD));
-    for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
+    for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
       requires((Subsystem) e.nextElement());
     }
   }
@@ -193,7 +193,7 @@ public class CommandGroup extends Command {
     command.setParent(this);
 
     m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD, timeout));
-    for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
+    for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
       requires((Subsystem) e.nextElement());
     }
   }
@@ -283,7 +283,7 @@ public class CommandGroup extends Command {
       cmd.removed();
     }
 
-    Enumeration children = m_children.elements();
+    Enumeration<?> children = m_children.elements();
     while (children.hasMoreElements()) {
       Command cmd = ((Entry) children.nextElement()).m_command;
       cmd._cancel();
@@ -361,7 +361,7 @@ public class CommandGroup extends Command {
     for (int i = 0; i < m_children.size(); i++) {
       Command child = m_children.elementAt(i).m_command;
 
-      Enumeration requirements = command.getRequirements();
+      Enumeration<?> requirements = command.getRequirements();
 
       while (requirements.hasMoreElements()) {
         Object requirement = requirements.nextElement();

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java

@@ -37,13 +37,13 @@ public abstract class ConditionalCommand extends Command {
 
   private void requireAll() {
     if (m_onTrue != null) {
-      for (Enumeration e = m_onTrue.getRequirements(); e.hasMoreElements(); ) {
+      for (Enumeration<?> e = m_onTrue.getRequirements(); e.hasMoreElements(); ) {
         requires((Subsystem) e.nextElement());
       }
     }
 
     if (m_onFalse != null) {
-      for (Enumeration e = m_onFalse.getRequirements(); e.hasMoreElements(); ) {
+      for (Enumeration<?> e = m_onFalse.getRequirements(); e.hasMoreElements(); ) {
         requires((Subsystem) e.nextElement());
       }
     }

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java

@@ -144,7 +144,7 @@ public final class Scheduler implements Sendable, AutoCloseable {
     // Only add if not already in
     if (!m_commandTable.containsKey(command)) {
       // Check that the requirements can be had
-      Enumeration requirements = command.getRequirements();
+      Enumeration<?> requirements = command.getRequirements();
       while (requirements.hasMoreElements()) {
         Subsystem lock = (Subsystem) requirements.nextElement();
         if (lock.getCurrentCommand() != null && !lock.getCurrentCommand().isInterruptible()) {
@@ -210,7 +210,7 @@ public final class Scheduler implements Sendable, AutoCloseable {
     }
 
     // Call every subsystem's periodic method
-    Enumeration subsystems = m_subsystems.getElements();
+    Enumeration<?> subsystems = m_subsystems.getElements();
     while (subsystems.hasMoreElements()) {
       ((Subsystem) subsystems.nextElement()).periodic();
     }
@@ -233,7 +233,7 @@ public final class Scheduler implements Sendable, AutoCloseable {
     m_additions.removeAllElements();
 
     // Add in the defaults
-    Enumeration locks = m_subsystems.getElements();
+    Enumeration<?> locks = m_subsystems.getElements();
     while (locks.hasMoreElements()) {
       Subsystem lock = (Subsystem) locks.nextElement();
       if (lock.getCurrentCommand() == null) {
@@ -276,7 +276,7 @@ public final class Scheduler implements Sendable, AutoCloseable {
     }
     element.remove();
 
-    Enumeration requirements = command.getRequirements();
+    Enumeration<?> requirements = command.getRequirements();
     while (requirements.hasMoreElements()) {
       ((Subsystem) requirements.nextElement()).setCurrentCommand(null);
     }

wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java

@@ -30,10 +30,9 @@ public final class MockHardwareExtension implements BeforeAllCallback {
 
   private void initializeHardware() {
     HAL.initialize(500, 0);
-    DriverStationSim dsSim = new DriverStationSim();
-    dsSim.setDsAttached(true);
-    dsSim.setAutonomous(false);
-    dsSim.setEnabled(true);
-    dsSim.setTest(true);
+    DriverStationSim.setDsAttached(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setTest(true);
   }
 }

wpilibc/publish.gradle

@@ -34,8 +34,14 @@ task cppHeadersZip(type: Zip) {
         into '/'
     }
 
-    from('src/main/native/include') {
-        into '/'
+    ext.includeDirs = [
+        project.file('src/main/native/include')
+    ]
+
+    ext.includeDirs.each {
+        from(it) {
+            into '/'
+        }
     }
 }
 

wpilibc/src/main/native/cpp/AnalogEncoder.cpp

@@ -12,6 +12,9 @@
 
 using namespace frc;
 
+AnalogEncoder::AnalogEncoder(int channel)
+    : AnalogEncoder(std::make_shared<AnalogInput>(channel)) {}
+
 AnalogEncoder::AnalogEncoder(AnalogInput& analogInput)
     : m_analogInput{&analogInput, NullDeleter<AnalogInput>{}},
       m_analogTrigger{m_analogInput.get()},

wpilibc/src/main/native/cpp/Compressor.cpp

@@ -236,14 +236,10 @@ int Compressor::GetModule() const {
 void Compressor::InitSendable(SendableBuilder& builder) {
   builder.SetSmartDashboardType("Compressor");
   builder.AddBooleanProperty(
-      "Enabled", [=]() { return Enabled(); },
-      [=](bool value) {
-        if (value) {
-          Start();
-        } else {
-          Stop();
-        }
-      });
+      "Closed Loop Control", [=]() { return GetClosedLoopControl(); },
+      [=](bool value) { SetClosedLoopControl(value); });
+  builder.AddBooleanProperty(
+      "Enabled", [=] { return Enabled(); }, nullptr);
   builder.AddBooleanProperty(
       "Pressure switch", [=]() { return GetPressureSwitchValue(); }, nullptr);
 }

wpilibc/src/main/native/cpp/DoubleSolenoid.cpp

@@ -148,6 +148,14 @@ void DoubleSolenoid::Toggle() {
   }
 }
 
+int DoubleSolenoid::GetFwdChannel() const {
+  return m_forwardChannel;
+}
+
+int DoubleSolenoid::GetRevChannel() const {
+  return m_reverseChannel;
+}
+
 bool DoubleSolenoid::IsFwdSolenoidBlackListed() const {
   int blackList = GetPCMSolenoidBlackList(m_moduleNumber);
   return (blackList & m_forwardMask) != 0;

wpilibc/src/main/native/cpp/Notifier.cpp

@@ -200,3 +200,8 @@ void Notifier::UpdateAlarm(uint64_t triggerTime) {
 void Notifier::UpdateAlarm() {
   UpdateAlarm(static_cast<uint64_t>(m_expirationTime * 1e6));
 }
+
+bool Notifier::SetHALThreadPriority(bool realTime, int32_t priority) {
+  int32_t status = 0;
+  return HAL_SetNotifierThreadPriority(realTime, priority, &status);
+}

wpilibc/src/main/native/cpp/RobotController.cpp

@@ -42,6 +42,13 @@ bool RobotController::GetUserButton() {
   return value;
 }
 
+units::volt_t RobotController::GetBatteryVoltage() {
+  int32_t status = 0;
+  double retVal = HAL_GetVinVoltage(&status);
+  wpi_setGlobalHALError(status);
+  return units::volt_t{retVal};
+}
+
 bool RobotController::IsSysActive() {
   int32_t status = 0;
   bool retVal = HAL_GetSystemActive(&status);

wpilibc/src/main/native/cpp/Solenoid.cpp

@@ -79,6 +79,10 @@ void Solenoid::Toggle() {
   Set(!Get());
 }
 
+int Solenoid::GetChannel() const {
+  return m_channel;
+}
+
 bool Solenoid::IsBlackListed() const {
   int value = GetPCMSolenoidBlackList(m_moduleNumber) & (1 << m_channel);
   return (value != 0);

wpilibc/src/main/native/cpp/SolenoidBase.cpp

@@ -58,3 +58,7 @@ void SolenoidBase::ClearAllPCMStickyFaults() {
 }
 
 SolenoidBase::SolenoidBase(int moduleNumber) : m_moduleNumber(moduleNumber) {}
+
+int SolenoidBase::GetModuleNumber() const {
+  return m_moduleNumber;
+}

wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp

@@ -19,7 +19,7 @@ HolonomicDriveController::HolonomicDriveController(
 
 bool HolonomicDriveController::AtReference() const {
   const auto& eTranslate = m_poseError.Translation();
-  const auto& eRotate = m_poseError.Rotation();
+  const auto& eRotate = m_rotationError;
   const auto& tolTranslate = m_poseTolerance.Translation();
   const auto& tolRotate = m_poseTolerance.Rotation();
   return units::math::abs(eTranslate.X()) < tolTranslate.X() &&
@@ -34,6 +34,13 @@ void HolonomicDriveController::SetTolerance(const Pose2d& tolerance) {
 ChassisSpeeds HolonomicDriveController::Calculate(
     const Pose2d& currentPose, const Pose2d& poseRef,
     units::meters_per_second_t linearVelocityRef, const Rotation2d& angleRef) {
+  // If this is the first run, then we need to reset the theta controller to the
+  // current pose's heading.
+  if (m_firstRun) {
+    m_thetaController.Reset(currentPose.Rotation().Radians());
+    m_firstRun = false;
+  }
+
   // Calculate feedforward velocities (field-relative)
   auto xFF = linearVelocityRef * poseRef.Rotation().Cos();
   auto yFF = linearVelocityRef * poseRef.Rotation().Sin();
@@ -41,6 +48,7 @@ ChassisSpeeds HolonomicDriveController::Calculate(
       currentPose.Rotation().Radians(), angleRef.Radians()));
 
   m_poseError = poseRef.RelativeTo(currentPose);
+  m_rotationError = angleRef - currentPose.Rotation();
 
   if (!m_enabled) {
     return ChassisSpeeds::FromFieldRelativeSpeeds(xFF, yFF, thetaFF,

wpilibc/src/main/native/cpp/controller/PIDController.cpp

@@ -9,6 +9,7 @@
 
 #include <hal/FRCUsageReporting.h>
 
+#include "frc/DriverStation.h"
 #include "frc/MathUtil.h"
 #include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableRegistry.h"
@@ -18,6 +19,12 @@ using namespace frc2;
 PIDController::PIDController(double Kp, double Ki, double Kd,
                              units::second_t period)
     : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
+  if (period <= 0_s) {
+    frc::DriverStation::ReportError(
+        "Controller period must be a non-zero positive number!");
+    m_period = 20_ms;
+    frc::DriverStation::ReportWarning("Controller period defaulted to 20ms.");
+  }
   static int instances = 0;
   instances++;
   HAL_Report(HALUsageReporting::kResourceType_PIDController2, instances);

wpilibc/src/main/native/cpp/simulation/DoubleSolenoidSim.cpp

@@ -0,0 +1,91 @@
+// 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.
+
+#include "frc/simulation/DoubleSolenoidSim.h"
+
+#include "frc/SensorUtil.h"
+#include "frc/simulation/PCMSim.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DoubleSolenoidSim::DoubleSolenoidSim(int fwd, int rev)
+    : m_fwd{fwd}, m_rev{rev} {}
+
+DoubleSolenoidSim::DoubleSolenoidSim(int module, int fwd, int rev)
+    : m_pcm{module}, m_fwd{fwd}, m_rev{rev} {}
+
+DoubleSolenoidSim::DoubleSolenoidSim(PCMSim& pcm, int fwd, int rev)
+    : m_pcm{pcm}, m_fwd{fwd}, m_rev{rev} {}
+
+DoubleSolenoidSim::DoubleSolenoidSim(DoubleSolenoid& solenoid)
+    : m_pcm{solenoid.GetModuleNumber()},
+      m_fwd{solenoid.GetFwdChannel()},
+      m_rev{solenoid.GetRevChannel()} {}
+
+std::unique_ptr<CallbackStore>
+DoubleSolenoidSim::RegisterFwdInitializedCallback(NotifyCallback callback,
+                                                  bool initialNotify) {
+  return m_pcm.RegisterSolenoidInitializedCallback(m_fwd, callback,
+                                                   initialNotify);
+}
+
+bool DoubleSolenoidSim::GetFwdInitialized() const {
+  return m_pcm.GetSolenoidInitialized(m_fwd);
+}
+
+void DoubleSolenoidSim::SetFwdInitialized(bool initialized) {
+  m_pcm.SetSolenoidInitialized(m_fwd, initialized);
+}
+
+std::unique_ptr<CallbackStore>
+DoubleSolenoidSim::RegisterRevInitializedCallback(NotifyCallback callback,
+                                                  bool initialNotify) {
+  return m_pcm.RegisterSolenoidInitializedCallback(m_rev, callback,
+                                                   initialNotify);
+}
+
+bool DoubleSolenoidSim::GetRevInitialized() const {
+  return m_pcm.GetSolenoidInitialized(m_rev);
+}
+
+void DoubleSolenoidSim::SetRevInitialized(bool initialized) {
+  m_pcm.SetSolenoidInitialized(m_rev, initialized);
+}
+
+void DoubleSolenoidSim::Set(DoubleSolenoid::Value value) {
+  bool forward = false;
+  bool reverse = false;
+
+  switch (value) {
+    case DoubleSolenoid::Value::kOff:
+      forward = false;
+      reverse = false;
+      break;
+    case DoubleSolenoid::Value::kForward:
+      forward = true;
+      reverse = false;
+      break;
+    case DoubleSolenoid::Value::kReverse:
+      forward = false;
+      reverse = true;
+      break;
+  }
+
+  m_pcm.SetSolenoidOutput(m_fwd, forward);
+  m_pcm.SetSolenoidOutput(m_rev, reverse);
+}
+
+DoubleSolenoid::Value DoubleSolenoidSim::Get() const {
+  bool valueForward = m_pcm.GetSolenoidOutput(m_fwd);
+  bool valueReverse = m_pcm.GetSolenoidOutput(m_rev);
+
+  if (valueForward) {
+    return DoubleSolenoid::Value::kForward;
+  } else if (valueReverse) {
+    return DoubleSolenoid::Value::kReverse;
+  } else {
+    return DoubleSolenoid::Value::kOff;
+  }
+}

wpilibc/src/main/native/cpp/simulation/SimDeviceSim.cpp

@@ -9,6 +9,8 @@
 
 #include <hal/SimDevice.h>
 #include <hal/simulation/SimDeviceData.h>
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
 
 using namespace frc;
 using namespace frc::sim;
@@ -16,6 +18,22 @@ using namespace frc::sim;
 SimDeviceSim::SimDeviceSim(const char* name)
     : m_handle{HALSIM_GetSimDeviceHandle(name)} {}
 
+SimDeviceSim::SimDeviceSim(const char* name, int index) {
+  wpi::SmallString<128> fullname;
+  wpi::raw_svector_ostream os(fullname);
+  os << name << '[' << index << ']';
+
+  m_handle = HALSIM_GetSimDeviceHandle(fullname.c_str());
+}
+
+SimDeviceSim::SimDeviceSim(const char* name, int index, int channel) {
+  wpi::SmallString<128> fullname;
+  wpi::raw_svector_ostream os(fullname);
+  os << name << '[' << index << ',' << channel << ']';
+
+  m_handle = HALSIM_GetSimDeviceHandle(fullname.c_str());
+}
+
 hal::SimValue SimDeviceSim::GetValue(const char* name) const {
   return HALSIM_GetSimValueHandle(m_handle, name);
 }

wpilibc/src/main/native/cpp/simulation/SolenoidSim.cpp

@@ -0,0 +1,50 @@
+// 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.
+
+#include "frc/simulation/SolenoidSim.h"
+
+#include "frc/SensorUtil.h"
+#include "frc/simulation/PCMSim.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+SolenoidSim::SolenoidSim(int channel) : m_channel{channel} {}
+
+SolenoidSim::SolenoidSim(int module, int channel)
+    : m_pcm{module}, m_channel{channel} {}
+
+SolenoidSim::SolenoidSim(PCMSim& pcm, int channel)
+    : m_pcm{pcm}, m_channel{channel} {}
+
+SolenoidSim::SolenoidSim(Solenoid& solenoid)
+    : m_pcm{solenoid.GetModuleNumber()}, m_channel{solenoid.GetChannel()} {}
+
+std::unique_ptr<CallbackStore> SolenoidSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  return m_pcm.RegisterSolenoidInitializedCallback(m_channel, callback,
+                                                   initialNotify);
+}
+
+bool SolenoidSim::GetInitialized() const {
+  return m_pcm.GetSolenoidInitialized(m_channel);
+}
+
+void SolenoidSim::SetInitialized(bool initialized) {
+  m_pcm.SetSolenoidInitialized(m_channel, initialized);
+}
+
+std::unique_ptr<CallbackStore> SolenoidSim::RegisterOutputCallback(
+    NotifyCallback callback, bool initialNotify) {
+  return m_pcm.RegisterSolenoidOutputCallback(m_channel, callback,
+                                              initialNotify);
+}
+
+bool SolenoidSim::GetOutput() const {
+  return m_pcm.GetSolenoidOutput(m_channel);
+}
+
+void SolenoidSim::SetOutput(bool output) {
+  m_pcm.SetSolenoidOutput(m_channel, output);
+}

wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp

@@ -4,6 +4,13 @@
 
 #include "frc/smartdashboard/FieldObject2d.h"
 
+#include <vector>
+
+#include <wpi/Endian.h>
+#include <wpi/MathExtras.h>
+
+#include "frc/trajectory/Trajectory.h"
+
 using namespace frc;
 
 FieldObject2d::FieldObject2d(FieldObject2d&& rhs) {
@@ -48,6 +55,16 @@ void FieldObject2d::SetPoses(std::initializer_list<Pose2d> poses) {
   SetPoses(wpi::makeArrayRef(poses.begin(), poses.end()));
 }
 
+void FieldObject2d::SetTrajectory(const Trajectory& trajectory) {
+  std::scoped_lock lock(m_mutex);
+  m_poses.clear();
+  m_poses.reserve(trajectory.States().size());
+  for (auto&& state : trajectory.States()) {
+    m_poses.push_back(state.pose);
+  }
+  UpdateEntry();
+}
+
 std::vector<Pose2d> FieldObject2d::GetPoses() const {
   std::scoped_lock lock(m_mutex);
   UpdateFromEntry();
@@ -66,17 +83,41 @@ void FieldObject2d::UpdateEntry(bool setDefault) {
   if (!m_entry) {
     return;
   }
-  wpi::SmallVector<double, 9> arr;
-  for (auto&& pose : m_poses) {
-    auto& translation = pose.Translation();
-    arr.push_back(translation.X().to<double>());
-    arr.push_back(translation.Y().to<double>());
-    arr.push_back(pose.Rotation().Degrees().to<double>());
-  }
-  if (setDefault) {
-    m_entry.SetDefaultDoubleArray(arr);
+  if (m_poses.size() < (255 / 3)) {
+    wpi::SmallVector<double, 9> arr;
+    for (auto&& pose : m_poses) {
+      auto& translation = pose.Translation();
+      arr.push_back(translation.X().to<double>());
+      arr.push_back(translation.Y().to<double>());
+      arr.push_back(pose.Rotation().Degrees().to<double>());
+    }
+    if (setDefault) {
+      m_entry.SetDefaultDoubleArray(arr);
+    } else {
+      m_entry.ForceSetDoubleArray(arr);
+    }
   } else {
-    m_entry.SetDoubleArray(arr);
+    // send as raw array of doubles if too big for NT array
+    std::vector<char> arr;
+    arr.resize(m_poses.size() * 3 * 8);
+    char* p = arr.data();
+    for (auto&& pose : m_poses) {
+      auto& translation = pose.Translation();
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(translation.X().to<double>()));
+      p += 8;
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(translation.Y().to<double>()));
+      p += 8;
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(pose.Rotation().Degrees().to<double>()));
+      p += 8;
+    }
+    if (setDefault) {
+      m_entry.SetDefaultRaw(wpi::StringRef{arr.data(), arr.size()});
+    } else {
+      m_entry.ForceSetRaw(wpi::StringRef{arr.data(), arr.size()});
+    }
   }
 }
 
@@ -85,18 +126,45 @@ void FieldObject2d::UpdateFromEntry() const {
     return;
   }
   auto val = m_entry.GetValue();
-  if (!val || !val->IsDoubleArray()) {
+  if (!val) {
     return;
   }
-  auto arr = val->GetDoubleArray();
-  auto size = arr.size();
-  if ((size % 3) != 0) {
-    return;
-  }
-  m_poses.resize(size / 3);
-  for (size_t i = 0; i < size / 3; ++i) {
-    m_poses[i] =
-        Pose2d{units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
-               Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
+
+  if (val->IsDoubleArray()) {
+    auto arr = val->GetDoubleArray();
+    auto size = arr.size();
+    if ((size % 3) != 0) {
+      return;
+    }
+    m_poses.resize(size / 3);
+    for (size_t i = 0; i < size / 3; ++i) {
+      m_poses[i] =
+          Pose2d{units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
+                 Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
+    }
+  } else if (val->IsRaw()) {
+    // treat it simply as an array of doubles
+    wpi::StringRef data = val->GetRaw();
+
+    // must be triples of doubles
+    auto size = data.size();
+    if ((size % (3 * 8)) != 0) {
+      return;
+    }
+    m_poses.resize(size / (3 * 8));
+    const char* p = data.begin();
+    for (size_t i = 0; i < size / (3 * 8); ++i) {
+      double x = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      double y = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      double rot = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      m_poses[i] = Pose2d{units::meter_t{x}, units::meter_t{y},
+                          Rotation2d{units::degree_t{rot}}};
+    }
   }
 }

wpilibc/src/main/native/include/frc/AnalogEncoder.h

@@ -26,6 +26,13 @@ class AnalogEncoder : public ErrorBase,
                       public Sendable,
                       public SendableHelper<AnalogEncoder> {
  public:
+  /**
+   * Construct a new AnalogEncoder attached to a specific AnalogIn channel.
+   *
+   * @param channel the analog input channel to attach to
+   */
+  explicit AnalogEncoder(int channel);
+
   /**
    * Construct a new AnalogEncoder attached to a specific AnalogInput.
    *

wpilibc/src/main/native/include/frc/DoubleSolenoid.h

@@ -74,6 +74,20 @@ class DoubleSolenoid : public SolenoidBase,
    */
   void Toggle();
 
+  /**
+   * Get the forward channel.
+   *
+   * @return the forward channel.
+   */
+  int GetFwdChannel() const;
+
+  /**
+   * Get the reverse channel.
+   *
+   * @return the reverse channel.
+   */
+  int GetRevChannel() const;
+
   /**
    * Check if the forward solenoid is blacklisted.
    *

wpilibc/src/main/native/include/frc/Notifier.h

@@ -143,6 +143,23 @@ class Notifier : public ErrorBase {
    */
   void Stop();
 
+  /**
+   * Sets the HAL notifier thread priority.
+   *
+   * The HAL notifier thread is responsible for managing the FPGA's notifier
+   * interrupt and waking up user's Notifiers when it's their time to run.
+   * Giving the HAL notifier thread real-time priority helps ensure the user's
+   * real-time Notifiers, if any, are notified to run in a timely manner.
+   *
+   * @param realTime Set to true to set a real-time priority, false for standard
+   *                 priority.
+   * @param priority Priority to set the thread to. For real-time, this is 1-99
+   *                 with 99 being highest. For non-real-time, this is forced to
+   *                 0. See "man 7 sched" for more details.
+   * @return         True on success.
+   */
+  static bool SetHALThreadPriority(bool realTime, int32_t priority);
+
  private:
   /**
    * Update the HAL alarm time.

wpilibc/src/main/native/include/frc/RobotController.h

@@ -6,6 +6,8 @@
 
 #include <stdint.h>
 
+#include <units/voltage.h>
+
 namespace frc {
 
 struct CANStatus {
@@ -55,6 +57,13 @@ class RobotController {
    */
   static bool GetUserButton();
 
+  /**
+   * Read the battery voltage.
+   *
+   * @return The battery voltage in Volts.
+   */
+  static units::volt_t GetBatteryVoltage();
+
   /**
    * Check if the FPGA outputs are enabled.
    *

wpilibc/src/main/native/include/frc/Solenoid.h

@@ -66,6 +66,11 @@ class Solenoid : public SolenoidBase,
    */
   void Toggle();
 
+  /**
+   * Get the channel this solenoid is connected to.
+   */
+  int GetChannel() const;
+
   /**
    * Check if solenoid is blacklisted.
    *

wpilibc/src/main/native/include/frc/SolenoidBase.h

@@ -14,6 +14,13 @@ namespace frc {
  */
 class SolenoidBase : public ErrorBase {
  public:
+  /**
+   * Get the CAN ID of the module this solenoid is connected to.
+   *
+   * @return the module number.
+   */
+  int GetModuleNumber() const;
+
   /**
    * Read all 8 solenoids as a single byte
    *

wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h

@@ -98,11 +98,14 @@ class HolonomicDriveController {
 
  private:
   Pose2d m_poseError;
+  Rotation2d m_rotationError;
   Pose2d m_poseTolerance;
   bool m_enabled = true;
 
   frc2::PIDController m_xController;
   frc2::PIDController m_yController;
   ProfiledPIDController<units::radian> m_thetaController;
+
+  bool m_firstRun = true;
 };
 }  // namespace frc

wpilibc/src/main/native/include/frc/controller/PIDController.h

@@ -27,7 +27,7 @@ class PIDController : public frc::Sendable,
    * @param Ki     The integral coefficient.
    * @param Kd     The derivative coefficient.
    * @param period The period between controller updates in seconds. The
-   *               default is 20 milliseconds.
+   *               default is 20 milliseconds. Must be non-zero and positive.
    */
   PIDController(double Kp, double Ki, double Kd,
                 units::second_t period = 20_ms);

wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h

@@ -257,7 +257,7 @@ class DifferentialDrivetrainSim {
                                                 * (26_in / 2) * (26_in / 2);
 
     return DifferentialDrivetrainSim{
-        motor,           gearing, batteryMoi + gearboxMoi, 25_kg,
+        motor,           gearing, batteryMoi + gearboxMoi, 60_lb,
         wheelSize / 2.0, 26_in,   measurementStdDevs};
   }
 
@@ -281,7 +281,7 @@ class DifferentialDrivetrainSim {
       units::kilogram_square_meter_t J,
       const std::array<double, 7>& measurementStdDevs = {}) {
     return DifferentialDrivetrainSim{
-        motor, gearing, J, 25_kg, wheelSize / 2.0, 26_in, measurementStdDevs};
+        motor, gearing, J, 60_lb, wheelSize / 2.0, 26_in, measurementStdDevs};
   }
 
  private:

wpilibc/src/main/native/include/frc/simulation/DoubleSolenoidSim.h

@@ -0,0 +1,116 @@
+// 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.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/DoubleSolenoid.h"
+#include "frc/simulation/CallbackStore.h"
+#include "frc/simulation/PCMSim.h"
+
+namespace frc::sim {
+
+/**
+ * Class to control a simulated Pneumatic Control Module (PCM).
+ */
+class DoubleSolenoidSim {
+ public:
+  /**
+   * Constructs for a solenoid on the default PCM.
+   *
+   * @param channel the solenoid channel.
+   */
+  DoubleSolenoidSim(int fwd, int rev);
+
+  /**
+   * Constructs for a solenoid on the given PCM.
+   *
+   * @param pcm the PCM the solenoid is connected to.
+   * @param channel the solenoid channel.
+   */
+  DoubleSolenoidSim(int module, int fwd, int rev);
+
+  /**
+   * Constructs from a PCMSim object.
+   *
+   * @param pcm the PCM the solenoid is connected to.
+   */
+  DoubleSolenoidSim(PCMSim& pcm, int fwd, int rev);
+
+  /**
+   * Constructs for the given solenoid.
+   *
+   * @param solenoid the solenoid to simulate.
+   */
+  explicit DoubleSolenoidSim(DoubleSolenoid& solenoid);
+
+  /**
+   * Register a callback to be run when the forward solenoid is initialized.
+   *
+   * @param callback      the callback
+   * @param initialNotify should the callback be run with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterFwdInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if the forward solenoid has been initialized.
+   *
+   * @return true if initialized
+   */
+  bool GetFwdInitialized() const;
+
+  /**
+   * Define whether the forward solenoid has been initialized.
+   *
+   * @param initialized whether the solenoid is intiialized.
+   */
+  void SetFwdInitialized(bool initialized);
+
+  /**
+   * Register a callback to be run when the reverse solenoid is initialized.
+   *
+   * @param callback      the callback
+   * @param initialNotify should the callback be run with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterRevInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Define whether the reverse solenoid has been initialized.
+   *
+   * @param initialized whether the solenoid is intiialized.
+   */
+  void SetRevInitialized(bool initialized);
+
+  /**
+   * Check if the reverse solenoid has been initialized.
+   *
+   * @return true if initialized
+   */
+  bool GetRevInitialized() const;
+
+  /**
+   * Set the value of the double solenoid output.
+   *
+   * @param value The value to set (Off, Forward, Reverse)
+   */
+  void Set(DoubleSolenoid::Value value);
+
+  /**
+   * Check the value of the double solenoid output.
+   *
+   * @return the output value of the double solenoid.
+   */
+  DoubleSolenoid::Value Get() const;
+
+ private:
+  PCMSim m_pcm;
+  int m_fwd;
+  int m_rev;
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h

@@ -18,7 +18,7 @@ namespace frc::sim {
 class FlywheelSim : public LinearSystemSim<1, 1, 1> {
  public:
   /**
-   * Creates a simulated flywhel mechanism.
+   * Creates a simulated flywheel mechanism.
    *
    * @param plant              The linear system representing the flywheel.
    * @param gearbox            The type of and number of motors in the flywheel
@@ -32,7 +32,7 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> {
               const std::array<double, 1>& measurementStdDevs = {0.0});
 
   /**
-   * Creates a simulated flywhel mechanism.
+   * Creates a simulated flywheel mechanism.
    *
    * @param gearbox            The type of and number of motors in the flywheel
    *                           gearbox.

wpilibc/src/main/native/include/frc/simulation/SimDeviceSim.h

@@ -25,6 +25,23 @@ class SimDeviceSim {
    */
   explicit SimDeviceSim(const char* name);
 
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   * @param index device index number to append to name
+   */
+  SimDeviceSim(const char* name, int index);
+
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   * @param index device index number to append to name
+   * @param channel device channel number to append to name
+   */
+  SimDeviceSim(const char* name, int index, int channel);
+
   /**
    * Get the property object with the given name.
    *

wpilibc/src/main/native/include/frc/simulation/SolenoidSim.h

@@ -0,0 +1,104 @@
+// 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.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/Solenoid.h"
+#include "frc/simulation/CallbackStore.h"
+#include "frc/simulation/PCMSim.h"
+
+namespace frc::sim {
+
+/**
+ * Class to control a simulated Pneumatic Control Module (PCM).
+ */
+class SolenoidSim {
+ public:
+  /**
+   * Constructs for a solenoid on the default PCM.
+   *
+   * @param channel the solenoid channel.
+   */
+  explicit SolenoidSim(int channel);
+
+  /**
+   * Constructs for the given solenoid.
+   *
+   * @param doubleSolenoid the solenoid to simulate.
+   */
+  explicit SolenoidSim(Solenoid& solenoid);
+
+  /**
+   * Constructs for a solenoid.
+   *
+   * @param module the CAN ID of the PCM the solenoid is connected to.
+   * @param channel the solenoid channel.
+   *
+   * @see PCMSim#PCMSim(int)
+   */
+  SolenoidSim(int module, int channel);
+
+  /**
+   * Constructs for a solenoid on the given PCM.
+   *
+   * @param pcm the PCM the solenoid is connected to.
+   * @param channel the solenoid channel.
+   */
+  SolenoidSim(PCMSim& pcm, int channel);
+
+  /**
+   * Register a callback to be run when this solenoid is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial state
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if this solenoid has been initialized.
+   *
+   * @return true if initialized
+   */
+  bool GetInitialized() const;
+
+  /**
+   * Define whether this solenoid has been initialized.
+   *
+   * @param initialized whether the solenoid is intiialized.
+   */
+  void SetInitialized(bool initialized);
+
+  /**
+   * Register a callback to be run when the output of this solenoid has changed.
+   *
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial value
+   * @return the {@link CallbackStore} object associated with this callback.
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterOutputCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check the solenoid output.
+   *
+   * @return the solenoid output
+   */
+  bool GetOutput() const;
+
+  /**
+   * Change the solenoid output.
+   *
+   * @param output the new solenoid output
+   */
+  void SetOutput(bool output);
+
+ private:
+  PCMSim m_pcm;
+  int m_channel;
+};
+}  // namespace frc::sim

wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h

@@ -21,6 +21,7 @@
 namespace frc {
 
 class Field2d;
+class Trajectory;
 
 /**
  * Game field object on a Field2d.
@@ -76,6 +77,13 @@ class FieldObject2d {
    */
   void SetPoses(std::initializer_list<Pose2d> poses);
 
+  /**
+   * Sets poses from a trajectory.
+   *
+   * @param trajectory The trajectory from which poses should be added.
+   */
+  void SetTrajectory(const Trajectory& trajectory);
+
   /**
    * Get multiple poses.
    *

wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp

@@ -47,3 +47,17 @@ TEST(HolonomicDriveControllerTest, ReachesReference) {
   EXPECT_NEAR_UNITS(frc::AngleModulus(robotPose.Rotation().Radians()), 0_rad,
                     kAngularTolerance);
 }
+
+TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
+  frc::HolonomicDriveController controller{
+      frc2::PIDController{1, 0, 0}, frc2::PIDController{1, 0, 0},
+      frc::ProfiledPIDController<units::radian>{
+          1, 0, 0,
+          frc::TrapezoidProfile<units::radian>::Constraints{
+              4_rad_per_s, 2_rad_per_s / 1_s}}};
+
+  frc::ChassisSpeeds speeds = controller.Calculate(
+      frc::Pose2d(0_m, 0_m, 1.57_rad), frc::Pose2d(), 0_mps, 1.57_rad);
+
+  EXPECT_EQ(0, speeds.omega.to<double>());
+}

wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp

@@ -3,7 +3,7 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
+#include <frc/PWMSparkMax.h>
 #include <frc/TimedRobot.h>
 #include <frc/drive/DifferentialDrive.h>
 
@@ -12,8 +12,8 @@
  * Runs the motors with arcade steering.
  */
 class Robot : public frc::TimedRobot {
-  frc::PWMVictorSPX m_leftMotor{0};
-  frc::PWMVictorSPX m_rightMotor{1};
+  frc::PWMSparkMax m_leftMotor{0};
+  frc::PWMSparkMax m_rightMotor{1};
   frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
   frc::Joystick m_stick{0};
 

wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp

@@ -3,7 +3,7 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <frc/GenericHID.h>
-#include <frc/PWMVictorSPX.h>
+#include <frc/PWMSparkMax.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 #include <frc/drive/DifferentialDrive.h>
@@ -13,8 +13,8 @@
  * Runs the motors with split arcade steering and an Xbox controller.
  */
 class Robot : public frc::TimedRobot {
-  frc::PWMVictorSPX m_leftMotor{0};
-  frc::PWMVictorSPX m_rightMotor{1};
+  frc::PWMSparkMax m_leftMotor{0};
+  frc::PWMSparkMax m_rightMotor{1};
   frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
   frc::XboxController m_driverController{0};
 

wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h

@@ -5,7 +5,7 @@
 #pragma once
 
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
+#include <frc/PWMSparkMax.h>
 #include <frc/controller/ArmFeedforward.h>
 #include <frc2/command/ProfiledPIDSubsystem.h>
 #include <units/angle.h>
@@ -24,7 +24,7 @@ class ArmSubsystem : public frc2::ProfiledPIDSubsystem<units::radians> {
   units::radian_t GetMeasurement() override;
 
  private:
-  frc::PWMVictorSPX m_motor;
+  frc::PWMSparkMax m_motor;
   frc::Encoder m_encoder;
   frc::ArmFeedforward m_feedforward;
 };