[ntcore] Change Java event mask to EnumSet (#4564)

Also convert NetworkTableInstance.getNetworkMode() to return EnumSet.

.github/workflows/upstream-utils.yml

@@ -49,6 +49,10 @@ jobs:
         run: |
           cd upstream_utils
           ./update_llvm.py
+      - name: Run update_mpack.py
+        run: |
+          cd upstream_utils
+          ./update_mpack.py
       - name: Run update_stack_walker.py
         run: |
           cd upstream_utils

buildSrc/build.gradle

@@ -5,5 +5,5 @@ repositories {
     }
 }
 dependencies {
-    implementation "edu.wpi.first:native-utils:2023.4.0"
+    implementation "edu.wpi.first:native-utils:2023.6.0"
 }

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

@@ -25,10 +25,12 @@
 static_assert(sizeof(int32_t) >= sizeof(int),
               "FRC_NetworkComm status variable is larger than 32 bits");
 
+namespace {
 struct HAL_JoystickAxesInt {
   int16_t count;
   int16_t axes[HAL_kMaxJoystickAxes];
 };
+}  // namespace
 
 namespace {
 struct JoystickDataCache {
@@ -57,10 +59,11 @@ static wpi::mutex msgMutex;
 
 static int32_t HAL_GetJoystickAxesInternal(int32_t joystickNum,
                                            HAL_JoystickAxes* axes) {
-  JoystickAxes_t netcommAxes;
+  HAL_JoystickAxesInt netcommAxes;
 
   int retVal = FRC_NetworkCommunication_getJoystickAxes(
-      joystickNum, &netcommAxes, HAL_kMaxJoystickAxes);
+      joystickNum, reinterpret_cast<JoystickAxes_t*>(&netcommAxes),
+      HAL_kMaxJoystickAxes);
 
   // copy integer values to double values
   axes->count = netcommAxes.count;

ntcore/src/generate/java/NetworkTableInstance.java.jinja

@@ -7,6 +7,7 @@ package edu.wpi.first.networktables;
 import edu.wpi.first.util.WPIUtilJNI;
 import edu.wpi.first.util.concurrent.Event;
 import edu.wpi.first.util.datalog.DataLog;
+import java.util.EnumSet;
 import java.util.HashMap;
 import java.util.Map;
 import java.util.concurrent.ConcurrentHashMap;
@@ -35,16 +36,33 @@ import java.util.function.Consumer;
  */
 @SuppressWarnings("PMD.CouplingBetweenObjects")
 public final class NetworkTableInstance implements AutoCloseable {
-  /**
-   * Client/server mode flag values (as returned by {@link #getNetworkMode()}). This is a bitmask.
-   */
-  public static final int kNetModeNone = 0x00;
+  /** Client/server mode flag values (as returned by {@link #getNetworkMode()}). */
+  public enum NetworkMode {
+    /** Running in server mode. */
+    kServer(0x01),
 
-  public static final int kNetModeServer = 0x01;
-  public static final int kNetModeClient3 = 0x02;
-  public static final int kNetModeClient4 = 0x04;
-  public static final int kNetModeStarting = 0x08;
-  public static final int kNetModeLocal = 0x10;
+    /** Running in NT3 client mode. */
+    kClient3(0x02),
+
+    /** Running in NT4 client mode. */
+    kClient4(0x04),
+
+    /** Currently starting up (either client or server). */
+    kStarting(0x08),
+
+    /** Running in local-only mode. */
+    kLocal(0x10);
+
+    private final int value;
+
+    NetworkMode(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+  }
 
   /** The default port that network tables operates on for NT3. */
   public static final int kDefaultPort3 = 1735;
@@ -68,6 +86,7 @@ public final class NetworkTableInstance implements AutoCloseable {
     if (m_owned && m_handle != 0) {
       m_listeners.close();
       NetworkTablesJNI.destroyInstance(m_handle);
+      m_handle = 0;
     }
   }
 
@@ -369,14 +388,17 @@ public final class NetworkTableInstance implements AutoCloseable {
       m_inst = inst;
     }
 
-    int add(String[] prefixes, int mask, Consumer<NetworkTableEvent> listener) {
+    int add(
+        String[] prefixes,
+        EnumSet<NetworkTableEvent.Kind> eventKinds,
+        Consumer<NetworkTableEvent> listener) {
       m_lock.lock();
       try {
         if (m_poller == 0) {
           m_poller = NetworkTablesJNI.createListenerPoller(m_inst.getHandle());
           startThread();
         }
-        int h = NetworkTablesJNI.addListener(m_poller, prefixes, mask);
+        int h = NetworkTablesJNI.addListener(m_poller, prefixes, eventKinds);
         m_listeners.put(h, listener);
         return h;
       } finally {
@@ -384,14 +406,17 @@ public final class NetworkTableInstance implements AutoCloseable {
       }
     }
 
-    int add(int handle, int mask, Consumer<NetworkTableEvent> listener) {
+    int add(
+        int handle,
+        EnumSet<NetworkTableEvent.Kind> eventKinds,
+        Consumer<NetworkTableEvent> listener) {
       m_lock.lock();
       try {
         if (m_poller == 0) {
           m_poller = NetworkTablesJNI.createListenerPoller(m_inst.getHandle());
           startThread();
         }
-        int h = NetworkTablesJNI.addListener(m_poller, handle, mask);
+        int h = NetworkTablesJNI.addListener(m_poller, handle, eventKinds);
         m_listeners.put(h, listener);
         return h;
       } finally {
@@ -562,9 +587,11 @@ public final class NetworkTableInstance implements AutoCloseable {
    */
   public int addConnectionListener(
       boolean immediateNotify, Consumer<NetworkTableEvent> listener) {
-    return m_listeners.add(m_handle,
-        NetworkTableEvent.kConnection | (immediateNotify ? NetworkTableEvent.kImmediate : 0),
-        listener);
+    EnumSet<NetworkTableEvent.Kind> eventKinds = EnumSet.of(NetworkTableEvent.Kind.kConnection);
+    if (immediateNotify) {
+      eventKinds.add(NetworkTableEvent.Kind.kImmediate);
+    }
+    return m_listeners.add(m_handle, eventKinds, listener);
   }
 
   /**
@@ -576,16 +603,18 @@ public final class NetworkTableInstance implements AutoCloseable {
    * listener.
    *
    * @param topic Topic
-   * @param eventMask Bitmask of TopicListenerFlags values
+   * @param eventKinds set of event kinds to listen to
    * @param listener Listener function
    * @return Listener handle
    */
   public int addListener(
-      Topic topic, int eventMask, Consumer<NetworkTableEvent> listener) {
+      Topic topic,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
     if (topic.getInstance().getHandle() != m_handle) {
       throw new IllegalArgumentException("topic is not from this instance");
     }
-    return m_listeners.add(topic.getHandle(), eventMask, listener);
+    return m_listeners.add(topic.getHandle(), eventKinds, listener);
   }
 
   /**
@@ -595,16 +624,18 @@ public final class NetworkTableInstance implements AutoCloseable {
    * active.
    *
    * @param subscriber Subscriber
-   * @param eventMask Bitmask of TopicListenerFlags values
+   * @param eventKinds set of event kinds to listen to
    * @param listener Listener function
    * @return Listener handle
    */
   public int addListener(
-      Subscriber subscriber, int eventMask, Consumer<NetworkTableEvent> listener) {
+      Subscriber subscriber,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
     if (subscriber.getTopic().getInstance().getHandle() != m_handle) {
       throw new IllegalArgumentException("subscriber is not from this instance");
     }
-    return m_listeners.add(subscriber.getHandle(), eventMask, listener);
+    return m_listeners.add(subscriber.getHandle(), eventKinds, listener);
   }
 
   /**
@@ -614,16 +645,18 @@ public final class NetworkTableInstance implements AutoCloseable {
    * active.
    *
    * @param subscriber Subscriber
-   * @param eventMask Bitmask of TopicListenerFlags values
+   * @param eventKinds set of event kinds to listen to
    * @param listener Listener function
    * @return Listener handle
    */
   public int addListener(
-      MultiSubscriber subscriber, int eventMask, Consumer<NetworkTableEvent> listener) {
+      MultiSubscriber subscriber,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
     if (subscriber.getInstance().getHandle() != m_handle) {
       throw new IllegalArgumentException("subscriber is not from this instance");
     }
-    return m_listeners.add(subscriber.getHandle(), eventMask, listener);
+    return m_listeners.add(subscriber.getHandle(), eventKinds, listener);
   }
 
   /**
@@ -632,16 +665,18 @@ public final class NetworkTableInstance implements AutoCloseable {
    * accessing any shared state from the callback function.
    *
    * @param entry Entry
-   * @param eventMask Bitmask of TopicListenerFlags values
+   * @param eventKinds set of event kinds to listen to
    * @param listener Listener function
    * @return Listener handle
    */
   public int addListener(
-      NetworkTableEntry entry, int eventMask, Consumer<NetworkTableEvent> listener) {
+      NetworkTableEntry entry,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
     if (entry.getTopic().getInstance().getHandle() != m_handle) {
       throw new IllegalArgumentException("entry is not from this instance");
     }
-    return m_listeners.add(entry.getHandle(), eventMask, listener);
+    return m_listeners.add(entry.getHandle(), eventKinds, listener);
   }
 
   /**
@@ -654,15 +689,15 @@ public final class NetworkTableInstance implements AutoCloseable {
    * listener.
    *
    * @param prefixes Topic name string prefixes
-   * @param eventMask Bitmask of TopicListenerFlags values
+   * @param eventKinds set of event kinds to listen to
    * @param listener Listener function
    * @return Listener handle
    */
   public int addListener(
       String[] prefixes,
-      int eventMask,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
       Consumer<NetworkTableEvent> listener) {
-    return m_listeners.add(prefixes, eventMask, listener);
+    return m_listeners.add(prefixes, eventKinds, listener);
   }
 
   /*
@@ -672,10 +707,17 @@ public final class NetworkTableInstance implements AutoCloseable {
   /**
    * Get the current network mode.
    *
-   * @return Bitmask of NetworkMode.
+   * @return Enum set of NetworkMode.
    */
-  public int getNetworkMode() {
-    return NetworkTablesJNI.getNetworkMode(m_handle);
+  public EnumSet<NetworkMode> getNetworkMode() {
+    int flags = NetworkTablesJNI.getNetworkMode(m_handle);
+    EnumSet<NetworkMode> rv = EnumSet.noneOf(NetworkMode.class);
+    for (NetworkMode mode : NetworkMode.values()) {
+      if ((flags & mode.getValue()) != 0) {
+        rv.add(mode);
+      }
+    }
+    return rv;
   }
 
   /**
@@ -986,5 +1028,5 @@ public final class NetworkTableInstance implements AutoCloseable {
   }
 
   private boolean m_owned;
-  private final int m_handle;
+  private int m_handle;
 }

ntcore/src/generate/java/NetworkTablesJNI.java.jinja

@@ -7,6 +7,7 @@ package edu.wpi.first.networktables;
 import edu.wpi.first.util.RuntimeLoader;
 import edu.wpi.first.util.datalog.DataLog;
 import java.io.IOException;
+import java.util.EnumSet;
 import java.util.concurrent.atomic.AtomicBoolean;
 
 public final class NetworkTablesJNI {
@@ -203,6 +204,22 @@ public final class NetworkTablesJNI {
 
   public static native void destroyListenerPoller(int poller);
 
+  private static int kindsToMask(EnumSet<NetworkTableEvent.Kind> kinds) {
+    int mask = 0;
+    for (NetworkTableEvent.Kind kind : kinds) {
+      mask |= kind.getValue();
+    }
+    return mask;
+  }
+
+  public static int addListener(int poller, String[] prefixes, EnumSet<NetworkTableEvent.Kind> kinds) {
+    return addListener(poller, prefixes, kindsToMask(kinds));
+  }
+
+  public static int addListener(int poller, int handle, EnumSet<NetworkTableEvent.Kind> kinds) {
+    return addListener(poller, handle, kindsToMask(kinds));
+  }
+
   public static native int addListener(int poller, String[] prefixes, int mask);
 
   public static native int addListener(int poller, int handle, int mask);

ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableEvent.java

@@ -7,52 +7,61 @@ package edu.wpi.first.networktables;
 /**
  * NetworkTables event.
  *
- * <p>Events have flags. The flags are a bitmask and must be OR'ed together when listening to an
- * event to indicate the combination of events desired to be received.
+ * <p>There are different kinds of events. When creating a listener, a combination of event kinds
+ * can be listened to by building an EnumSet of NetworkTableEvent.Kind.
  */
 @SuppressWarnings("MemberName")
 public final class NetworkTableEvent {
-  /** No flags. */
-  public static final int kNone = 0;
+  public enum Kind {
+    /**
+     * Initial listener addition. Set this to receive immediate notification of matches to other
+     * criteria.
+     */
+    kImmediate(0x0001),
 
-  /**
-   * Initial listener addition. Set this flag to receive immediate notification of matches to the
-   * flag criteria.
-   */
-  public static final int kImmediate = 0x01;
+    /** Client connected (on server, any client connected). */
+    kConnected(0x0002),
 
-  /** Client connected (on server, any client connected). */
-  public static final int kConnected = 0x02;
+    /** Client disconnected (on server, any client disconnected). */
+    kDisconnected(0x0004),
 
-  /** Client disconnected (on server, any client disconnected). */
-  public static final int kDisconnected = 0x04;
+    /** Any connection event (connect or disconnect). */
+    kConnection(0x0004 | 0x0002),
 
-  /** Any connection event (connect or disconnect). */
-  public static final int kConnection = kConnected | kDisconnected;
+    /** New topic published. */
+    kPublish(0x0008),
 
-  /** New topic published. */
-  public static final int kPublish = 0x08;
+    /** Topic unpublished. */
+    kUnpublish(0x0010),
 
-  /** Topic unpublished. */
-  public static final int kUnpublish = 0x10;
+    /** Topic properties changed. */
+    kProperties(0x0020),
 
-  /** Topic properties changed. */
-  public static final int kProperties = 0x20;
+    /** Any topic event (publish, unpublish, or properties changed). */
+    kTopic(0x0020 | 0x0010 | 0x0008),
 
-  /** Any topic event (publish, unpublish, or properties changed). */
-  public static final int kTopic = kPublish | kUnpublish | kProperties;
+    /** Topic value updated (via network). */
+    kValueRemote(0x0040),
 
-  /** Topic value updated (via network). */
-  public static final int kValueRemote = 0x40;
+    /** Topic value updated (local). */
+    kValueLocal(0x0080),
 
-  /** Topic value updated (local). */
-  public static final int kValueLocal = 0x80;
+    /** Topic value updated (network or local). */
+    kValueAll(0x0080 | 0x0040),
 
-  /** Topic value updated (network or local). */
-  public static final int kValueAll = kValueRemote | kValueLocal;
+    /** Log message. */
+    kLogMessage(0x0100);
 
-  /** Log message. */
-  public static final int kLogMessage = 0x100;
+    private final int value;
+
+    Kind(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+  }
 
   /**
    * Handle of listener that was triggered. The value returned when adding the listener can be used
@@ -61,8 +70,8 @@ public final class NetworkTableEvent {
   public final int listener;
 
   /**
-   * Event flags. For example, kPublish if the topic was not previously published. Also indicates
-   * the data included with the event:
+   * Determine if event is of a particular kind. For example, kPublish if the topic was not
+   * previously published. Also indicates the data included with the event:
    *
    * <ul>
    *   <li>kConnected or kDisconnected: connInfo
@@ -70,8 +79,15 @@ public final class NetworkTableEvent {
    *   <li>kValueRemote, kValueLocal: valueData
    *   <li>kLogMessage: logMessage
    * </ul>
+   *
+   * @param kind Kind
+   * @return True if event matches kind
    */
-  public final int flags;
+  public boolean is(Kind kind) {
+    return (m_flags & kind.getValue()) != 0;
+  }
+
+  private final int m_flags;
 
   /** Connection information (for connection events). */
   public final ConnectionInfo connInfo;
@@ -106,7 +122,7 @@ public final class NetworkTableEvent {
       LogMessage logMessage) {
     this.m_inst = inst;
     this.listener = listener;
-    this.flags = flags;
+    this.m_flags = flags;
     this.connInfo = connInfo;
     this.topicInfo = topicInfo;
     this.valueData = valueData;

ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableListener.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.networktables;
 
+import java.util.EnumSet;
 import java.util.function.Consumer;
 
 /**
@@ -18,16 +19,16 @@ public final class NetworkTableListener implements AutoCloseable {
    *
    * @param inst Instance
    * @param prefixes Topic name string prefixes
-   * @param eventMask Bitmask of NetworkTableEvent flags values
+   * @param eventKinds set of event kinds to listen to
    * @param listener Listener function
    * @return Listener
    */
   public static NetworkTableListener createListener(
       NetworkTableInstance inst,
       String[] prefixes,
-      int eventMask,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
       Consumer<NetworkTableEvent> listener) {
-    return new NetworkTableListener(inst, inst.addListener(prefixes, eventMask, listener));
+    return new NetworkTableListener(inst, inst.addListener(prefixes, eventKinds, listener));
   }
 
   /**
@@ -35,56 +36,64 @@ public final class NetworkTableListener implements AutoCloseable {
    * subscriber with the lifetime of the listener.
    *
    * @param topic Topic
-   * @param eventMask Bitmask of NetworkTableEvent flags values
+   * @param eventKinds set of event kinds to listen to
    * @param listener Listener function
    * @return Listener
    */
   public static NetworkTableListener createListener(
-      Topic topic, int eventMask, Consumer<NetworkTableEvent> listener) {
+      Topic topic,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
     NetworkTableInstance inst = topic.getInstance();
-    return new NetworkTableListener(inst, inst.addListener(topic, eventMask, listener));
+    return new NetworkTableListener(inst, inst.addListener(topic, eventKinds, listener));
   }
 
   /**
    * Create a listener for topic changes on a subscriber. This does NOT keep the subscriber active.
    *
    * @param subscriber Subscriber
-   * @param eventMask Bitmask of NetworkTableEvent flags values
+   * @param eventKinds set of event kinds to listen to
    * @param listener Listener function
    * @return Listener
    */
   public static NetworkTableListener createListener(
-      Subscriber subscriber, int eventMask, Consumer<NetworkTableEvent> listener) {
+      Subscriber subscriber,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
     NetworkTableInstance inst = subscriber.getTopic().getInstance();
-    return new NetworkTableListener(inst, inst.addListener(subscriber, eventMask, listener));
+    return new NetworkTableListener(inst, inst.addListener(subscriber, eventKinds, listener));
   }
 
   /**
    * Create a listener for topic changes on a subscriber. This does NOT keep the subscriber active.
    *
    * @param subscriber Subscriber
-   * @param eventMask Bitmask of NetworkTableEvent flags values
+   * @param eventKinds set of event kinds to listen to
    * @param listener Listener function
    * @return Listener
    */
   public static NetworkTableListener createListener(
-      MultiSubscriber subscriber, int eventMask, Consumer<NetworkTableEvent> listener) {
+      MultiSubscriber subscriber,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
     NetworkTableInstance inst = subscriber.getInstance();
-    return new NetworkTableListener(inst, inst.addListener(subscriber, eventMask, listener));
+    return new NetworkTableListener(inst, inst.addListener(subscriber, eventKinds, listener));
   }
 
   /**
    * Create a listener for topic changes on an entry.
    *
    * @param entry Entry
-   * @param eventMask Bitmask of NetworkTableEvent flags values
+   * @param eventKinds set of event kinds to listen to
    * @param listener Listener function
    * @return Listener
    */
   public static NetworkTableListener createListener(
-      NetworkTableEntry entry, int eventMask, Consumer<NetworkTableEvent> listener) {
+      NetworkTableEntry entry,
+      EnumSet<NetworkTableEvent.Kind> eventKinds,
+      Consumer<NetworkTableEvent> listener) {
     NetworkTableInstance inst = entry.getInstance();
-    return new NetworkTableListener(inst, inst.addListener(entry, eventMask, listener));
+    return new NetworkTableListener(inst, inst.addListener(entry, eventKinds, listener));
   }
 
   /**

ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableListenerPoller.java

@@ -4,6 +4,8 @@
 
 package edu.wpi.first.networktables;
 
+import java.util.EnumSet;
+
 /**
  * Topic change listener. This queues topic change events matching the specified mask. Code using
  * the listener must periodically call readQueue() to read the events.
@@ -24,11 +26,11 @@ public final class NetworkTableListenerPoller implements AutoCloseable {
    * prefixes. This creates a corresponding internal subscriber with the lifetime of the listener.
    *
    * @param prefixes Topic name string prefixes
-   * @param eventMask Bitmask of TopicListenerFlags values
+   * @param eventKinds set of event kinds to listen to
    * @return Listener handle
    */
-  public int addListener(String[] prefixes, int eventMask) {
-    return NetworkTablesJNI.addListener(m_handle, prefixes, eventMask);
+  public int addListener(String[] prefixes, EnumSet<NetworkTableEvent.Kind> eventKinds) {
+    return NetworkTablesJNI.addListener(m_handle, prefixes, eventKinds);
   }
 
   /**
@@ -36,44 +38,44 @@ public final class NetworkTableListenerPoller implements AutoCloseable {
    * subscriber with the lifetime of the listener.
    *
    * @param topic Topic
-   * @param eventMask Bitmask of TopicListenerFlags values
+   * @param eventKinds set of event kinds to listen to
    * @return Listener handle
    */
-  public int addListener(Topic topic, int eventMask) {
-    return NetworkTablesJNI.addListener(m_handle, topic.getHandle(), eventMask);
+  public int addListener(Topic topic, EnumSet<NetworkTableEvent.Kind> eventKinds) {
+    return NetworkTablesJNI.addListener(m_handle, topic.getHandle(), eventKinds);
   }
 
   /**
    * Start listening to topic changes on a subscriber. This does NOT keep the subscriber active.
    *
    * @param subscriber Subscriber
-   * @param eventMask Bitmask of TopicListenerFlags values
+   * @param eventKinds set of event kinds to listen to
    * @return Listener handle
    */
-  public int addListener(Subscriber subscriber, int eventMask) {
-    return NetworkTablesJNI.addListener(m_handle, subscriber.getHandle(), eventMask);
+  public int addListener(Subscriber subscriber, EnumSet<NetworkTableEvent.Kind> eventKinds) {
+    return NetworkTablesJNI.addListener(m_handle, subscriber.getHandle(), eventKinds);
   }
 
   /**
    * Start listening to topic changes on a subscriber. This does NOT keep the subscriber active.
    *
    * @param subscriber Subscriber
-   * @param eventMask Bitmask of TopicListenerFlags values
+   * @param eventKinds set of event kinds to listen to
    * @return Listener handle
    */
-  public int addListener(MultiSubscriber subscriber, int eventMask) {
-    return NetworkTablesJNI.addListener(m_handle, subscriber.getHandle(), eventMask);
+  public int addListener(MultiSubscriber subscriber, EnumSet<NetworkTableEvent.Kind> eventKinds) {
+    return NetworkTablesJNI.addListener(m_handle, subscriber.getHandle(), eventKinds);
   }
 
   /**
    * Start listening to topic changes on an entry.
    *
    * @param entry Entry
-   * @param eventMask Bitmask of TopicListenerFlags values
+   * @param eventKinds set of event kinds to listen to
    * @return Listener handle
    */
-  public int addListener(NetworkTableEntry entry, int eventMask) {
-    return NetworkTablesJNI.addListener(m_handle, entry.getHandle(), eventMask);
+  public int addListener(NetworkTableEntry entry, EnumSet<NetworkTableEvent.Kind> eventKinds) {
+    return NetworkTablesJNI.addListener(m_handle, entry.getHandle(), eventKinds);
   }
 
   /**
@@ -85,10 +87,11 @@ public final class NetworkTableListenerPoller implements AutoCloseable {
    * @return Listener handle
    */
   public int addConnectionListener(boolean immediateNotify) {
-    return NetworkTablesJNI.addListener(
-        m_handle,
-        m_inst.getHandle(),
-        NetworkTableEvent.kConnection | (immediateNotify ? NetworkTableEvent.kImmediate : 0));
+    EnumSet<NetworkTableEvent.Kind> eventKinds = EnumSet.of(NetworkTableEvent.Kind.kConnection);
+    if (immediateNotify) {
+      eventKinds.add(NetworkTableEvent.Kind.kImmediate);
+    }
+    return NetworkTablesJNI.addListener(m_handle, m_inst.getHandle(), eventKinds);
   }
 
   /**

ntcore/src/main/native/cpp/LocalStorage.cpp

@@ -25,6 +25,12 @@
 
 using namespace nt;
 
+// maximum number of local publishers / subscribers to any given topic
+static constexpr size_t kMaxPublishers = 512;
+static constexpr size_t kMaxSubscribers = 512;
+static constexpr size_t kMaxMultiSubscribers = 512;
+static constexpr size_t kMaxListeners = 512;
+
 namespace {
 
 // Utility wrapper for making a set-like vector
@@ -495,15 +501,19 @@ void LSImpl::NotifyValue(TopicData* topic, unsigned int eventFlags) {
     if (subscriber->active) {
       subscriber->pollStorage.emplace_back(topic->lastValue);
       subscriber->handle.Set();
-      m_listenerStorage.Notify(subscriber->valueListeners, eventFlags,
-                               topic->handle, 0, topic->lastValue);
+      if (!subscriber->valueListeners.empty()) {
+        m_listenerStorage.Notify(subscriber->valueListeners, eventFlags,
+                                 topic->handle, 0, topic->lastValue);
+      }
     }
   }
 
   for (auto&& subscriber : topic->multiSubscribers) {
     subscriber->handle.Set();
-    m_listenerStorage.Notify(subscriber->valueListeners, eventFlags,
-                             topic->handle, 0, topic->lastValue);
+    if (!subscriber->valueListeners.empty()) {
+      m_listenerStorage.Notify(subscriber->valueListeners, eventFlags,
+                               topic->handle, 0, topic->lastValue);
+    }
   }
 }
 
@@ -889,6 +899,12 @@ std::unique_ptr<MultiSubscriberData> LSImpl::RemoveMultiSubscriber(
 
 void LSImpl::AddListenerImpl(NT_Listener listenerHandle, TopicData* topic,
                              unsigned int eventMask) {
+  if (topic->localSubscribers.size() >= kMaxSubscribers) {
+    ERROR(
+        "reached maximum number of subscribers to '{}', ignoring listener add",
+        topic->name);
+    return;
+  }
   // subscribe to make sure topic updates are received
   PubSubConfig config;
   config.topicsOnly = (eventMask & NT_EVENT_VALUE_ALL) == 0;
@@ -906,6 +922,12 @@ void LSImpl::AddListenerImpl(NT_Listener listenerHandle,
   auto topic = subscriber->topic;
 
   if ((eventMask & NT_EVENT_TOPIC) != 0) {
+    if (topic->listeners.size() >= kMaxListeners) {
+      ERROR("reached maximum number of listeners to '{}', not adding listener",
+            topic->name);
+      return;
+    }
+
     m_listenerStorage.Activate(
         listenerHandle, eventMask & (NT_EVENT_TOPIC | NT_EVENT_IMMEDIATE));
 
@@ -922,6 +944,11 @@ void LSImpl::AddListenerImpl(NT_Listener listenerHandle,
   }
 
   if ((eventMask & NT_EVENT_VALUE_ALL) != 0) {
+    if (subscriber->valueListeners.size() >= kMaxListeners) {
+      ERROR("reached maximum number of listeners to '{}', not adding listener",
+            topic->name);
+      return;
+    }
     m_listenerStorage.Activate(
         listenerHandle, eventMask & (NT_EVENT_VALUE_ALL | NT_EVENT_IMMEDIATE),
         [subentryHandle](unsigned int mask, Event* event) {
@@ -968,6 +995,11 @@ void LSImpl::AddListenerImpl(NT_Listener listenerHandle,
   }
 
   if ((eventMask & NT_EVENT_TOPIC) != 0) {
+    if (m_topicPrefixListeners.size() >= kMaxListeners) {
+      ERROR("reached maximum number of listeners, not adding listener");
+      return;
+    }
+
     m_listenerStorage.Activate(
         listenerHandle, eventMask & (NT_EVENT_TOPIC | NT_EVENT_IMMEDIATE));
 
@@ -989,6 +1021,11 @@ void LSImpl::AddListenerImpl(NT_Listener listenerHandle,
   }
 
   if ((eventMask & NT_EVENT_VALUE_ALL) != 0) {
+    if (subscriber->valueListeners.size() >= kMaxListeners) {
+      ERROR("reached maximum number of listeners, not adding listener");
+      return;
+    }
+
     m_listenerStorage.Activate(
         listenerHandle, eventMask & (NT_EVENT_VALUE_ALL | NT_EVENT_IMMEDIATE),
         [subentryHandle = subscriber->handle.GetHandle()](unsigned int mask,
@@ -1018,6 +1055,10 @@ void LSImpl::AddListenerImpl(NT_Listener listenerHandle,
 void LSImpl::AddListener(NT_Listener listenerHandle,
                          std::span<const std::string_view> prefixes,
                          unsigned int eventMask) {
+  if (m_multiSubscribers.size() >= kMaxMultiSubscribers) {
+    ERROR("reached maximum number of multi-subscribers, not adding listener");
+    return;
+  }
   // subscribe to make sure topic updates are received
   PubSubOptions options;
   options.topicsOnly = (eventMask & NT_EVENT_VALUE_ALL) == 0;
@@ -1548,6 +1589,13 @@ NT_Subscriber LocalStorage::Subscribe(NT_Topic topicHandle, NT_Type type,
     return 0;
   }
 
+  if (topic->localSubscribers.size() >= kMaxSubscribers) {
+    WPI_ERROR(m_impl->m_logger,
+              "reached maximum number of subscribers to '{}', not subscribing",
+              topic->name);
+    return 0;
+  }
+
   // Create subscriber
   return m_impl->AddLocalSubscriber(topic, PubSubConfig{type, typeStr, options})
       ->handle;
@@ -1562,6 +1610,13 @@ NT_MultiSubscriber LocalStorage::SubscribeMultiple(
     std::span<const std::string_view> prefixes,
     std::span<const PubSubOption> options) {
   std::scoped_lock lock{m_mutex};
+
+  if (m_impl->m_multiSubscribers.size() >= kMaxMultiSubscribers) {
+    WPI_ERROR(m_impl->m_logger,
+              "reached maximum number of multi-subscribers, not subscribing");
+    return 0;
+  }
+
   PubSubOptions opts{options};
   opts.prefixMatch = true;
   return m_impl->AddMultiSubscriber(prefixes, opts)->handle;
@@ -1594,6 +1649,13 @@ NT_Publisher LocalStorage::Publish(NT_Topic topicHandle, NT_Type type,
     return 0;
   }
 
+  if (topic->localPublishers.size() >= kMaxPublishers) {
+    WPI_ERROR(m_impl->m_logger,
+              "reached maximum number of publishers to '{}', not publishing",
+              topic->name);
+    return 0;
+  }
+
   return m_impl
       ->AddLocalPublisher(topic, properties,
                           PubSubConfig{type, typeStr, options})
@@ -1627,6 +1689,14 @@ NT_Entry LocalStorage::GetEntry(NT_Topic topicHandle, NT_Type type,
     return 0;
   }
 
+  if (topic->localSubscribers.size() >= kMaxSubscribers) {
+    WPI_ERROR(
+        m_impl->m_logger,
+        "reached maximum number of subscribers to '{}', not creating entry",
+        topic->name);
+    return 0;
+  }
+
   // Create subscriber
   auto subscriber =
       m_impl->AddLocalSubscriber(topic, PubSubConfig{type, typeStr, options});
@@ -2010,6 +2080,14 @@ NT_Entry LocalStorage::GetEntry(std::string_view name) {
   auto* topic = m_impl->GetOrCreateTopic(name);
 
   if (topic->entry == 0) {
+    if (topic->localSubscribers.size() >= kMaxSubscribers) {
+      WPI_ERROR(
+          m_impl->m_logger,
+          "reached maximum number of subscribers to '{}', not creating entry",
+          topic->name);
+      return 0;
+    }
+
     // Create subscriber
     auto* subscriber = m_impl->AddLocalSubscriber(topic, {});
 

ntcore/src/main/native/cpp/net/ServerImpl.cpp

@@ -620,8 +620,20 @@ void ClientData4Base::ClientSubscribe(int64_t subuid,
     sub->periodMs = kMinPeriodMs;
   }
 
+  // update periodic sender (if not local)
+  if (!m_local) {
+    if (m_periodMs == UINT32_MAX) {
+      m_periodMs = sub->periodMs;
+    } else {
+      m_periodMs = std::gcd(m_periodMs, sub->periodMs);
+    }
+    if (m_periodMs < kMinPeriodMs) {
+      m_periodMs = kMinPeriodMs;
+    }
+    m_setPeriodic(m_periodMs);
+  }
+
   // see if this immediately subscribes to any topics
-  bool updatedPeriodic = false;
   for (auto&& topic : m_server.m_topics) {
     bool removed = false;
     if (replace) {
@@ -647,14 +659,6 @@ void ClientData4Base::ClientSubscribe(int64_t subuid,
       m_server.UpdateMetaTopicSub(topic.get());
     }
 
-    if (added || removed) {
-      // update periodic sender (if not local)
-      if (!m_local) {
-        m_periodMs = std::gcd(m_periodMs, sub->periodMs);
-        updatedPeriodic = true;
-      }
-    }
-
     if (!wasSubscribed && added && !removed) {
       // announce topic to client
       DEBUG4("client {}: announce {}", m_id, topic->name);
@@ -667,12 +671,6 @@ void ClientData4Base::ClientSubscribe(int64_t subuid,
       }
     }
   }
-  if (updatedPeriodic) {
-    if (m_periodMs < kMinPeriodMs) {
-      m_periodMs = kMinPeriodMs;
-    }
-    m_setPeriodic(m_periodMs);
-  }
 
   // update meta data
   UpdateMetaClientSub();

ntcore/src/main/native/cpp/networktables/NetworkTableInstance.cpp

@@ -101,9 +101,44 @@ void NetworkTableInstance::SetServer(std::span<const std::string_view> servers,
   SetServer(serversArr);
 }
 
-NT_Listener NetworkTableInstance::AddListener(MultiSubscriber& subscriber,
-                                              int eventMask,
+NT_Listener NetworkTableInstance::AddListener(Topic topic,
+                                              unsigned int eventMask,
                                               ListenerCallback listener) {
+  if (::nt::GetInstanceFromHandle(topic.GetHandle()) != m_handle) {
+    fmt::print(stderr, "AddListener: topic is not from this instance\n");
+    return 0;
+  }
+  return ::nt::AddListener(topic.GetHandle(), eventMask, std::move(listener));
+}
+
+NT_Listener NetworkTableInstance::AddListener(Subscriber& subscriber,
+                                              unsigned int eventMask,
+                                              ListenerCallback listener) {
+  if (::nt::GetInstanceFromHandle(subscriber.GetHandle()) != m_handle) {
+    fmt::print(stderr, "AddListener: subscriber is not from this instance\n");
+    return 0;
+  }
+  return ::nt::AddListener(subscriber.GetHandle(), eventMask,
+                           std::move(listener));
+}
+
+NT_Listener NetworkTableInstance::AddListener(NetworkTableEntry& entry,
+                                              int eventMask,
+                                              ListenerCallback listener) {
+  if (::nt::GetInstanceFromHandle(entry.GetHandle()) != m_handle) {
+    fmt::print(stderr, "AddListener: entry is not from this instance\n");
+    return 0;
+  }
+  return ::nt::AddListener(entry.GetHandle(), eventMask, std::move(listener));
+}
+
+NT_Listener NetworkTableInstance::AddListener(MultiSubscriber& subscriber,
+                                              int eventMask,
+                                              ListenerCallback listener) {
+  if (::nt::GetInstanceFromHandle(subscriber.GetHandle()) != m_handle) {
+    fmt::print(stderr, "AddListener: subscriber is not from this instance\n");
+    return 0;
+  }
   return ::nt::AddListener(subscriber.GetHandle(), eventMask,
                            std::move(listener));
 }

ntcore/src/main/native/cpp/ntcore_cpp.cpp

@@ -497,6 +497,14 @@ NT_Listener AddPolledListener(NT_ListenerPoller poller,
 NT_Listener AddPolledListener(NT_ListenerPoller poller, NT_Handle handle,
                               unsigned int mask) {
   if (auto ii = InstanceImpl::GetTyped(poller, Handle::kListenerPoller)) {
+    if (Handle{handle}.GetInst() != Handle{poller}.GetInst()) {
+      WPI_ERROR(
+          ii->logger,
+          "AddPolledListener(): trying to listen to handle {} (instance {}) "
+          "with poller {} (instance {}), ignored due to different instance",
+          handle, Handle{handle}.GetInst(), poller, Handle{poller}.GetInst());
+      return {};
+    }
     auto listener = ii->listenerStorage.AddListener(poller);
     DoAddListener(*ii, listener, handle, mask);
     return listener;

ntcore/src/main/native/include/networktables/NetworkTableInstance.h

@@ -132,7 +132,7 @@ class NetworkTableInstance final {
    *
    * @param inst Instance
    */
-  static void Destroy(NetworkTableInstance inst);
+  static void Destroy(NetworkTableInstance& inst);
 
   /**
    * Gets the native handle for the entry.

ntcore/src/main/native/include/networktables/NetworkTableInstance.inc

@@ -27,9 +27,10 @@ inline NetworkTableInstance NetworkTableInstance::Create() {
   return NetworkTableInstance{CreateInstance()};
 }
 
-inline void NetworkTableInstance::Destroy(NetworkTableInstance inst) {
+inline void NetworkTableInstance::Destroy(NetworkTableInstance& inst) {
   if (inst.m_handle != 0) {
     DestroyInstance(inst.m_handle);
+    inst.m_handle = 0;
   }
 }
 
@@ -99,22 +100,6 @@ inline NT_Listener NetworkTableInstance::AddConnectionListener(
       std::move(callback));
 }
 
-inline NT_Listener NetworkTableInstance::AddListener(
-    Topic topic, unsigned int eventMask, ListenerCallback listener) {
-  return ::nt::AddListener(topic.GetHandle(), eventMask, std::move(listener));
-}
-
-inline NT_Listener NetworkTableInstance::AddListener(
-    Subscriber& subscriber, unsigned int eventMask, ListenerCallback listener) {
-  return ::nt::AddListener(subscriber.GetHandle(), eventMask,
-                           std::move(listener));
-}
-
-inline NT_Listener NetworkTableInstance::AddListener(
-    NetworkTableEntry& entry, int eventMask, ListenerCallback listener) {
-  return ::nt::AddListener(entry.GetHandle(), eventMask, std::move(listener));
-}
-
 inline NT_Listener NetworkTableInstance::AddListener(
     std::span<const std::string_view> prefixes, int eventMask,
     ListenerCallback listener) {

ntcore/src/main/native/include/networktables/NetworkTableListener.inc

@@ -72,7 +72,11 @@ inline NetworkTableListener::NetworkTableListener(NetworkTableListener&& rhs)
 
 inline NetworkTableListener& NetworkTableListener::operator=(
     NetworkTableListener&& rhs) {
-  std::swap(m_handle, rhs.m_handle);
+  if (m_handle != 0) {
+    nt::RemoveListener(m_handle);
+  }
+  m_handle = rhs.m_handle;
+  rhs.m_handle = 0;
   return *this;
 }
 
@@ -102,7 +106,11 @@ inline NetworkTableListenerPoller::NetworkTableListenerPoller(
 
 inline NetworkTableListenerPoller& NetworkTableListenerPoller::operator=(
     NetworkTableListenerPoller&& rhs) {
-  std::swap(m_handle, rhs.m_handle);
+  if (m_handle != 0) {
+    nt::DestroyListenerPoller(m_handle);
+  }
+  m_handle = rhs.m_handle;
+  rhs.m_handle = 0;
   return *this;
 }
 

ntcore/src/main/native/include/ntcore_cpp.h

@@ -223,6 +223,14 @@ class Event {
    */
   unsigned int flags{0};
 
+  /**
+   * Test event flags.
+   *
+   * @param kind event flag(s) to test
+   * @return True if flags matches kind
+   */
+  bool Is(unsigned int kind) const { return (flags & kind) != 0; }
+
   /** Event data; content depends on flags. */
   std::variant<ConnectionInfo, TopicInfo, ValueEventData, LogMessage> data;
 

ntcore/src/test/java/edu/wpi/first/networktables/ConnectionListenerTest.java

@@ -8,10 +8,12 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertNotNull;
 import static org.junit.jupiter.api.Assertions.assertNotSame;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.api.Assertions.fail;
 
 import edu.wpi.first.util.WPIUtilJNI;
 import java.util.ArrayList;
+import java.util.EnumSet;
 import java.util.List;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
@@ -64,7 +66,7 @@ class ConnectionListenerTest {
     assertNotSame(poller, 0, "bad poller handle");
     int handle =
         NetworkTablesJNI.addListener(
-            poller, m_serverInst.getHandle(), NetworkTableEvent.kConnection);
+            poller, m_serverInst.getHandle(), EnumSet.of(NetworkTableEvent.Kind.kConnection));
     assertNotSame(handle, 0, "bad listener handle");
 
     // trigger a connect event
@@ -82,7 +84,7 @@ class ConnectionListenerTest {
     assertEquals(1, events.length);
     assertEquals(handle, events[0].listener);
     assertNotNull(events[0].connInfo);
-    assertEquals(events[0].flags, NetworkTableEvent.kConnected);
+    assertTrue(events[0].is(NetworkTableEvent.Kind.kConnected));
 
     // trigger a disconnect event
     m_clientInst.stopClient();
@@ -103,7 +105,7 @@ class ConnectionListenerTest {
     assertNotNull(events);
     assertEquals(1, events.length);
     assertEquals(handle, events[0].listener);
-    assertEquals(events[0].flags, NetworkTableEvent.kDisconnected);
+    assertTrue(events[0].is(NetworkTableEvent.Kind.kDisconnected));
   }
 
   private static int threadedPort = 10001;
@@ -155,7 +157,7 @@ class ConnectionListenerTest {
       assertEquals(1, events.size());
       assertEquals(handle, events.get(0).listener);
       assertNotNull(events.get(0).connInfo);
-      assertEquals(events.get(0).flags, NetworkTableEvent.kConnected);
+      assertTrue(events.get(0).is(NetworkTableEvent.Kind.kConnected));
       events.clear();
     }
 
@@ -180,7 +182,7 @@ class ConnectionListenerTest {
       assertEquals(1, events.size());
       assertEquals(handle, events.get(0).listener);
       assertNotNull(events.get(0).connInfo);
-      assertEquals(events.get(0).flags, NetworkTableEvent.kDisconnected);
+      assertTrue(events.get(0).is(NetworkTableEvent.Kind.kDisconnected));
     }
   }
 }

ntcore/src/test/java/edu/wpi/first/networktables/LoggerTest.java

@@ -37,7 +37,7 @@ class LoggerTest {
     // wait for client to report it's started, then wait another 0.1 sec
     try {
       int count = 0;
-      while ((m_clientInst.getNetworkMode() & NetworkTableInstance.kNetModeClient4) == 0) {
+      while (!m_clientInst.getNetworkMode().contains(NetworkTableInstance.NetworkMode.kClient4)) {
         Thread.sleep(100);
         count++;
         if (count > 30) {

ntcore/src/test/java/edu/wpi/first/networktables/TopicListenerTest.java

@@ -6,9 +6,11 @@ package edu.wpi.first.networktables;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertNotNull;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.api.Assertions.fail;
 
 import edu.wpi.first.util.WPIUtilJNI;
+import java.util.EnumSet;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Disabled;
@@ -37,7 +39,8 @@ class TopicListenerTest {
 
     // Use connection listener to ensure we've connected
     int poller = NetworkTablesJNI.createListenerPoller(m_clientInst.getHandle());
-    NetworkTablesJNI.addListener(poller, m_clientInst.getHandle(), NetworkTableEvent.kConnected);
+    NetworkTablesJNI.addListener(
+        poller, m_clientInst.getHandle(), EnumSet.of(NetworkTableEvent.Kind.kConnected));
     try {
       if (WPIUtilJNI.waitForObjectTimeout(poller, 1.0)) {
         fail("client didn't connect to server");
@@ -55,7 +58,8 @@ class TopicListenerTest {
     connect();
     final int poller = NetworkTablesJNI.createListenerPoller(m_serverInst.getHandle());
     final int handle =
-        NetworkTablesJNI.addListener(poller, new String[] {"/foo"}, NetworkTableEvent.kPublish);
+        NetworkTablesJNI.addListener(
+            poller, new String[] {"/foo"}, EnumSet.of(NetworkTableEvent.Kind.kPublish));
 
     // Trigger an event
     m_clientInst.getEntry("/foo/bar").setDouble(1.0);
@@ -83,6 +87,6 @@ class TopicListenerTest {
     assertNotNull(events[0].topicInfo);
     assertEquals(m_serverInst.getTopic("/foo/bar"), events[0].topicInfo.getTopic());
     assertEquals("/foo/bar", events[0].topicInfo.name);
-    assertEquals(NetworkTableEvent.kPublish, events[0].flags);
+    assertTrue(events[0].is(NetworkTableEvent.Kind.kPublish));
   }
 }

ntcore/src/test/native/cpp/ValueListenerTest.cpp

@@ -346,4 +346,64 @@ TEST_F(ValueListenerTest, PollImmediateSubMultiple) {
   EXPECT_EQ(valueData->value, nt::Value::MakeDouble(1.0));
 }
 
+TEST_F(ValueListenerTest, TwoSubOneListener) {
+  auto topic = nt::GetTopic(m_inst, "foo");
+  auto pub = nt::Publish(topic, NT_DOUBLE, "double");
+  auto sub1 = nt::Subscribe(topic, NT_DOUBLE, "double");
+  auto sub2 = nt::Subscribe(topic, NT_DOUBLE, "double");
+  auto sub3 = nt::SubscribeMultiple(m_inst, {{"foo"}});
+
+  auto poller = nt::CreateListenerPoller(m_inst);
+  auto h = nt::AddPolledListener(poller, sub1, nt::EventFlags::kValueLocal);
+  (void)sub2;
+  (void)sub3;
+
+  nt::SetDouble(pub, 0);
+
+  bool timedOut = false;
+  ASSERT_TRUE(wpi::WaitForObject(poller, 1.0, &timedOut));
+  ASSERT_FALSE(timedOut);
+  auto results = nt::ReadListenerQueue(poller);
+
+  ASSERT_EQ(results.size(), 1u);
+  EXPECT_EQ(results[0].flags & nt::EventFlags::kValueLocal,
+            nt::EventFlags::kValueLocal);
+  EXPECT_EQ(results[0].listener, h);
+  auto valueData = results[0].GetValueEventData();
+  ASSERT_TRUE(valueData);
+  EXPECT_EQ(valueData->subentry, sub1);
+  EXPECT_EQ(valueData->topic, topic);
+  EXPECT_EQ(valueData->value, nt::Value::MakeDouble(0.0));
+}
+
+TEST_F(ValueListenerTest, TwoSubOneMultiListener) {
+  auto topic = nt::GetTopic(m_inst, "foo");
+  auto pub = nt::Publish(topic, NT_DOUBLE, "double");
+  auto sub1 = nt::Subscribe(topic, NT_DOUBLE, "double");
+  auto sub2 = nt::Subscribe(topic, NT_DOUBLE, "double");
+  auto sub3 = nt::SubscribeMultiple(m_inst, {{"foo"}});
+
+  auto poller = nt::CreateListenerPoller(m_inst);
+  auto h = nt::AddPolledListener(poller, sub3, nt::EventFlags::kValueLocal);
+  (void)sub1;
+  (void)sub2;
+
+  nt::SetDouble(pub, 0);
+
+  bool timedOut = false;
+  ASSERT_TRUE(wpi::WaitForObject(poller, 1.0, &timedOut));
+  ASSERT_FALSE(timedOut);
+  auto results = nt::ReadListenerQueue(poller);
+
+  ASSERT_EQ(results.size(), 1u);
+  EXPECT_EQ(results[0].flags & nt::EventFlags::kValueLocal,
+            nt::EventFlags::kValueLocal);
+  EXPECT_EQ(results[0].listener, h);
+  auto valueData = results[0].GetValueEventData();
+  ASSERT_TRUE(valueData);
+  EXPECT_EQ(valueData->subentry, sub3);
+  EXPECT_EQ(valueData->topic, topic);
+  EXPECT_EQ(valueData->value, nt::Value::MakeDouble(0.0));
+}
+
 }  // namespace nt

upstream_utils/mpack_patches/0001-Don-t-emit-inline-defs.patch

@@ -0,0 +1,22 @@
+From 05864e768ca1458c1e24f433d091306a7d47562b Mon Sep 17 00:00:00 2001
+From: PJ Reiniger <pj.reiniger@gmail.com>
+Date: Sat, 29 Oct 2022 12:09:03 -0400
+Subject: [PATCH 1/3] Don't emit inline defs
+
+---
+ src/mpack/mpack-platform.c | 2 +-
+ 1 file changed, 1 insertion(+), 1 deletion(-)
+
+diff --git a/src/mpack/mpack-platform.c b/src/mpack/mpack-platform.c
+index 6599e1f..d4a2fa3 100644
+--- a/src/mpack/mpack-platform.c
++++ b/src/mpack/mpack-platform.c
+@@ -24,7 +24,7 @@
+ // standalone definitions of all (non-static) inline functions in MPack.
+ 
+ #define MPACK_INTERNAL 1
+-#define MPACK_EMIT_INLINE_DEFS 1
++#define MPACK_EMIT_INLINE_DEFS 0
+ 
+ #include "mpack-platform.h"
+ #include "mpack.h"

upstream_utils/mpack_patches/0002-Update-amalgamation-script.patch

@@ -0,0 +1,24 @@
+From d4d045c843d4b4de747d800e570c32cff3759a80 Mon Sep 17 00:00:00 2001
+From: PJ Reiniger <pj.reiniger@gmail.com>
+Date: Sat, 29 Oct 2022 12:16:36 -0400
+Subject: [PATCH 2/3] Update amalgamation script
+
+---
+ tools/amalgamate.sh | 4 ++--
+ 1 file changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/tools/amalgamate.sh b/tools/amalgamate.sh
+index 2e24e27..4dfe999 100755
+--- a/tools/amalgamate.sh
++++ b/tools/amalgamate.sh
+@@ -74,8 +74,8 @@ echo -e "#endif\n" >> $HEADER
+ 
+ # assemble source
+ echo -e "#define MPACK_INTERNAL 1" >> $SOURCE
+-echo -e "#define MPACK_EMIT_INLINE_DEFS 1\n" >> $SOURCE
+-echo -e "#include \"mpack.h\"\n" >> $SOURCE
++echo -e "#define MPACK_EMIT_INLINE_DEFS 0\n" >> $SOURCE
++echo -e "#include \"wpi/mpack.h\"\n" >> $SOURCE
+ for f in $SOURCES; do
+     echo -e "\n/* $f.c */" >> $SOURCE
+     sed -e 's@^#include ".*@/* & */@' -e '0,/^ \*\/$/d' src/$f >> $SOURCE

upstream_utils/mpack_patches/0003-Use-namespace-for-C.patch

@@ -0,0 +1,158 @@
+From 37854ea8a4a4b387940719c40bd32792f1e6e027 Mon Sep 17 00:00:00 2001
+From: PJ Reiniger <pj.reiniger@gmail.com>
+Date: Sat, 29 Oct 2022 12:22:50 -0400
+Subject: [PATCH 3/3] Use namespace for C++
+
+---
+ src/mpack/mpack-common.c   | 2 ++
+ src/mpack/mpack-expect.c   | 2 ++
+ src/mpack/mpack-node.c     | 2 ++
+ src/mpack/mpack-platform.c | 2 ++
+ src/mpack/mpack-platform.h | 2 +-
+ src/mpack/mpack-reader.c   | 2 ++
+ src/mpack/mpack-writer.c   | 2 ++
+ src/mpack/mpack-writer.h   | 3 ++-
+ 8 files changed, 15 insertions(+), 2 deletions(-)
+
+diff --git a/src/mpack/mpack-common.c b/src/mpack/mpack-common.c
+index 2c133a3..dc7207f 100644
+--- a/src/mpack/mpack-common.c
++++ b/src/mpack/mpack-common.c
+@@ -24,6 +24,7 @@
+ #include "mpack-common.h"
+ 
+ MPACK_SILENCE_WARNINGS_BEGIN
++namespace mpack {
+ 
+ const char* mpack_error_to_string(mpack_error_t error) {
+     #if MPACK_STRINGS
+@@ -748,4 +749,5 @@ void mpack_print_file_callback(void* context, const char* data, size_t count) {
+ }
+ #endif
+ 
++}  // namespace mpack
+ MPACK_SILENCE_WARNINGS_END
+diff --git a/src/mpack/mpack-expect.c b/src/mpack/mpack-expect.c
+index 81576d1..6232a67 100644
+--- a/src/mpack/mpack-expect.c
++++ b/src/mpack/mpack-expect.c
+@@ -24,6 +24,7 @@
+ #include "mpack-expect.h"
+ 
+ MPACK_SILENCE_WARNINGS_BEGIN
++namespace mpack {
+ 
+ #if MPACK_EXPECT
+ 
+@@ -880,4 +881,5 @@ size_t mpack_expect_key_cstr(mpack_reader_t* reader, const char* keys[], bool fo
+ 
+ #endif
+ 
++}  // namespace mpack
+ MPACK_SILENCE_WARNINGS_END
+diff --git a/src/mpack/mpack-node.c b/src/mpack/mpack-node.c
+index 3d4b0f4..aba9897 100644
+--- a/src/mpack/mpack-node.c
++++ b/src/mpack/mpack-node.c
+@@ -24,6 +24,7 @@
+ #include "mpack-node.h"
+ 
+ MPACK_SILENCE_WARNINGS_BEGIN
++namespace mpack {
+ 
+ #if MPACK_NODE
+ 
+@@ -2401,4 +2402,5 @@ mpack_node_t mpack_node_map_value_at(mpack_node_t node, size_t index) {
+ 
+ #endif
+ 
++}  // namespace mpack
+ MPACK_SILENCE_WARNINGS_END
+diff --git a/src/mpack/mpack-platform.c b/src/mpack/mpack-platform.c
+index d4a2fa3..75d2de3 100644
+--- a/src/mpack/mpack-platform.c
++++ b/src/mpack/mpack-platform.c
+@@ -30,6 +30,7 @@
+ #include "mpack.h"
+ 
+ MPACK_SILENCE_WARNINGS_BEGIN
++namespace mpack {
+ 
+ #if MPACK_DEBUG
+ 
+@@ -218,4 +219,5 @@ void* mpack_realloc(void* old_ptr, size_t used_size, size_t new_size) {
+ }
+ #endif
+ 
++}  // namespace mpack
+ MPACK_SILENCE_WARNINGS_END
+diff --git a/src/mpack/mpack-platform.h b/src/mpack/mpack-platform.h
+index 79604c9..27a2f9e 100644
+--- a/src/mpack/mpack-platform.h
++++ b/src/mpack/mpack-platform.h
+@@ -1043,7 +1043,7 @@ void mpack_assert_fail(const char* message);
+  */
+ 
+ #ifdef __cplusplus
+-    #define MPACK_EXTERN_C_BEGIN extern "C" {
++    #define MPACK_EXTERN_C_BEGIN namespace mpack {
+     #define MPACK_EXTERN_C_END   }
+ #else
+     #define MPACK_EXTERN_C_BEGIN /*nothing*/
+diff --git a/src/mpack/mpack-reader.c b/src/mpack/mpack-reader.c
+index c6d2223..a135879 100644
+--- a/src/mpack/mpack-reader.c
++++ b/src/mpack/mpack-reader.c
+@@ -24,6 +24,7 @@
+ #include "mpack-reader.h"
+ 
+ MPACK_SILENCE_WARNINGS_BEGIN
++namespace mpack {
+ 
+ #if MPACK_READER
+ 
+@@ -1284,4 +1285,5 @@ void mpack_print_stdfile_to_callback(FILE* file, mpack_print_callback_t callback
+ 
+ #endif
+ 
++}  // namespace mpack
+ MPACK_SILENCE_WARNINGS_END
+diff --git a/src/mpack/mpack-writer.c b/src/mpack/mpack-writer.c
+index 4d052b1..9630d9e 100644
+--- a/src/mpack/mpack-writer.c
++++ b/src/mpack/mpack-writer.c
+@@ -24,6 +24,7 @@
+ #include "mpack-writer.h"
+ 
+ MPACK_SILENCE_WARNINGS_BEGIN
++namespace mpack {
+ 
+ #if MPACK_WRITER
+ 
+@@ -1772,4 +1773,5 @@ void mpack_complete_array(mpack_writer_t* writer) {
+ #endif // MPACK_BUILDER
+ #endif // MPACK_WRITER
+ 
++}  // namespace mpack
+ MPACK_SILENCE_WARNINGS_END
+diff --git a/src/mpack/mpack-writer.h b/src/mpack/mpack-writer.h
+index c239ee6..abeee1a 100644
+--- a/src/mpack/mpack-writer.h
++++ b/src/mpack/mpack-writer.h
+@@ -1168,6 +1168,7 @@ MPACK_EXTERN_C_END
+ 
+ #if defined(__cplusplus) || defined(MPACK_DOXYGEN)
+ 
++namespace mpack {
+ /**
+  * @name C++ write overloads
+  * @{
+@@ -1304,7 +1305,7 @@ MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, const
+ /**
+  * @}
+  */
+-
++}  // namespace mpack
+ #endif /* __cplusplus */
+ 
+ /**

upstream_utils/update_mpack.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+
+import os
+import shutil
+import subprocess
+
+from upstream_utils import (
+    get_repo_root,
+    clone_repo,
+    walk_cwd_and_copy_if,
+    git_am,
+)
+
+
+def main():
+    upstream_root = clone_repo("https://github.com/ludocode/mpack", "v1.1")
+    wpilib_root = get_repo_root()
+    wpiutil = os.path.join(wpilib_root, "wpiutil")
+
+    # Delete old install
+    for d in [
+        "src/main/native/thirdparty/mpack/src",
+        "src/main/native/thirdparty/mpack/include",
+    ]:
+        shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True)
+
+    # Apply patches to upstream Git repo
+    os.chdir(upstream_root)
+
+    for f in [
+        "0001-Don-t-emit-inline-defs.patch",
+        "0002-Update-amalgamation-script.patch",
+        "0003-Use-namespace-for-C.patch",
+    ]:
+        git_am(
+            os.path.join(wpilib_root, "upstream_utils/mpack_patches", f),
+        )
+
+    # Run the amalgmation script
+    subprocess.check_call(["bash", "tools/amalgamate.sh"])
+
+    # Copy the files
+    amalgamation_source_dir = os.path.join(
+        ".", ".build", "amalgamation", "src", "mpack"
+    )
+    os.chdir(amalgamation_source_dir)
+
+    walk_cwd_and_copy_if(
+        lambda dp, f: f.endswith(".h"),
+        os.path.join(wpiutil, "src/main/native/thirdparty/mpack/include/wpi"),
+    )
+    walk_cwd_and_copy_if(
+        lambda dp, f: f.endswith(".c"),
+        os.path.join(wpiutil, "src/main/native/thirdparty/mpack/src"),
+    )
+
+
+if __name__ == "__main__":
+    main()

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

@@ -16,7 +16,7 @@ import java.util.Set;
  * <p>Wrapped commands may only be used through the wrapper, trying to directly schedule them or add
  * them to a group will throw an exception.
  */
-public abstract class WrapperCommand implements Command {
+public abstract class WrapperCommand extends CommandBase {
   protected final Command m_command;
 
   /**
@@ -99,14 +99,4 @@ public abstract class WrapperCommand implements Command {
   public InterruptionBehavior getInterruptionBehavior() {
     return m_command.getInterruptionBehavior();
   }
-
-  /**
-   * Gets the name of this Command.
-   *
-   * @return Name
-   */
-  @Override
-  public String getName() {
-    return m_command.getName();
-  }
 }

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandJoystick.java

@@ -4,7 +4,6 @@
 
 package edu.wpi.first.wpilibj2.command.button;
 
-import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.event.EventLoop;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -33,7 +32,7 @@ public class CommandJoystick extends CommandGenericHID {
    * @return the wrapped GenericHID object
    */
   @Override
-  public GenericHID getHID() {
+  public Joystick getHID() {
     return m_hid;
   }
 
@@ -171,6 +170,24 @@ public class CommandJoystick extends CommandGenericHID {
     return m_hid.getThrottleChannel();
   }
 
+  /**
+   * Get the x position of the HID.
+   *
+   * @return the x position
+   */
+  public double getX() {
+    return m_hid.getX();
+  }
+
+  /**
+   * Get the y position of the HID.
+   *
+   * @return the y position
+   */
+  public double getY() {
+    return m_hid.getY();
+  }
+
   /**
    * Get the z position of the HID.
    *

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/CommandPS4Controller.java

@@ -4,7 +4,6 @@
 
 package edu.wpi.first.wpilibj2.command.button;
 
-import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.PS4Controller;
 import edu.wpi.first.wpilibj.event.EventLoop;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -34,7 +33,7 @@ public class CommandPS4Controller extends CommandGenericHID {
    * @return the wrapped GenericHID object
    */
   @Override
-  public GenericHID getHID() {
+  public PS4Controller getHID() {
     return m_hid;
   }
 

wpilibc/src/main/native/cpp/apriltag/AprilTag.cpp

@@ -0,0 +1,26 @@
+// 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/apriltag/AprilTag.h"
+
+#include <wpi/json.h>
+
+using namespace frc;
+
+bool AprilTag::operator==(const AprilTag& other) const {
+  return ID == other.ID && pose == other.pose;
+}
+
+bool AprilTag::operator!=(const AprilTag& other) const {
+  return !operator==(other);
+}
+
+void frc::to_json(wpi::json& json, const AprilTag& apriltag) {
+  json = wpi::json{{"ID", apriltag.ID}, {"pose", apriltag.pose}};
+}
+
+void frc::from_json(const wpi::json& json, AprilTag& apriltag) {
+  apriltag.ID = json.at("ID").get<int>();
+  apriltag.pose = json.at("pose").get<Pose3d>();
+}

wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp

@@ -0,0 +1,97 @@
+// 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/apriltag/AprilTagFieldLayout.h"
+
+#include <algorithm>
+#include <system_error>
+
+#include <units/angle.h>
+#include <units/length.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) {
+  std::error_code error_code;
+
+  wpi::raw_fd_istream input{path, error_code};
+  if (error_code) {
+    throw std::runtime_error(fmt::format("Cannot open file: {}", path));
+  }
+
+  wpi::json json;
+  input >> json;
+
+  m_apriltags = json.at("tags").get<std::vector<AprilTag>>();
+  m_fieldWidth = units::meter_t{json.at("field").at("width").get<double>()};
+  m_fieldLength = units::meter_t{json.at("field").at("height").get<double>()};
+}
+
+AprilTagFieldLayout::AprilTagFieldLayout(std::vector<AprilTag> apriltags,
+                                         units::meter_t fieldLength,
+                                         units::meter_t fieldWidth)
+    : m_apriltags(std::move(apriltags)),
+      m_fieldLength(std::move(fieldLength)),
+      m_fieldWidth(std::move(fieldWidth)) {}
+
+void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) {
+  m_mirror = alliance == DriverStation::Alliance::kRed;
+}
+
+std::optional<frc::Pose3d> AprilTagFieldLayout::GetTagPose(int ID) const {
+  Pose3d returnPose;
+  auto it = std::find_if(m_apriltags.begin(), m_apriltags.end(),
+                         [=](const auto& tag) { return tag.ID == ID; });
+  if (it != m_apriltags.end()) {
+    returnPose = it->pose;
+  } else {
+    return std::optional<Pose3d>();
+  }
+  if (m_mirror) {
+    returnPose = returnPose.RelativeTo(Pose3d{
+        m_fieldLength, m_fieldWidth, 0_m, Rotation3d{0_deg, 0_deg, 180_deg}});
+  }
+  return std::make_optional(returnPose);
+}
+
+void AprilTagFieldLayout::Serialize(std::string_view path) {
+  std::error_code error_code;
+
+  wpi::raw_fd_ostream output{path, error_code};
+  if (error_code) {
+    throw std::runtime_error(fmt::format("Cannot open file: {}", path));
+  }
+
+  wpi::json json = *this;
+  output << json;
+  output.flush();
+}
+
+bool AprilTagFieldLayout::operator==(const AprilTagFieldLayout& other) const {
+  return m_apriltags == other.m_apriltags && m_mirror == other.m_mirror &&
+         m_fieldLength == other.m_fieldLength &&
+         m_fieldWidth == other.m_fieldWidth;
+}
+
+bool AprilTagFieldLayout::operator!=(const AprilTagFieldLayout& other) const {
+  return !operator==(other);
+}
+
+void frc::to_json(wpi::json& json, const AprilTagFieldLayout& layout) {
+  json = wpi::json{{"field",
+                    {{"length", layout.m_fieldLength.value()},
+                     {"width", layout.m_fieldWidth.value()}}},
+                   {"tags", layout.m_apriltags}};
+}
+
+void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) {
+  layout.m_apriltags = json.at("tags").get<std::vector<AprilTag>>();
+  layout.m_fieldLength =
+      units::meter_t{json.at("field").at("length").get<double>()};
+  layout.m_fieldWidth =
+      units::meter_t{json.at("field").at("width").get<double>()};
+}

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

@@ -13,11 +13,13 @@ PS4ControllerSim::PS4ControllerSim(const PS4Controller& joystick)
     : GenericHIDSim{joystick} {
   SetAxisCount(6);
   SetButtonCount(14);
+  SetPOVCount(1);
 }
 
 PS4ControllerSim::PS4ControllerSim(int port) : GenericHIDSim{port} {
   SetAxisCount(6);
   SetButtonCount(14);
+  SetPOVCount(1);
 }
 
 void PS4ControllerSim::SetLeftX(double value) {

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

@@ -40,11 +40,11 @@ SingleJointedArmSim::SingleJointedArmSim(
           simulateGravity, measurementStdDevs) {}
 
 bool SingleJointedArmSim::WouldHitLowerLimit(units::radian_t armAngle) const {
-  return armAngle < m_minAngle;
+  return armAngle <= m_minAngle;
 }
 
 bool SingleJointedArmSim::WouldHitUpperLimit(units::radian_t armAngle) const {
-  return armAngle > m_maxAngle;
+  return armAngle >= m_maxAngle;
 }
 
 bool SingleJointedArmSim::HasHitLowerLimit() const {

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

@@ -13,11 +13,13 @@ XboxControllerSim::XboxControllerSim(const XboxController& joystick)
     : GenericHIDSim{joystick} {
   SetAxisCount(6);
   SetButtonCount(10);
+  SetPOVCount(1);
 }
 
 XboxControllerSim::XboxControllerSim(int port) : GenericHIDSim{port} {
   SetAxisCount(6);
   SetButtonCount(10);
+  SetPOVCount(1);
 }
 
 void XboxControllerSim::SetLeftX(double value) {

wpilibc/src/main/native/include/frc/apriltag/AprilTag.h

@@ -0,0 +1,45 @@
+// 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 <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose3d.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+
+struct WPILIB_DLLEXPORT AprilTag {
+  int ID;
+
+  Pose3d pose;
+
+  /**
+   * Checks equality between this AprilTag and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const AprilTag& other) const;
+
+  /**
+   * Checks inequality between this AprilTag and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const AprilTag& other) const;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const AprilTag& apriltag);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, AprilTag& apriltag);
+
+}  // namespace frc

wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h

@@ -0,0 +1,122 @@
+// 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 <optional>
+#include <string_view>
+#include <vector>
+
+#include <units/length.h>
+#include <wpi/SymbolExports.h>
+
+#include "frc/DriverStation.h"
+#include "frc/apriltag/AprilTag.h"
+#include "frc/geometry/Pose3d.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+/**
+ * Class for representing a layout of AprilTags on a field and reading them from
+ * a JSON format.
+ *
+ * The JSON format contains two top-level objects, "tags" and "field".
+ * The "tags" object is a list of all AprilTags contained within a layout. Each
+ * AprilTag serializes to a JSON object containing an ID and a Pose3d. The
+ * "field" object is a descriptor of the size of the field in meters with
+ * "width" and "height" values.  This is to account for arbitrary field sizes
+ * when mirroring the poses.
+ *
+ * Pose3ds are assumed to be measured from the bottom-left corner of the field,
+ * when the blue alliance is at the left. Pose3ds will automatically be returned
+ * as passed in when calling GetTagPose(int).  Setting an alliance color via
+ * SetAlliance(DriverStation::Alliance) will mirror the poses returned from
+ * GetTagPose(int) to be correct relative to the other alliance.
+ */
+class WPILIB_DLLEXPORT AprilTagFieldLayout {
+ public:
+  AprilTagFieldLayout() = default;
+
+  /**
+   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
+   *
+   * @param path Path of the JSON file to import from.
+   */
+  explicit AprilTagFieldLayout(std::string_view path);
+
+  /**
+   * Construct a new AprilTagFieldLayout from a vector of AprilTag objects.
+   *
+   * @param apriltags Vector of AprilTags.
+   * @param fieldLength Length of field the layout of representing.
+   * @param fieldWidth Width of field the layout is representing.
+   */
+  AprilTagFieldLayout(std::vector<AprilTag> apriltags,
+                      units::meter_t fieldLength, units::meter_t fieldWidth);
+
+  /**
+   * Set the alliance that your team is on.
+   *
+   * This changes the GetTagPose(int) method to return the correct pose for your
+   * alliance.
+   *
+   * @param alliance The alliance to mirror poses for.
+   */
+  void SetAlliance(DriverStation::Alliance alliance);
+
+  /**
+   * Gets an AprilTag pose by its ID.
+   *
+   * @param ID The ID of the tag.
+   * @return The pose corresponding to the ID that was passed in or an empty
+   * optional if a tag with that ID is not found.
+   */
+  std::optional<Pose3d> GetTagPose(int ID) const;
+
+  /**
+   * Serializes an AprilTagFieldLayout to a JSON file.
+   *
+   * @param path The path to write the JSON file to.
+   */
+  void Serialize(std::string_view path);
+
+  /*
+   * Checks equality between this AprilTagFieldLayout and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const AprilTagFieldLayout& other) const;
+
+  /**
+   * Checks inequality between this AprilTagFieldLayout and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const AprilTagFieldLayout& other) const;
+
+ private:
+  std::vector<AprilTag> m_apriltags;
+  units::meter_t m_fieldLength;
+  units::meter_t m_fieldWidth;
+  bool m_mirror = false;
+
+  friend WPILIB_DLLEXPORT void to_json(wpi::json& json,
+                                       const AprilTagFieldLayout& layout);
+
+  friend WPILIB_DLLEXPORT void from_json(const wpi::json& json,
+                                         AprilTagFieldLayout& layout);
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const AprilTagFieldLayout& layout);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, AprilTagFieldLayout& layout);
+
+}  // namespace frc

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

@@ -36,12 +36,16 @@ class SuppliedValueWidget : public ShuffleboardWidget<SuppliedValueWidget<T>> {
   void BuildInto(std::shared_ptr<nt::NetworkTable> parentTable,
                  std::shared_ptr<nt::NetworkTable> metaTable) override {
     this->BuildMetadata(metaTable);
-    m_controllablePub =
-        nt::BooleanTopic{metaTable->GetTopic("Controllable")}.Publish();
-    m_controllablePub.Set(false);
+    if (!m_controllablePub) {
+      m_controllablePub =
+          nt::BooleanTopic{metaTable->GetTopic("Controllable")}.Publish();
+      m_controllablePub.Set(false);
+    }
 
-    m_entry =
-        parentTable->GetTopic(this->GetTitle()).GenericPublish(m_typeString);
+    if (!m_entry) {
+      m_entry =
+          parentTable->GetTopic(this->GetTitle()).GenericPublish(m_typeString);
+    }
     m_setter(m_entry, m_supplier());
   }
 

wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp

@@ -0,0 +1,27 @@
+// 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 <vector>
+
+#include <wpi/json.h>
+
+#include "frc/apriltag/AprilTag.h"
+#include "frc/apriltag/AprilTagFieldLayout.h"
+#include "frc/geometry/Pose3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(AprilTagJsonTest, DeserializeMatches) {
+  auto layout = AprilTagFieldLayout{
+      std::vector{
+          AprilTag{1, Pose3d{}},
+          AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}},
+      54_ft, 27_ft};
+
+  AprilTagFieldLayout deserialized;
+  wpi::json json = layout;
+  EXPECT_NO_THROW(deserialized = json.get<AprilTagFieldLayout>());
+  EXPECT_EQ(layout, deserialized);
+}

wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp

@@ -0,0 +1,32 @@
+// 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 <vector>
+
+#include <wpi/json.h>
+
+#include "frc/apriltag/AprilTag.h"
+#include "frc/apriltag/AprilTagFieldLayout.h"
+#include "frc/geometry/Pose3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(AprilTagPoseMirroringTest, MirroringMatches) {
+  auto layout = AprilTagFieldLayout{
+      std::vector<AprilTag>{
+          AprilTag{1,
+                   Pose3d{0_ft, 0_ft, 0_ft, Rotation3d{0_deg, 0_deg, 0_deg}}},
+          AprilTag{
+              2, Pose3d{4_ft, 4_ft, 4_ft, Rotation3d{0_deg, 0_deg, 180_deg}}}},
+      54_ft, 27_ft};
+
+  layout.SetAlliance(DriverStation::Alliance::kRed);
+
+  auto mirrorPose =
+      Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}};
+  EXPECT_EQ(mirrorPose, *layout.GetTagPose(1));
+  mirrorPose = Pose3d{50_ft, 23_ft, 4_ft, Rotation3d{0_deg, 0_deg, 0_deg}};
+  EXPECT_EQ(mirrorPose, *layout.GetTagPose(2));
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java

@@ -17,6 +17,7 @@ import edu.wpi.first.networktables.NetworkTableListener;
 import edu.wpi.first.networktables.StringPublisher;
 import edu.wpi.first.networktables.Topic;
 import java.util.Collection;
+import java.util.EnumSet;
 
 /**
  * The preferences class provides a relatively simple way to save important values to the roboRIO to
@@ -77,7 +78,7 @@ public final class Preferences {
     m_listener =
         NetworkTableListener.createListener(
             m_tableSubscriber,
-            NetworkTableEvent.kImmediate | NetworkTableEvent.kPublish,
+            EnumSet.of(NetworkTableEvent.Kind.kImmediate, NetworkTableEvent.Kind.kPublish),
             event -> {
               if (event.topicInfo != null) {
                 Topic topic = event.topicInfo.getTopic();

wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java

@@ -155,7 +155,7 @@ public abstract class RobotBase implements AutoCloseable {
     // wait for the NT server to actually start
     try {
       int count = 0;
-      while ((inst.getNetworkMode() & NetworkTableInstance.kNetModeStarting) != 0) {
+      while (inst.getNetworkMode().contains(NetworkTableInstance.NetworkMode.kStarting)) {
         Thread.sleep(10);
         count++;
         if (count > 100) {

wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.apriltag;
+
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.geometry.Pose3d;
+import java.util.Objects;
+
+@SuppressWarnings("MemberName")
+public class AprilTag {
+  @JsonProperty(value = "ID")
+  public int ID;
+
+  @JsonProperty(value = "pose")
+  public Pose3d pose;
+
+  @SuppressWarnings("ParameterName")
+  @JsonCreator
+  public AprilTag(
+      @JsonProperty(required = true, value = "ID") int ID,
+      @JsonProperty(required = true, value = "pose") Pose3d pose) {
+    this.ID = ID;
+    this.pose = pose;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof AprilTag) {
+      var other = (AprilTag) obj;
+      return ID == other.ID && pose.equals(other.pose);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(ID, pose);
+  }
+
+  @Override
+  public String toString() {
+    return "AprilTag(ID: " + ID + ", pose: " + pose + ")";
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java

@@ -0,0 +1,187 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.apriltag;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.DriverStation;
+import java.io.IOException;
+import java.nio.file.Path;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Objects;
+import java.util.Optional;
+
+/**
+ * Class for representing a layout of AprilTags on a field and reading them from a JSON format.
+ *
+ * <p>The JSON format contains two top-level objects, "tags" and "field". The "tags" object is a
+ * list of all AprilTags contained within a layout. Each AprilTag serializes to a JSON object
+ * containing an ID and a Pose3d. The "field" object is a descriptor of the size of the field in
+ * meters with "width" and "height" values. This is to account for arbitrary field sizes when
+ * mirroring the poses.
+ *
+ * <p>Pose3ds are assumed to be measured from the bottom-left corner of the field, when the blue
+ * alliance is at the left. Pose3ds will automatically be returned as passed in when calling {@link
+ * AprilTagFieldLayout#getTagPose(int)}. Setting an alliance color via {@link
+ * AprilTagFieldLayout#setAlliance(DriverStation.Alliance)} will mirror the poses returned from
+ * {@link AprilTagFieldLayout#getTagPose(int)} to be correct relative to the other alliance.
+ */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class AprilTagFieldLayout {
+  @JsonProperty(value = "tags")
+  private final List<AprilTag> m_apriltags = new ArrayList<>();
+
+  @JsonProperty(value = "field")
+  private FieldDimensions m_fieldDimensions;
+
+  private boolean m_mirror;
+
+  /**
+   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
+   *
+   * @param path Path of the JSON file to import from.
+   * @throws IOException If reading from the file fails.
+   */
+  public AprilTagFieldLayout(String path) throws IOException {
+    this(Path.of(path));
+  }
+
+  /**
+   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
+   *
+   * @param path Path of the JSON file to import from.
+   * @throws IOException If reading from the file fails.
+   */
+  public AprilTagFieldLayout(Path path) throws IOException {
+    AprilTagFieldLayout layout =
+        new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class);
+    m_apriltags.addAll(layout.m_apriltags);
+    m_fieldDimensions = layout.m_fieldDimensions;
+  }
+
+  /**
+   * Construct a new AprilTagFieldLayout from a list of {@link AprilTag} objects.
+   *
+   * @param apriltags List of {@link AprilTag}.
+   * @param fieldLength Length of the field in meters.
+   * @param fieldWidth Width of the field in meters.
+   */
+  public AprilTagFieldLayout(List<AprilTag> apriltags, double fieldLength, double fieldWidth) {
+    this(apriltags, new FieldDimensions(fieldLength, fieldWidth));
+  }
+
+  @JsonCreator
+  private AprilTagFieldLayout(
+      @JsonProperty(required = true, value = "tags") List<AprilTag> apriltags,
+      @JsonProperty(required = true, value = "field") FieldDimensions fieldDimensions) {
+    // To ensure the underlying semantics don't change with what kind of list is passed in
+    m_apriltags.addAll(apriltags);
+    m_fieldDimensions = fieldDimensions;
+  }
+
+  /**
+   * Set the alliance that your team is on.
+   *
+   * <p>This changes the {@link #getTagPose(int)} method to return the correct pose for your
+   * alliance.
+   *
+   * @param alliance The alliance to mirror poses for.
+   */
+  public void setAlliance(DriverStation.Alliance alliance) {
+    m_mirror = alliance == DriverStation.Alliance.Red;
+  }
+
+  /**
+   * Gets an AprilTag pose by its ID.
+   *
+   * @param ID The ID of the tag.
+   * @return The pose corresponding to the ID passed in or an empty optional if a tag with that ID
+   *     was not found.
+   */
+  @SuppressWarnings("ParameterName")
+  public Optional<Pose3d> getTagPose(int ID) {
+    Pose3d pose = null;
+    for (AprilTag apriltag : m_apriltags) {
+      if (apriltag.ID == ID) {
+        pose = apriltag.pose;
+        break;
+      }
+    }
+    if (pose == null) {
+      return Optional.empty();
+    }
+    if (m_mirror) {
+      pose =
+          pose.relativeTo(
+              new Pose3d(
+                  new Translation3d(
+                      m_fieldDimensions.fieldWidth, m_fieldDimensions.fieldLength, 0.0),
+                  new Rotation3d(0.0, 0.0, Math.PI)));
+    }
+    return Optional.of(pose);
+  }
+
+  /**
+   * Serializes a AprilTagFieldLayout to a JSON file.
+   *
+   * @param path The path to write to.
+   * @throws IOException If writing to the file fails.
+   */
+  public void serialize(String path) throws IOException {
+    serialize(Path.of(path));
+  }
+
+  /**
+   * Serializes a AprilTagFieldLayout to a JSON file.
+   *
+   * @param path The path to write to.
+   * @throws IOException If writing to the file fails.
+   */
+  public void serialize(Path path) throws IOException {
+    new ObjectMapper().writeValue(path.toFile(), this);
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof AprilTagFieldLayout) {
+      var other = (AprilTagFieldLayout) obj;
+      return m_apriltags.equals(other.m_apriltags) && m_mirror == other.m_mirror;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_apriltags, m_mirror);
+  }
+
+  @JsonIgnoreProperties(ignoreUnknown = true)
+  @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+  private static class FieldDimensions {
+    @SuppressWarnings("MemberName")
+    @JsonProperty(value = "width")
+    public double fieldWidth;
+
+    @SuppressWarnings("MemberName")
+    @JsonProperty(value = "height")
+    public double fieldLength;
+
+    @JsonCreator()
+    FieldDimensions(
+        @JsonProperty(required = true, value = "width") double fieldWidth,
+        @JsonProperty(required = true, value = "height") double fieldLength) {
+      this.fieldWidth = fieldWidth;
+      this.fieldLength = fieldLength;
+    }
+  }
+}

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

@@ -48,10 +48,14 @@ public final class SuppliedValueWidget<T> extends ShuffleboardWidget<SuppliedVal
   @Override
   public void buildInto(NetworkTable parentTable, NetworkTable metaTable) {
     buildMetadata(metaTable);
-    m_controllablePub = new BooleanTopic(metaTable.getTopic("Controllable")).publish();
-    m_controllablePub.set(false);
+    if (m_controllablePub == null) {
+      m_controllablePub = new BooleanTopic(metaTable.getTopic("Controllable")).publish();
+      m_controllablePub.set(false);
+    }
 
-    m_entry = parentTable.getTopic(getTitle()).genericPublish(m_typeString);
+    if (m_entry == null) {
+      m_entry = parentTable.getTopic(getTitle()).genericPublish(m_typeString);
+    }
     m_setter.accept(m_entry, m_supplier.get());
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PS4ControllerSim.java

@@ -17,6 +17,7 @@ public class PS4ControllerSim extends GenericHIDSim {
     super(joystick);
     setAxisCount(6);
     setButtonCount(14);
+    setPOVCount(1);
   }
 
   /**
@@ -28,6 +29,7 @@ public class PS4ControllerSim extends GenericHIDSim {
     super(port);
     setAxisCount(6);
     setButtonCount(14);
+    setPOVCount(1);
   }
 
   /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java

@@ -177,7 +177,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
    * @return Whether the arm would hit the lower limit.
    */
   public boolean wouldHitLowerLimit(double currentAngleRads) {
-    return currentAngleRads < this.m_minAngle;
+    return currentAngleRads <= this.m_minAngle;
   }
 
   /**
@@ -187,7 +187,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
    * @return Whether the arm would hit the upper limit.
    */
   public boolean wouldHitUpperLimit(double currentAngleRads) {
-    return currentAngleRads > this.m_maxAngle;
+    return currentAngleRads >= this.m_maxAngle;
   }
 
   /**

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/XboxControllerSim.java

@@ -17,6 +17,7 @@ public class XboxControllerSim extends GenericHIDSim {
     super(joystick);
     setAxisCount(6);
     setButtonCount(10);
+    setPOVCount(1);
   }
 
   /**
@@ -28,6 +29,7 @@ public class XboxControllerSim extends GenericHIDSim {
     super(port);
     setAxisCount(6);
     setButtonCount(10);
+    setPOVCount(1);
   }
 
   /**

wpilibj/src/test/java/edu/wpi/first/wpilibj/PreferencesTest.java

@@ -54,7 +54,7 @@ class PreferencesTest {
     m_inst.startServer(filepath.toString(), "", 0, 0);
     try {
       int count = 0;
-      while ((m_inst.getNetworkMode() & NetworkTableInstance.kNetModeStarting) != 0) {
+      while (m_inst.getNetworkMode().contains(NetworkTableInstance.NetworkMode.kStarting)) {
         Thread.sleep(100);
         count++;
         if (count > 30) {

wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.apriltag;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class AprilTagPoseMirroringTest {
+  @Test
+  void poseMirroring() {
+    var layout =
+        new AprilTagFieldLayout(
+            List.of(
+                new AprilTag(1, new Pose3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0))),
+                new AprilTag(
+                    2,
+                    new Pose3d(
+                        new Translation3d(
+                            Units.feetToMeters(4.0), Units.feetToMeters(4), Units.feetToMeters(4)),
+                        new Rotation3d(0, 0, Units.degreesToRadians(180))))),
+            Units.feetToMeters(54.0),
+            Units.feetToMeters(27.0));
+    layout.setAlliance(DriverStation.Alliance.Red);
+
+    assertEquals(
+        new Pose3d(
+            new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(180.0))),
+        layout.getTagPose(1).orElse(null));
+
+    assertEquals(
+        new Pose3d(
+            new Translation3d(
+                Units.feetToMeters(50.0), Units.feetToMeters(23.0), Units.feetToMeters(4)),
+            new Rotation3d(0.0, 0.0, 0)),
+        layout.getTagPose(2).orElse(null));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.apriltag;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.util.Units;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class AprilTagSerializationTest {
+  @Test
+  void deserializeMatches() {
+    var layout =
+        new AprilTagFieldLayout(
+            List.of(
+                new AprilTag(1, new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0))),
+                new AprilTag(3, new Pose3d(0, 1, 0, new Rotation3d(0, 0, 0)))),
+            Units.feetToMeters(54.0),
+            Units.feetToMeters(27.0));
+
+    var objectMapper = new ObjectMapper();
+
+    var deserialized =
+        assertDoesNotThrow(
+            () ->
+                objectMapper.readValue(
+                    objectMapper.writeValueAsString(layout), AprilTagFieldLayout.class));
+
+    assertEquals(layout, deserialized);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java

@@ -49,7 +49,8 @@ import java.util.function.BiConsumer;
  * left, front right, rear left, and rear right wheels.
  *
  * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
- * heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
+ * heading; or <strong> y = [theta, s_fl, s_fr, s_rl, s_rr]ᵀ </strong> containing gyro heading,
+ * followed by the distance driven by the front left, front right, rear left, and rear right wheels.
  */
 public class MecanumDrivePoseEstimator {
   private final UnscentedKalmanFilter<N7, N7, N5> m_observer;

wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java

@@ -43,14 +43,15 @@ import java.util.function.BiConsumer;
  * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
  * (y):
  *
- * <p><strong> x = [x, y, theta, s_0, ... s_n]ᵀ </strong> in the field coordinate system containing
+ * <p><strong> x = [x, y, theta, s_0, ..., s_n]ᵀ </strong> in the field coordinate system containing
  * x position, y position, and heading, followed by the distance travelled by each wheel.
  *
  * <p><strong> u = [v_x, v_y, omega, v_0, ... v_n]ᵀ </strong> containing x velocity, y velocity, and
  * angular rate in the field coordinate system, followed by the velocity measured at each wheel.
  *
  * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
- * heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
+ * heading; or <strong> y = [theta, s_0, ..., s_n]ᵀ </strong> containing gyro heading, followed by
+ * the distance travelled by each wheel.
  */
 public class SwerveDrivePoseEstimator<States extends Num, Inputs extends Num, Outputs extends Num> {
   private final UnscentedKalmanFilter<States, Inputs, Outputs> m_observer;
@@ -243,6 +244,8 @@ public class SwerveDrivePoseEstimator<States extends Num, Inputs extends Num, Ou
   /**
    * Resets the robot's position on the field.
    *
+   * <p>You NEED to reset your encoders (to zero) when calling this method.
+   *
    * <p>The gyroscope angle does not need to be reset in the user's robot code. The library
    * automatically takes care of offsetting the gyro angle.
    *

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java

@@ -4,6 +4,10 @@
 
 package edu.wpi.first.math.geometry;
 
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -14,6 +18,8 @@ import edu.wpi.first.math.numbers.N3;
 import java.util.Objects;
 
 /** Represents a 3D pose containing translational and rotational elements. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Pose3d implements Interpolatable<Pose3d> {
   private final Translation3d m_translation;
   private final Rotation3d m_rotation;
@@ -30,7 +36,10 @@ public class Pose3d implements Interpolatable<Pose3d> {
    * @param translation The translational component of the pose.
    * @param rotation The rotational component of the pose.
    */
-  public Pose3d(Translation3d translation, Rotation3d rotation) {
+  @JsonCreator
+  public Pose3d(
+      @JsonProperty(required = true, value = "translation") Translation3d translation,
+      @JsonProperty(required = true, value = "rotation") Rotation3d rotation) {
     m_translation = translation;
     m_rotation = rotation;
   }
@@ -84,6 +93,7 @@ public class Pose3d implements Interpolatable<Pose3d> {
    *
    * @return The translational component of the pose.
    */
+  @JsonProperty
   public Translation3d getTranslation() {
     return m_translation;
   }
@@ -120,6 +130,7 @@ public class Pose3d implements Interpolatable<Pose3d> {
    *
    * @return The rotational component of the pose.
    */
+  @JsonProperty
   public Rotation3d getRotation() {
     return m_rotation;
   }

wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java

@@ -4,11 +4,17 @@
 
 package edu.wpi.first.math.geometry;
 
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.numbers.N3;
 import java.util.Objects;
 
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Quaternion {
   private final double m_r;
   private final Vector<N3> m_v;
@@ -27,7 +33,12 @@ public class Quaternion {
    * @param y Y component of the quaternion.
    * @param z Z component of the quaternion.
    */
-  public Quaternion(double w, double x, double y, double z) {
+  @JsonCreator
+  public Quaternion(
+      @JsonProperty(required = true, value = "W") double w,
+      @JsonProperty(required = true, value = "X") double x,
+      @JsonProperty(required = true, value = "Y") double y,
+      @JsonProperty(required = true, value = "Z") double z) {
     m_r = w;
     m_v = VecBuilder.fill(x, y, z);
   }
@@ -113,6 +124,7 @@ public class Quaternion {
    *
    * @return W component of the quaternion.
    */
+  @JsonProperty(value = "W")
   public double getW() {
     return m_r;
   }
@@ -122,6 +134,7 @@ public class Quaternion {
    *
    * @return X component of the quaternion.
    */
+  @JsonProperty(value = "X")
   public double getX() {
     return m_v.get(0, 0);
   }
@@ -131,6 +144,7 @@ public class Quaternion {
    *
    * @return Y component of the quaternion.
    */
+  @JsonProperty(value = "Y")
   public double getY() {
     return m_v.get(1, 0);
   }
@@ -140,6 +154,7 @@ public class Quaternion {
    *
    * @return Z component of the quaternion.
    */
+  @JsonProperty(value = "Z")
   public double getZ() {
     return m_v.get(2, 0);
   }

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java

@@ -4,6 +4,10 @@
 
 package edu.wpi.first.math.geometry;
 
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUtil;
@@ -17,6 +21,8 @@ import java.util.Objects;
 import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
 
 /** A rotation in a 3D coordinate frame represented by a quaternion. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation3d implements Interpolatable<Rotation3d> {
   private Quaternion m_q = new Quaternion();
 
@@ -28,7 +34,8 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
    *
    * @param q The quaternion.
    */
-  public Rotation3d(Quaternion q) {
+  @JsonCreator
+  public Rotation3d(@JsonProperty(required = true, value = "quaternion") Quaternion q) {
     m_q = q.normalize();
   }
 
@@ -270,6 +277,7 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
    *
    * @return The quaternion representation of the Rotation3d.
    */
+  @JsonProperty(value = "quaternion")
   public Quaternion getQuaternion() {
     return m_q;
   }

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java

@@ -4,6 +4,10 @@
 
 package edu.wpi.first.math.geometry;
 
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import java.util.Objects;
@@ -15,6 +19,8 @@ import java.util.Objects;
  * origin facing in the positive X direction, forward is positive X, left is positive Y, and up is
  * positive Z.
  */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Translation3d implements Interpolatable<Translation3d> {
   private final double m_x;
   private final double m_y;
@@ -32,7 +38,11 @@ public class Translation3d implements Interpolatable<Translation3d> {
    * @param y The y component of the translation.
    * @param z The z component of the translation.
    */
-  public Translation3d(double x, double y, double z) {
+  @JsonCreator
+  public Translation3d(
+      @JsonProperty(required = true, value = "x") double x,
+      @JsonProperty(required = true, value = "y") double y,
+      @JsonProperty(required = true, value = "z") double z) {
     m_x = x;
     m_y = y;
     m_z = z;
@@ -70,6 +80,7 @@ public class Translation3d implements Interpolatable<Translation3d> {
    *
    * @return The X component of the translation.
    */
+  @JsonProperty
   public double getX() {
     return m_x;
   }
@@ -79,6 +90,7 @@ public class Translation3d implements Interpolatable<Translation3d> {
    *
    * @return The Y component of the translation.
    */
+  @JsonProperty
   public double getY() {
     return m_y;
   }
@@ -88,6 +100,7 @@ public class Translation3d implements Interpolatable<Translation3d> {
    *
    * @return The Z component of the translation.
    */
+  @JsonProperty
   public double getZ() {
     return m_z;
   }

wpimath/src/main/native/cpp/geometry/Pose3d.cpp

@@ -6,6 +6,8 @@
 
 #include <cmath>
 
+#include <wpi/json.h>
+
 using namespace frc;
 
 namespace {
@@ -149,3 +151,13 @@ Twist3d Pose3d::Log(const Pose3d& end) const {
 Pose2d Pose3d::ToPose2d() const {
   return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
 }
+
+void frc::to_json(wpi::json& json, const Pose3d& pose) {
+  json = wpi::json{{"translation", pose.Translation()},
+                   {"rotation", pose.Rotation()}};
+}
+
+void frc::from_json(const wpi::json& json, Pose3d& pose) {
+  pose = Pose3d{json.at("translation").get<Translation3d>(),
+                json.at("rotation").get<Rotation3d>()};
+}

wpimath/src/main/native/cpp/geometry/Quaternion.cpp

@@ -4,6 +4,8 @@
 
 #include "frc/geometry/Quaternion.h"
 
+#include <wpi/json.h>
+
 using namespace frc;
 
 Quaternion::Quaternion(double w, double x, double y, double z)
@@ -81,3 +83,16 @@ Eigen::Vector3d Quaternion::ToRotationVector() const {
     }
   }
 }
+
+void frc::to_json(wpi::json& json, const Quaternion& quaternion) {
+  json = wpi::json{{"W", quaternion.W()},
+                   {"X", quaternion.X()},
+                   {"Y", quaternion.Y()},
+                   {"Z", quaternion.Z()}};
+}
+
+void frc::from_json(const wpi::json& json, Quaternion& quaternion) {
+  quaternion =
+      Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
+                 json.at("Y").get<double>(), json.at("Z").get<double>()};
+}

wpimath/src/main/native/cpp/geometry/Rotation3d.cpp

@@ -7,6 +7,8 @@
 #include <cmath>
 #include <numbers>
 
+#include <wpi/json.h>
+
 #include "Eigen/Core"
 #include "Eigen/LU"
 #include "Eigen/QR"
@@ -238,3 +240,11 @@ units::radian_t Rotation3d::Angle() const {
 Rotation2d Rotation3d::ToRotation2d() const {
   return Rotation2d{Z()};
 }
+
+void frc::to_json(wpi::json& json, const Rotation3d& rotation) {
+  json = wpi::json{{"quaternion", rotation.GetQuaternion()}};
+}
+
+void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
+  rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
+}

wpimath/src/main/native/cpp/geometry/Translation3d.cpp

@@ -4,6 +4,11 @@
 
 #include "frc/geometry/Translation3d.h"
 
+#include <wpi/json.h>
+
+#include "units/length.h"
+#include "units/math.h"
+
 using namespace frc;
 
 Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
@@ -39,3 +44,15 @@ bool Translation3d::operator==(const Translation3d& other) const {
 bool Translation3d::operator!=(const Translation3d& other) const {
   return !operator==(other);
 }
+
+void frc::to_json(wpi::json& json, const Translation3d& translation) {
+  json = wpi::json{{"x", translation.X().value()},
+                   {"y", translation.Y().value()},
+                   {"z", translation.Z().value()}};
+}
+
+void frc::from_json(const wpi::json& json, Translation3d& translation) {
+  translation = Translation3d{units::meter_t{json.at("x").get<double>()},
+                              units::meter_t{json.at("y").get<double>()},
+                              units::meter_t{json.at("z").get<double>()}};
+}

wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h

@@ -6,6 +6,7 @@
 
 #include <wpi/MathExtras.h>
 
+#include "units/length.h"
 #include "units/time.h"
 #include "units/voltage.h"
 
@@ -14,9 +15,9 @@ namespace frc {
  * A helper class that computes feedforward outputs for a simple elevator
  * (modeled as a motor acting against the force of gravity).
  */
-template <class Distance>
 class ElevatorFeedforward {
  public:
+  using Distance = units::meters;
   using Velocity =
       units::compound_unit<Distance, units::inverse<units::seconds>>;
   using Acceleration =

wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h

@@ -46,8 +46,9 @@ namespace frc {
  * right wheels.
  *
  * <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
- * position, and heading; or <strong> y = [theta]ᵀ </strong> containing gyro
- * heading.
+ * position, and heading; or <strong> y = [theta, s_fl, s_fr, s_rl, s_rr]ᵀ
+ * </strong> containing gyro heading, followed by the distance driven by the
+ * front left, front right, rear left, and rear right wheels.
  */
 class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
  public:

wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h

@@ -38,7 +38,7 @@ namespace frc {
  * The state-space system used internally has the following states (x), inputs
  * (u), and outputs (y):
  *
- * <strong> x = [x, y, theta, s_0, ... s_n]ᵀ </strong> in the field coordinate
+ * <strong> x = [x, y, theta, s_0, ..., s_n]ᵀ </strong> in the field coordinate
  * system containing x position, y position, and heading, followed by the
  * distance travelled by each wheel.
  *
@@ -47,8 +47,8 @@ namespace frc {
  * followed by the velocity measured at each wheel.
  *
  * <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
- * position, and heading; or <strong> y = [theta]ᵀ </strong> containing gyro
- * heading.
+ * position, and heading; or <strong> y = [theta, s_0, ..., s_n]ᵀ </strong>
+ * containing gyro heading, followed by the distance travelled by each wheel.
  */
 template <size_t NumModules>
 class SwerveDrivePoseEstimator {
@@ -138,6 +138,8 @@ class SwerveDrivePoseEstimator {
   /**
    * Resets the robot's position on the field.
    *
+   * You NEED to reset your encoders (to zero) when calling this method.
+   *
    * The gyroscope angle does not need to be reset in the user's robot code.
    * The library automatically takes care of offsetting the gyro angle.
    *

wpimath/src/main/native/include/frc/filter/Debouncer.h

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
 #include <wpi/timestamp.h>
 
 #include "units/time.h"

wpimath/src/main/native/include/frc/geometry/Pose3d.h

@@ -11,6 +11,10 @@
 #include "Translation3d.h"
 #include "Twist3d.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 /**
@@ -202,4 +206,10 @@ class WPILIB_DLLEXPORT Pose3d {
   Rotation3d m_rotation;
 };
 
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Pose3d& pose);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Pose3d& pose);
+
 }  // namespace frc

wpimath/src/main/native/include/frc/geometry/Quaternion.h

@@ -8,6 +8,10 @@
 
 #include "frc/EigenCore.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 class WPILIB_DLLEXPORT Quaternion {
@@ -92,4 +96,10 @@ class WPILIB_DLLEXPORT Quaternion {
   Eigen::Vector3d m_v{0.0, 0.0, 0.0};
 };
 
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Quaternion& quaternion);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Quaternion& quaternion);
+
 }  // namespace frc

wpimath/src/main/native/include/frc/geometry/Rotation3d.h

@@ -11,6 +11,10 @@
 #include "frc/EigenCore.h"
 #include "units/angle.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 /**
@@ -180,4 +184,10 @@ class WPILIB_DLLEXPORT Rotation3d {
   Quaternion m_q;
 };
 
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Rotation3d& rotation);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Rotation3d& rotation);
+
 }  // namespace frc

wpimath/src/main/native/include/frc/geometry/Translation3d.h

@@ -10,6 +10,10 @@
 #include "Translation2d.h"
 #include "units/length.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 /**
@@ -182,6 +186,12 @@ class WPILIB_DLLEXPORT Translation3d {
   units::meter_t m_z = 0_m;
 };
 
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Translation3d& state);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Translation3d& state);
+
 }  // namespace frc
 
 #include "Translation3d.inc"

wpimath/src/main/native/include/units/base.h

@@ -84,8 +84,6 @@
 	#include <fmt/format.h>
 #endif
 
-#include <wpi/SymbolExports.h>
-
 //------------------------------
 //	STRING FORMATTER
 //------------------------------

wpiutil/src/main/native/thirdparty/mpack/src/mpack.cpp

@@ -237,6 +237,7 @@ void* mpack_realloc(void* old_ptr, size_t used_size, size_t new_size) {
 }
 #endif
 
+}  // namespace mpack
 MPACK_SILENCE_WARNINGS_END
 
 /* mpack/mpack-common.c.c */
@@ -246,6 +247,7 @@ MPACK_SILENCE_WARNINGS_END
 /* #include "mpack-common.h" */
 
 MPACK_SILENCE_WARNINGS_BEGIN
+namespace mpack {
 
 const char* mpack_error_to_string(mpack_error_t error) {
     #if MPACK_STRINGS
@@ -970,6 +972,7 @@ void mpack_print_file_callback(void* context, const char* data, size_t count) {
 }
 #endif
 
+}  // namespace mpack
 MPACK_SILENCE_WARNINGS_END
 
 /* mpack/mpack-writer.c.c */
@@ -979,6 +982,7 @@ MPACK_SILENCE_WARNINGS_END
 /* #include "mpack-writer.h" */
 
 MPACK_SILENCE_WARNINGS_BEGIN
+namespace mpack {
 
 #if MPACK_WRITER
 
@@ -2727,6 +2731,7 @@ void mpack_complete_array(mpack_writer_t* writer) {
 #endif // MPACK_BUILDER
 #endif // MPACK_WRITER
 
+}  // namespace mpack
 MPACK_SILENCE_WARNINGS_END
 
 /* mpack/mpack-reader.c.c */
@@ -2736,6 +2741,7 @@ MPACK_SILENCE_WARNINGS_END
 /* #include "mpack-reader.h" */
 
 MPACK_SILENCE_WARNINGS_BEGIN
+namespace mpack {
 
 #if MPACK_READER
 
@@ -3996,6 +4002,7 @@ void mpack_print_stdfile_to_callback(FILE* file, mpack_print_callback_t callback
 
 #endif
 
+}  // namespace mpack
 MPACK_SILENCE_WARNINGS_END
 
 /* mpack/mpack-expect.c.c */
@@ -4005,6 +4012,7 @@ MPACK_SILENCE_WARNINGS_END
 /* #include "mpack-expect.h" */
 
 MPACK_SILENCE_WARNINGS_BEGIN
+namespace mpack {
 
 #if MPACK_EXPECT
 
@@ -4861,6 +4869,7 @@ size_t mpack_expect_key_cstr(mpack_reader_t* reader, const char* keys[], bool fo
 
 #endif
 
+}  // namespace mpack
 MPACK_SILENCE_WARNINGS_END
 
 /* mpack/mpack-node.c.c */
@@ -4870,6 +4879,7 @@ MPACK_SILENCE_WARNINGS_END
 /* #include "mpack-node.h" */
 
 MPACK_SILENCE_WARNINGS_BEGIN
+namespace mpack {
 
 #if MPACK_NODE
 
@@ -7247,5 +7257,5 @@ mpack_node_t mpack_node_map_value_at(mpack_node_t node, size_t index) {
 
 #endif
 
-}  // namespace wpi
+}  // namespace mpack
 MPACK_SILENCE_WARNINGS_END