[wpilib] Update values on controllable sendables (#4667)

Also don't save duplicate NT sets to data log (unless publish keep duplicates flag is set).

.github/workflows/cmake.yml

@@ -12,7 +12,7 @@ jobs:
       fail-fast: false
       matrix:
         include:
-          - os: ubuntu-latest
+          - os: ubuntu-22.04
             name: Linux
             container: wpilib/roborio-cross-ubuntu:2023-22.04
             flags: ""

.github/workflows/comment-command.yml

@@ -6,7 +6,7 @@ on:
 jobs:
   wpiformat:
     if: github.event.issue.pull_request && startsWith(github.event.comment.body, '/wpiformat')
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
       - name: React Rocket
         uses: actions/github-script@v4
@@ -37,16 +37,23 @@ jobs:
         uses: actions/setup-python@v2
         with:
           python-version: 3.8
+      - name: Setup Java
+        uses: actions/setup-java@v3
+        with:
+          distribution: 'zulu'
+          java-version: 11
       - name: Install clang-format
         run: |
           wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key | sudo apt-key add -
-          sudo sh -c "echo 'deb http://apt.llvm.org/focal/ llvm-toolchain-focal-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
+          sudo sh -c "echo 'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
           sudo apt-get update -q
           sudo apt-get install -y clang-format-14
       - name: Install wpiformat
         run: pip3 install wpiformat
       - name: Run wpiformat
         run: wpiformat -clang 14
+      - name: Run spotlessApply
+        run: ./gradlew spotlessApply
       - name: Commit
         run: |
           # Set credentials

.github/workflows/documentation.yml

@@ -12,7 +12,7 @@ env:
 jobs:
   publish:
     name: "Documentation - Publish"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     if: github.repository_owner == 'wpilibsuite' && (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
     concurrency: ci-docs-publish
     steps:
@@ -24,8 +24,6 @@ jobs:
         with:
           distribution: 'zulu'
           java-version: 13
-      - name: Install libclang-9
-        run: sudo apt update && sudo apt install -y libclang-cpp9 libclang1-9
       - name: Set environment variables (Development)
         run: |
           echo "TARGET_FOLDER=$BASE_PATH/development" >> $GITHUB_ENV

.github/workflows/gazebo.yml

@@ -9,7 +9,7 @@ concurrency:
 jobs:
   build:
     name: "Build"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     container: wpilib/gazebo-ubuntu:22.04
     steps:
       - uses: actions/checkout@v3

.github/workflows/gradle.yml

@@ -25,7 +25,7 @@ jobs:
             artifact-name: Linux
             build-options: "-Ponlylinuxx86-64"
     name: "Build - ${{ matrix.artifact-name }}"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     container: ${{ matrix.container }}
     steps:
       - uses: actions/checkout@v3
@@ -66,7 +66,6 @@ jobs:
           - os: macOS-11
             artifact-name: macOS
             architecture: x64
-            build-options: "-Pbuildalldesktop"
             task: "build"
             outputs: "build/allOutputs"
           - os: windows-2022
@@ -112,7 +111,7 @@ jobs:
           ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
           ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
       - name: Sign Libraries with Developer ID
-        run: ./gradlew build -PbuildServer -PskipJavaFormat -PdeveloperID=${{ secrets.APPLE_DEVELOPER_ID }} ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
+        run: ./gradlew copyAllOutputs --build-cache -PbuildServer -PskipJavaFormat -PdeveloperID=${{ secrets.APPLE_DEVELOPER_ID }} ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
         if: |
           matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
           (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v')))
@@ -123,7 +122,7 @@ jobs:
 
   build-documentation:
     name: "Build - Documentation"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v3
         with:
@@ -132,8 +131,6 @@ jobs:
         with:
           distribution: 'zulu'
           java-version: 13
-      - name: Install libclang-9
-        run: sudo apt update && sudo apt install -y libclang-cpp9 libclang1-9
       - name: Set release environment variable
         run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
         if: startsWith(github.ref, 'refs/tags/v')
@@ -150,7 +147,7 @@ jobs:
   combine:
     name: Combine
     needs: [build-docker, build-host, build-documentation]
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v3
         with:

.github/workflows/lint-format.yml

@@ -13,7 +13,7 @@ concurrency:
 jobs:
   wpiformat:
     name: "wpiformat"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v3
       - name: Fetch all history and metadata
@@ -29,7 +29,7 @@ jobs:
       - name: Install clang-format
         run: |
           wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key | sudo apt-key add -
-          sudo sh -c "echo 'deb http://apt.llvm.org/focal/ llvm-toolchain-focal-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
+          sudo sh -c "echo 'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
           sudo apt-get update -q
           sudo apt-get install -y clang-format-14
       - name: Install wpiformat
@@ -56,7 +56,7 @@ jobs:
 
   tidy:
     name: "clang-tidy"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     container: wpilib/roborio-cross-ubuntu:2023-22.04
     steps:
       - uses: actions/checkout@v3
@@ -73,7 +73,7 @@ jobs:
       - name: Install clang-tidy
         run: |
           wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key | sudo apt-key add -
-          sudo sh -c "echo 'deb http://apt.llvm.org/focal/ llvm-toolchain-focal-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
+          sudo sh -c "echo 'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
           sudo apt-get update -q
           sudo apt-get install -y clang-tidy-14 clang-format-14
       - name: Install wpiformat
@@ -86,7 +86,7 @@ jobs:
         run: wpiformat -clang 14 -no-format -tidy-changed -compile-commands=build/compile_commands/linuxx86-64 -vv
   javaformat:
     name: "Java format"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     container: wpilib/ubuntu-base:22.04
     steps:
       - uses: actions/checkout@v3
@@ -105,7 +105,7 @@ jobs:
         if: ${{ failure() }}
   documentation:
     name: "Documentation"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v3
         with:
@@ -114,7 +114,5 @@ jobs:
         with:
           distribution: 'zulu'
           java-version: 13
-      - name: Install libclang-9
-        run: sudo apt update && sudo apt install -y libclang-cpp9 libclang1-9
       - name: Build with Gradle
         run: ./gradlew docs:zipDocs -PbuildServer -PdocWarningsAsErrors ${{ env.EXTRA_GRADLE_ARGS }}

.github/workflows/sanitizers.yml

@@ -25,7 +25,7 @@ jobs:
             ctest-env: ""
             ctest-flags: ""
     name: "${{ matrix.name }}"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     container: wpilib/roborio-cross-ubuntu:2023-22.04
     steps:
       - uses: actions/checkout@v3

.github/workflows/upstream-utils.yml

@@ -13,7 +13,7 @@ concurrency:
 jobs:
   update:
     name: "Update"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v3
       - name: Fetch all history and metadata

CMakeLists.txt

@@ -315,6 +315,7 @@ endif()
 
 if (WITH_WPILIB)
     set(WPILIBC_DEP_REPLACE ${WPILIBC_DEP_REPLACE_IMPL})
+    add_subdirectory(apriltag)
     add_subdirectory(wpilibj)
     add_subdirectory(wpilibc)
     add_subdirectory(wpilibNewCommands)

DevelopmentBuilds.md

@@ -8,7 +8,7 @@ This article contains instructions on building projects using a development buil
 
 Development builds are the per-commit build hosted every time a commit is pushed to the [allwpilib](https://github.com/wpilibsuite/allwpilib/) repository. These builds are then hosted on [artifactory](https://frcmaven.wpi.edu/artifactory/webapp/#/home).
 
-In order to build a project using a development build, find the build.gradle file and open it. Then, add the following code below the plugin section and replace YEAR with the year of the development version. It is also necessary to use a 2023 GradleRIO version, ie `2023.0.0-alpha-1`
+To build a project using a development build, find the build.gradle file and open it. Then, add the following code below the plugin section and replace YEAR with the year of the development version. It is also necessary to use a 2023 GradleRIO version, ie `2023.0.0-alpha-1`
 
 ```groovy
 wpi.maven.useLocal = false
@@ -46,6 +46,11 @@ wpi.versions.wpilibVersion = '2023.+'
 wpi.versions.wpimathVersion = '2023.+'
 ```
 
+### Development Build Documentation
+
+* C++: https://github.wpilib.org/allwpilib/docs/development/cpp/
+* Java: https://github.wpilib.org/allwpilib/docs/development/java/
+
 ## Local Build
 
 Building with a local build is very similar to building with a development build. Ensure you have built and published WPILib by following the instructions attached [here](https://github.com/wpilibsuite/allwpilib#building-wpilib). Next, find the ``build.gradle`` file in your robot project and open it. Then, add the following code below the plugin section and replace ``YEAR`` with the year of the local version.

MavenArtifacts.md

@@ -72,12 +72,16 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
 * wpigui
   * imgui
 
-* ntcore
-  * wpiutil
-
 * wpimath
   * wpiutil
 
+* wpinet
+  * wpiutil
+
+* ntcore
+  * wpiutil
+  * wpinet
+
 * glass/libglass
   * wpiutil
   * wpimath
@@ -85,6 +89,7 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
 
 * glass/libglassnt
   * wpiutil
+  * wpinet
   * ntcore
   * wpimath
   * wpigui
@@ -94,6 +99,7 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
 
 * halsim
   * wpiutil
+  * wpinet
   * ntcore
   * wpimath
   * wpigui
@@ -102,12 +108,14 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
 
 * cscore
   * opencv
+  * wpinet
   * wpiutil
 
 * cameraserver
   * ntcore
   * cscore
   * opencv
+  * wpinet
   * wpiutil
 
 * wpilibj
@@ -115,6 +123,7 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
   * cameraserver
   * ntcore
   * cscore
+  * wpinet
   * wpiutil
 
 * wpilibc
@@ -123,6 +132,7 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
   * ntcore
   * cscore
   * wpimath
+  * wpinet
   * wpiutil
 
 * wpilibNewCommands
@@ -132,6 +142,7 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
   * ntcore
   * cscore
   * wpimath
+  * wpinet
   * wpiutil
 
 

README.md

@@ -1,8 +1,8 @@
 # WPILib Project
 
 ![CI](https://github.com/wpilibsuite/allwpilib/workflows/CI/badge.svg)
-[![C++ Documentation](https://img.shields.io/badge/documentation-c%2B%2B-blue)](https://first.wpi.edu/wpilib/allwpilib/docs/development/cpp/)
-[![Java Documentation](https://img.shields.io/badge/documentation-java-orange)](https://first.wpi.edu/wpilib/allwpilib/docs/development/java/)
+[![C++ Documentation](https://img.shields.io/badge/documentation-c%2B%2B-blue)](https://github.wpilib.org/allwpilib/docs/development/cpp/)
+[![Java Documentation](https://img.shields.io/badge/documentation-java-orange)](https://github.wpilib.org/allwpilib/docs/development/java/)
 
 Welcome to the WPILib project. This repository contains the HAL, WPILibJ, and WPILibC projects. These are the core libraries for creating robot programs for the roboRIO.
 
@@ -33,7 +33,7 @@ Below is a list of instructions that guide you through cloning, building, publis
 1. Clone the repository with `git clone https://github.com/wpilibsuite/allwpilib.git`
 2. Build the repository with `./gradlew build` or `./gradlew build --build-cache` if you have an internet connection
 3. Publish the artifacts locally by running `./gradlew publish`
-4. [Update your](OtherVersions.md) `build.gradle` [to use the artifacts](OtherVersions.md)
+4. [Update your](DevelopmentBuilds.md) `build.gradle` [to use the artifacts](DevelopmentBuilds.md)
 
 # Building WPILib
 
@@ -104,7 +104,7 @@ Run with `--build-cache` on the command-line to use the shared [build cache](htt
 
 ### Using Development Builds
 
-Please read the documentation available [here](OtherVersions.md)
+Please read the documentation available [here](DevelopmentBuilds.md)
 
 ### Custom toolchain location
 

apriltag/CMakeLists.txt

@@ -0,0 +1,41 @@
+project(fieldImages)
+
+include(CompileWarnings)
+include(GenResources)
+
+if (WITH_JAVA)
+     find_package(Java REQUIRED)
+     include(UseJava)
+
+     file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
+
+     set(CMAKE_JAVA_INCLUDE_PATH apriltags.jar ${JACKSON_JARS})
+
+     file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+     file(GLOB_RECURSE JAVA_RESOURCES src/main/native/resources/*.json)
+     add_jar(apriltags_jar SOURCES ${JAVA_SOURCES} RESOURCES NAMESPACE "edu/wpi/first/apriltag" ${JAVA_RESOURCES} INCLUDE_JARS wpimath_jar OUTPUT_NAME apriltag)
+endif()
+
+
+GENERATE_RESOURCES(src/main/native/resources/edu/wpi/first/apriltag generated/main/cpp APRILTAG frc apriltags_resources_src)
+
+file(GLOB_RECURSE apriltags_native_src src/main/native/cpp/*.cpp)
+
+add_library(apriltags ${apriltags_native_src} ${apriltags_resources_src})
+set_target_properties(apriltags PROPERTIES DEBUG_POSTFIX "d")
+
+set_property(TARGET apriltags PROPERTY FOLDER "libraries")
+target_compile_features(apriltags PUBLIC cxx_std_20)
+wpilib_target_warnings(apriltags)
+target_link_libraries(apriltags wpimath)
+
+target_include_directories(apriltags PUBLIC
+                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+                            $<INSTALL_INTERFACE:${include_dest}/apriltags>)
+
+
+if (WITH_TESTS)
+    wpilib_add_test(apriltags src/test/native/cpp)
+    target_include_directories(apriltags_test PRIVATE src/test/native/include)
+    target_link_libraries(apriltags_test apriltags gmock_main)
+endif()

apriltag/build.gradle

@@ -0,0 +1,77 @@
+apply from: "${rootDir}/shared/resources.gradle"
+
+ext {
+    nativeName = 'apriltags'
+    devMain = 'edu.wpi.first.apriltag.DevMain'
+
+    def generateTask = createGenerateResourcesTask('main', 'APRILTAGS', 'frc', project)
+
+    tasks.withType(CppCompile) {
+        dependsOn generateTask
+    }
+    extraSetup = {
+        it.sources {
+            resourcesCpp(CppSourceSet) {
+                source {
+                    srcDirs "$buildDir/generated/main/cpp", "$rootDir/shared/singlelib"
+                    include '*.cpp'
+                }
+            }
+        }
+    }
+}
+
+evaluationDependsOn(':wpimath')
+
+apply from: "${rootDir}/shared/javacpp/setupBuild.gradle"
+
+dependencies {
+    implementation project(':wpimath')
+}
+
+sourceSets {
+    main {
+        resources {
+            srcDirs 'src/main/native/resources'
+        }
+    }
+}
+
+
+model {
+    components {}
+    binaries {
+        all {
+            if (!it.buildable || !(it instanceof NativeBinarySpec)) {
+                return
+            }
+            it.cppCompiler.define 'WPILIB_EXPORTS'
+
+            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+
+            if ((it instanceof NativeExecutableBinarySpec || it instanceof GoogleTestTestSuiteBinarySpec) && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
+            }
+        }
+    }
+    tasks {
+        def c = $.components
+        def found = false
+        def systemArch = getCurrentArch()
+        c.each {
+            if (it in NativeExecutableSpec && it.name == "${nativeName}Dev") {
+                it.binaries.each {
+                    if (!found) {
+                        def arch = it.targetPlatform.name
+                        if (arch == systemArch) {
+                            def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib'
+
+                            found = true
+                        }
+                    }
+                }
+            }
+        }
+    }
+}

apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.java

@@ -0,0 +1,14 @@
+// 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.apriltag;
+
+public final class DevMain {
+  /** Main entry point. */
+  public static void main(String[] args) {
+    System.out.println("Hello World!");
+  }
+
+  private DevMain() {}
+}

apriltag/src/dev/native/cpp/main.cpp

@@ -0,0 +1,5 @@
+// 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.
+
+int main() {}

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

@@ -2,7 +2,7 @@
 // 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;
+package edu.wpi.first.apriltag;
 
 import com.fasterxml.jackson.annotation.JsonCreator;
 import com.fasterxml.jackson.annotation.JsonProperty;

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

@@ -2,21 +2,25 @@
 // 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;
+package edu.wpi.first.apriltag;
 
 import com.fasterxml.jackson.annotation.JsonAutoDetect;
 import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnore;
 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.io.InputStream;
+import java.io.InputStreamReader;
 import java.nio.file.Path;
 import java.util.ArrayList;
+import java.util.HashMap;
 import java.util.List;
+import java.util.Map;
 import java.util.Objects;
 import java.util.Optional;
 
@@ -26,25 +30,29 @@ import java.util.Optional;
  * <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.
+ * meters with "width" and "length" values. This is to account for arbitrary field sizes when
+ * transforming 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.
+ * alliance is at the left. By default, Pose3ds will be returned as declared when calling {@link
+ * AprilTagFieldLayout#getTagPose(int)}. {@link #setOrigin(OriginPosition)} can be used to transform
+ * the poses returned from {@link AprilTagFieldLayout#getTagPose(int)} to be correct relative to a
+ * different coordinate frame.
  */
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class AprilTagFieldLayout {
-  @JsonProperty(value = "tags")
-  private final List<AprilTag> m_apriltags = new ArrayList<>();
+  public enum OriginPosition {
+    kBlueAllianceWallRightSide,
+    kRedAllianceWallRightSide,
+  }
+
+  private final Map<Integer, AprilTag> m_apriltags = new HashMap<>();
 
   @JsonProperty(value = "field")
   private FieldDimensions m_fieldDimensions;
 
-  private boolean m_mirror;
+  private Pose3d m_origin;
 
   /**
    * Construct a new AprilTagFieldLayout with values imported from a JSON file.
@@ -65,16 +73,17 @@ public class AprilTagFieldLayout {
   public AprilTagFieldLayout(Path path) throws IOException {
     AprilTagFieldLayout layout =
         new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class);
-    m_apriltags.addAll(layout.m_apriltags);
+    m_apriltags.putAll(layout.m_apriltags);
     m_fieldDimensions = layout.m_fieldDimensions;
+    setOrigin(OriginPosition.kBlueAllianceWallRightSide);
   }
 
   /**
    * 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.
+   * @param fieldLength Length of the field the layout is representing in meters.
+   * @param fieldWidth Width of the field the layout is representing in meters.
    */
   public AprilTagFieldLayout(List<AprilTag> apriltags, double fieldLength, double fieldWidth) {
     this(apriltags, new FieldDimensions(fieldLength, fieldWidth));
@@ -85,20 +94,60 @@ public class 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);
+    for (AprilTag tag : apriltags) {
+      m_apriltags.put(tag.ID, tag);
+    }
     m_fieldDimensions = fieldDimensions;
+    setOrigin(OriginPosition.kBlueAllianceWallRightSide);
   }
 
   /**
-   * Set the alliance that your team is on.
+   * Returns a List of the {@link AprilTag AprilTags} used in this layout.
    *
-   * <p>This changes the {@link #getTagPose(int)} method to return the correct pose for your
-   * alliance.
-   *
-   * @param alliance The alliance to mirror poses for.
+   * @return The {@link AprilTag AprilTags} used in this layout.
    */
-  public void setAlliance(DriverStation.Alliance alliance) {
-    m_mirror = alliance == DriverStation.Alliance.Red;
+  @JsonProperty("tags")
+  public List<AprilTag> getTags() {
+    return new ArrayList<>(m_apriltags.values());
+  }
+
+  /**
+   * Sets the origin based on a predefined enumeration of coordinate frame origins. The origins are
+   * calculated from the field dimensions.
+   *
+   * <p>This transforms the Pose3ds returned by {@link #getTagPose(int)} to return the correct pose
+   * relative to a predefined coordinate frame.
+   *
+   * @param origin The predefined origin
+   */
+  @JsonIgnore
+  public void setOrigin(OriginPosition origin) {
+    switch (origin) {
+      case kBlueAllianceWallRightSide:
+        setOrigin(new Pose3d());
+        break;
+      case kRedAllianceWallRightSide:
+        setOrigin(
+            new Pose3d(
+                new Translation3d(m_fieldDimensions.fieldLength, m_fieldDimensions.fieldWidth, 0),
+                new Rotation3d(0, 0, Math.PI)));
+        break;
+      default:
+        throw new IllegalArgumentException("Unsupported enum value");
+    }
+  }
+
+  /**
+   * Sets the origin for tag pose transformation.
+   *
+   * <p>This transforms the Pose3ds returned by {@link #getTagPose(int)} to return the correct pose
+   * relative to the provided origin.
+   *
+   * @param origin The new origin for tag transformations
+   */
+  @JsonIgnore
+  public void setOrigin(Pose3d origin) {
+    m_origin = origin;
   }
 
   /**
@@ -110,25 +159,11 @@ public class AprilTagFieldLayout {
    */
   @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) {
+    AprilTag tag = m_apriltags.get(ID);
+    if (tag == 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);
+    return Optional.of(tag.pose.relativeTo(m_origin));
   }
 
   /**
@@ -151,37 +186,51 @@ public class AprilTagFieldLayout {
     new ObjectMapper().writeValue(path.toFile(), this);
   }
 
+  /**
+   * Deserializes a field layout from a resource within a jar file.
+   *
+   * @param resourcePath The absolute path of the resource
+   * @return The deserialized layout
+   * @throws IOException If the resource could not be loaded
+   */
+  public static AprilTagFieldLayout loadFromResource(String resourcePath) throws IOException {
+    try (InputStream stream = AprilTagFieldLayout.class.getResourceAsStream(resourcePath);
+        InputStreamReader reader = new InputStreamReader(stream)) {
+      return new ObjectMapper().readerFor(AprilTagFieldLayout.class).readValue(reader);
+    }
+  }
+
   @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 m_apriltags.equals(other.m_apriltags) && m_origin.equals(other.m_origin);
     }
     return false;
   }
 
   @Override
   public int hashCode() {
-    return Objects.hash(m_apriltags, m_mirror);
+    return Objects.hash(m_apriltags, m_origin);
   }
 
   @JsonIgnoreProperties(ignoreUnknown = true)
   @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
   private static class FieldDimensions {
+    @SuppressWarnings("MemberName")
+    @JsonProperty(value = "length")
+    public double fieldLength;
+
     @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;
+        @JsonProperty(required = true, value = "length") double fieldLength,
+        @JsonProperty(required = true, value = "width") double fieldWidth) {
       this.fieldLength = fieldLength;
+      this.fieldWidth = fieldWidth;
     }
   }
 }

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java

@@ -0,0 +1,20 @@
+// 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.apriltag;
+
+public enum AprilTagFields {
+  k2022RapidReact("2022-rapidreact.json");
+
+  public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/";
+
+  /** Alias to the current game. */
+  public static final AprilTagFields kDefaultField = k2022RapidReact;
+
+  public final String m_resourceFile;
+
+  AprilTagFields(String resourceFile) {
+    m_resourceFile = kBaseResourceDir + resourceFile;
+  }
+}

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

@@ -4,7 +4,6 @@
 
 #include "frc/apriltag/AprilTagFieldLayout.h"
 
-#include <algorithm>
 #include <system_error>
 
 #include <units/angle.h>
@@ -26,7 +25,9 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) {
   wpi::json json;
   input >> json;
 
-  m_apriltags = json.at("tags").get<std::vector<AprilTag>>();
+  for (const auto& tag : json.at("tags").get<std::vector<AprilTag>>()) {
+    m_apriltags[tag.ID] = tag;
+  }
   m_fieldWidth = units::meter_t{json.at("field").at("width").get<double>()};
   m_fieldLength = units::meter_t{json.at("field").at("height").get<double>()};
 }
@@ -34,28 +35,37 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) {
 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)) {}
+    : m_fieldLength(std::move(fieldLength)),
+      m_fieldWidth(std::move(fieldWidth)) {
+  for (const auto& tag : apriltags) {
+    m_apriltags[tag.ID] = tag;
+  }
+}
 
-void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) {
-  m_mirror = alliance == DriverStation::Alliance::kRed;
+void AprilTagFieldLayout::SetOrigin(OriginPosition origin) {
+  switch (origin) {
+    case OriginPosition::kBlueAllianceWallRightSide:
+      SetOrigin(Pose3d{});
+      break;
+    case OriginPosition::kRedAllianceWallRightSide:
+      SetOrigin(Pose3d{Translation3d{m_fieldLength, m_fieldWidth, 0_m},
+                       Rotation3d{0_deg, 0_deg, 180_deg}});
+      break;
+    default:
+      throw std::invalid_argument("Invalid origin");
+  }
+}
+
+void AprilTagFieldLayout::SetOrigin(const Pose3d& origin) {
+  m_origin = origin;
 }
 
 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>();
+  const auto& it = m_apriltags.find(ID);
+  if (it == m_apriltags.end()) {
+    return std::nullopt;
   }
-  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);
+  return it->second.pose.RelativeTo(m_origin);
 }
 
 void AprilTagFieldLayout::Serialize(std::string_view path) {
@@ -72,7 +82,7 @@ void AprilTagFieldLayout::Serialize(std::string_view path) {
 }
 
 bool AprilTagFieldLayout::operator==(const AprilTagFieldLayout& other) const {
-  return m_apriltags == other.m_apriltags && m_mirror == other.m_mirror &&
+  return m_apriltags == other.m_apriltags && m_origin == other.m_origin &&
          m_fieldLength == other.m_fieldLength &&
          m_fieldWidth == other.m_fieldWidth;
 }
@@ -82,14 +92,24 @@ bool AprilTagFieldLayout::operator!=(const AprilTagFieldLayout& other) const {
 }
 
 void frc::to_json(wpi::json& json, const AprilTagFieldLayout& layout) {
+  std::vector<AprilTag> tagVector;
+  tagVector.reserve(layout.m_apriltags.size());
+  for (const auto& pair : layout.m_apriltags) {
+    tagVector.push_back(pair.second);
+  }
+
   json = wpi::json{{"field",
                     {{"length", layout.m_fieldLength.value()},
                      {"width", layout.m_fieldWidth.value()}}},
-                   {"tags", layout.m_apriltags}};
+                   {"tags", tagVector}};
 }
 
 void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) {
-  layout.m_apriltags = json.at("tags").get<std::vector<AprilTag>>();
+  layout.m_apriltags.clear();
+  for (const auto& tag : json.at("tags").get<std::vector<AprilTag>>()) {
+    layout.m_apriltags[tag.ID] = tag;
+  }
+
   layout.m_fieldLength =
       units::meter_t{json.at("field").at("length").get<double>()};
   layout.m_fieldWidth =

apriltag/src/main/native/cpp/AprilTagFields.cpp

@@ -0,0 +1,28 @@
+// 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/AprilTagFields.h"
+
+#include <wpi/json.h>
+
+namespace frc {
+
+// C++ generated from resource files
+std::string_view GetResource_2022_rapidreact_json();
+
+AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) {
+  std::string_view fieldString;
+  switch (field) {
+    case AprilTagField::k2022RapidReact:
+      fieldString = GetResource_2022_rapidreact_json();
+      break;
+    case AprilTagField::kNumFields:
+      throw std::invalid_argument("Invalid Field");
+  }
+
+  wpi::json json = wpi::json::parse(fieldString);
+  return json.get<AprilTagFieldLayout>();
+}
+
+}  // namespace frc

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

@@ -6,12 +6,12 @@
 
 #include <optional>
 #include <string_view>
+#include <unordered_map>
 #include <vector>
 
 #include <units/length.h>
 #include <wpi/SymbolExports.h>
 
-#include "frc/DriverStation.h"
 #include "frc/apriltag/AprilTag.h"
 #include "frc/geometry/Pose3d.h"
 
@@ -28,17 +28,23 @@ namespace frc {
  * 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.
+ * "width" and "length" values.  This is to account for arbitrary field sizes
+ * when transforming 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.
+ * when the blue alliance is at the left. By default, Pose3ds will be returned
+ * as declared when calling GetTagPose(int).
+ * SetOrigin(AprilTagFieldLayout::OriginPosition) can be used to transform the
+ * poses returned by GetTagPose(int) to be correct relative to a different
+ * coordinate frame.
  */
 class WPILIB_DLLEXPORT AprilTagFieldLayout {
  public:
+  enum class OriginPosition {
+    kBlueAllianceWallRightSide,
+    kRedAllianceWallRightSide,
+  };
+
   AprilTagFieldLayout() = default;
 
   /**
@@ -52,21 +58,32 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout {
    * 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 fieldLength Length of field the layout is 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.
+   * Sets the origin based on a predefined enumeration of coordinate frame
+   * origins. The origins are calculated from the field dimensions.
    *
-   * This changes the GetTagPose(int) method to return the correct pose for your
-   * alliance.
+   * This transforms the Pose3ds returned by GetTagPose(int) to return the
+   * correct pose relative to a predefined coordinate frame.
    *
-   * @param alliance The alliance to mirror poses for.
+   * @param origin The predefined origin
    */
-  void SetAlliance(DriverStation::Alliance alliance);
+  void SetOrigin(OriginPosition origin);
+
+  /**
+   * Sets the origin for tag pose transformation.
+   *
+   * This tranforms the Pose3ds returned by GetTagPose(int) to return the
+   * correct pose relative to the provided origin.
+   *
+   * @param origin The new origin for tag transformations
+   */
+  void SetOrigin(const Pose3d& origin);
 
   /**
    * Gets an AprilTag pose by its ID.
@@ -101,10 +118,10 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout {
   bool operator!=(const AprilTagFieldLayout& other) const;
 
  private:
-  std::vector<AprilTag> m_apriltags;
+  std::unordered_map<int, AprilTag> m_apriltags;
   units::meter_t m_fieldLength;
   units::meter_t m_fieldWidth;
-  bool m_mirror = false;
+  Pose3d m_origin;
 
   friend WPILIB_DLLEXPORT void to_json(wpi::json& json,
                                        const AprilTagFieldLayout& layout);

apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/apriltag/AprilTagFieldLayout.h"
+
+namespace frc {
+
+enum class AprilTagField {
+  k2022RapidReact,
+
+  // This is a placeholder for denoting the last supported field. This should
+  // always be the last entry in the enum and should not be used by users
+  kNumFields,
+};
+
+/**
+ * Loads an AprilTagFieldLayout from a predefined field
+ *
+ * @param field The predefined field
+ */
+WPILIB_DLLEXPORT AprilTagFieldLayout
+LoadAprilTagLayoutField(AprilTagField field);
+
+}  // namespace frc

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2022-rapidreact.json

@@ -0,0 +1,415 @@
+{
+  "tags" : [ {
+    "ID" : 0,
+    "pose" : {
+      "translation" : {
+        "x" : -0.0035306,
+        "y" : 7.578928199999999,
+        "z" : 0.8858503999999999
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 1.0,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 0.0
+        }
+      }
+    }
+  }, {
+    "ID" : 1,
+    "pose" : {
+      "translation" : {
+        "x" : 3.2327088,
+        "y" : 5.486654,
+        "z" : 1.7254728
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 1.0,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 0.0
+        }
+      }
+    }
+  }, {
+    "ID" : 2,
+    "pose" : {
+      "translation" : {
+        "x" : 3.067812,
+        "y" : 5.3305202,
+        "z" : 1.3762228
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 0.7071067811865476,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : -0.7071067811865475
+        }
+      }
+    }
+  }, {
+    "ID" : 3,
+    "pose" : {
+      "translation" : {
+        "x" : 0.0039878,
+        "y" : 5.058536999999999,
+        "z" : 0.80645
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 1.0,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 0.0
+        }
+      }
+    }
+  }, {
+    "ID" : 4,
+    "pose" : {
+      "translation" : {
+        "x" : 0.0039878,
+        "y" : 3.5124898,
+        "z" : 0.80645
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 1.0,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 0.0
+        }
+      }
+    }
+  }, {
+    "ID" : 5,
+    "pose" : {
+      "translation" : {
+        "x" : 0.12110719999999998,
+        "y" : 1.7178274,
+        "z" : 0.8906002000000001
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 0.9196502204050923,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 0.39273842708457407
+        }
+      }
+    }
+  }, {
+    "ID" : 6,
+    "pose" : {
+      "translation" : {
+        "x" : 0.8733027999999999,
+        "y" : 0.9412985999999999,
+        "z" : 0.8906002000000001
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 0.9196502204050923,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 0.39273842708457407
+        }
+      }
+    }
+  }, {
+    "ID" : 7,
+    "pose" : {
+      "translation" : {
+        "x" : 1.6150844,
+        "y" : 0.15725139999999999,
+        "z" : 0.8906002000000001
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 0.9196502204050923,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 0.39273842708457407
+        }
+      }
+    }
+  }, {
+    "ID" : 10,
+    "pose" : {
+      "translation" : {
+        "x" : 16.4627306,
+        "y" : 0.6506718,
+        "z" : 0.8858503999999999
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 6.123233995736766E-17,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 1.0
+        }
+      }
+    }
+  }, {
+    "ID" : 11,
+    "pose" : {
+      "translation" : {
+        "x" : 13.2350002,
+        "y" : 2.743454,
+        "z" : 1.7254728
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 6.123233995736766E-17,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 1.0
+        }
+      }
+    }
+  }, {
+    "ID" : 12,
+    "pose" : {
+      "translation" : {
+        "x" : 13.391388000000001,
+        "y" : 2.8998418,
+        "z" : 1.3762228
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 0.7071067811865476,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 0.7071067811865475
+        }
+      }
+    }
+  }, {
+    "ID" : 13,
+    "pose" : {
+      "translation" : {
+        "x" : 16.4552122,
+        "y" : 3.1755079999999998,
+        "z" : 0.80645
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 6.123233995736766E-17,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 1.0
+        }
+      }
+    }
+  }, {
+    "ID" : 14,
+    "pose" : {
+      "translation" : {
+        "x" : 16.4552122,
+        "y" : 4.7171356,
+        "z" : 0.80645
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 6.123233995736766E-17,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 1.0
+        }
+      }
+    }
+  }, {
+    "ID" : 15,
+    "pose" : {
+      "translation" : {
+        "x" : 16.3350194,
+        "y" : 6.5149729999999995,
+        "z" : 0.8937752
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : -0.37298778257580906,
+          "X" : -0.0,
+          "Y" : 0.0,
+          "Z" : 0.9278362538989199
+        }
+      }
+    }
+  }, {
+    "ID" : 16,
+    "pose" : {
+      "translation" : {
+        "x" : 15.5904946,
+        "y" : 7.292695599999999,
+        "z" : 0.8906002000000001
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : -0.37298778257580906,
+          "X" : -0.0,
+          "Y" : 0.0,
+          "Z" : 0.9278362538989199
+        }
+      }
+    }
+  }, {
+    "ID" : 17,
+    "pose" : {
+      "translation" : {
+        "x" : 14.847188999999998,
+        "y" : 8.0691228,
+        "z" : 0.8906002000000001
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : -0.37298778257580906,
+          "X" : -0.0,
+          "Y" : 0.0,
+          "Z" : 0.9278362538989199
+        }
+      }
+    }
+  }, {
+    "ID" : 40,
+    "pose" : {
+      "translation" : {
+        "x" : 7.874127,
+        "y" : 4.9131728,
+        "z" : 0.7032752
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 0.5446390350150271,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 0.838670567945424
+        }
+      }
+    }
+  }, {
+    "ID" : 41,
+    "pose" : {
+      "translation" : {
+        "x" : 7.4312271999999995,
+        "y" : 3.759327,
+        "z" : 0.7032752
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : -0.20791169081775934,
+          "X" : -0.0,
+          "Y" : 0.0,
+          "Z" : 0.9781476007338057
+        }
+      }
+    }
+  }, {
+    "ID" : 42,
+    "pose" : {
+      "translation" : {
+        "x" : 8.585073,
+        "y" : 3.3164272,
+        "z" : 0.7032752
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 0.838670567945424,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : -0.5446390350150271
+        }
+      }
+    }
+  }, {
+    "ID" : 43,
+    "pose" : {
+      "translation" : {
+        "x" : 9.0279728,
+        "y" : 4.470273,
+        "z" : 0.7032752
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 0.9781476007338057,
+          "X" : 0.0,
+          "Y" : 0.0,
+          "Z" : 0.20791169081775934
+        }
+      }
+    }
+  }, {
+    "ID" : 50,
+    "pose" : {
+      "translation" : {
+        "x" : 7.6790296,
+        "y" : 4.3261534,
+        "z" : 2.4177244
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 0.17729273396782605,
+          "X" : -0.22744989571511945,
+          "Y" : 0.04215534644161733,
+          "Z" : 0.9565859910053995
+        }
+      }
+    }
+  }, {
+    "ID" : 51,
+    "pose" : {
+      "translation" : {
+        "x" : 8.0182466,
+        "y" : 3.5642296,
+        "z" : 2.4177244
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : -0.5510435465842192,
+          "X" : -0.19063969497246985,
+          "Y" : -0.13102303230819815,
+          "Z" : 0.8017733354717242
+        }
+      }
+    }
+  }, {
+    "ID" : 52,
+    "pose" : {
+      "translation" : {
+        "x" : 8.7801704,
+        "y" : 3.9034466,
+        "z" : 2.4177244
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : -0.9565859910053994,
+          "X" : -0.04215534644161739,
+          "Y" : -0.22744989571511942,
+          "Z" : 0.17729273396782633
+        }
+      }
+    }
+  }, {
+    "ID" : 53,
+    "pose" : {
+      "translation" : {
+        "x" : 8.4409534,
+        "y" : 4.6653704,
+        "z" : 2.4177244
+      },
+      "rotation" : {
+        "quaternion" : {
+          "W" : 0.8017733354717241,
+          "X" : -0.1310230323081982,
+          "Y" : 0.19063969497246983,
+          "Z" : 0.5510435465842194
+        }
+      }
+    }
+  } ],
+  "field" : {
+    "length" : 8.2296,
+    "width" : 16.4592
+  }
+}

apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagPoseSetOriginTest.java

@@ -2,7 +2,7 @@
 // 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;
+package edu.wpi.first.apriltag;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
@@ -10,13 +10,12 @@ 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 {
+class AprilTagPoseSetOriginTest {
   @Test
-  void poseMirroring() {
+  void transformationMatches() {
     var layout =
         new AprilTagFieldLayout(
             List.of(
@@ -29,7 +28,7 @@ class AprilTagPoseMirroringTest {
                         new Rotation3d(0, 0, Units.degreesToRadians(180))))),
             Units.feetToMeters(54.0),
             Units.feetToMeters(27.0));
-    layout.setAlliance(DriverStation.Alliance.Red);
+    layout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide);
 
     assertEquals(
         new Pose3d(

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

@@ -2,7 +2,7 @@
 // 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;
+package edu.wpi.first.apriltag;
 
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertEquals;

apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java

@@ -0,0 +1,74 @@
+// 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.apriltag;
+
+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.assertTrue;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.util.Units;
+import java.io.IOException;
+import java.util.Optional;
+import org.junit.jupiter.api.Assertions;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+class LoadConfigTest {
+  @ParameterizedTest
+  @EnumSource(AprilTagFields.class)
+  void testLoad(AprilTagFields field) {
+    AprilTagFieldLayout layout =
+        Assertions.assertDoesNotThrow(
+            () -> AprilTagFieldLayout.loadFromResource(field.m_resourceFile));
+    assertNotNull(layout);
+  }
+
+  @Test
+  void test2022RapidReact() throws IOException {
+    AprilTagFieldLayout layout =
+        AprilTagFieldLayout.loadFromResource(AprilTagFields.k2022RapidReact.m_resourceFile);
+
+    // Blue Hangar Truss - Hub
+    Pose3d expectedPose =
+        new Pose3d(
+            Units.inchesToMeters(127.272),
+            Units.inchesToMeters(216.01),
+            Units.inchesToMeters(67.932),
+            new Rotation3d(0, 0, 0));
+    Optional<Pose3d> maybePose = layout.getTagPose(1);
+    assertTrue(maybePose.isPresent());
+    assertEquals(expectedPose, maybePose.get());
+
+    // Blue Terminal Near Station
+    expectedPose =
+        new Pose3d(
+            Units.inchesToMeters(4.768),
+            Units.inchesToMeters(67.631),
+            Units.inchesToMeters(35.063),
+            new Rotation3d(0, 0, Math.toRadians(46.25)));
+    maybePose = layout.getTagPose(5);
+    assertTrue(maybePose.isPresent());
+    assertEquals(expectedPose, maybePose.get());
+
+    // Upper Hub Blue-Near
+    expectedPose =
+        new Pose3d(
+            Units.inchesToMeters(332.321),
+            Units.inchesToMeters(183.676),
+            Units.inchesToMeters(95.186),
+            new Rotation3d(0, Math.toRadians(26.75), Math.toRadians(69)));
+    maybePose = layout.getTagPose(53);
+    assertTrue(maybePose.isPresent());
+    assertEquals(expectedPose, maybePose.get());
+
+    // Doesn't exist
+    maybePose = layout.getTagPose(54);
+    assertFalse(maybePose.isPresent());
+  }
+}

apriltag/src/test/native/cpp/AprilTagPoseSetOriginTest.cpp

@@ -13,7 +13,7 @@
 
 using namespace frc;
 
-TEST(AprilTagPoseMirroringTest, MirroringMatches) {
+TEST(AprilTagPoseSetOriginTest, TransformationMatches) {
   auto layout = AprilTagFieldLayout{
       std::vector<AprilTag>{
           AprilTag{1,
@@ -22,7 +22,8 @@ TEST(AprilTagPoseMirroringTest, MirroringMatches) {
               2, Pose3d{4_ft, 4_ft, 4_ft, Rotation3d{0_deg, 0_deg, 180_deg}}}},
       54_ft, 27_ft};
 
-  layout.SetAlliance(DriverStation::Alliance::kRed);
+  layout.SetOrigin(
+      AprilTagFieldLayout::OriginPosition::kRedAllianceWallRightSide);
 
   auto mirrorPose =
       Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}};

apriltag/src/test/native/cpp/LoadConfigTest.cpp

@@ -0,0 +1,61 @@
+// 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/AprilTagFields.h"
+#include "gtest/gtest.h"
+
+namespace frc {
+
+std::vector<AprilTagField> GetAllFields() {
+  std::vector<AprilTagField> output;
+
+  for (int i = 0; i < static_cast<int>(AprilTagField::kNumFields); ++i) {
+    output.push_back(static_cast<AprilTagField>(i));
+  }
+
+  return output;
+}
+
+TEST(AprilTagFieldsTest, TestLoad2022RapidReact) {
+  AprilTagFieldLayout layout =
+      LoadAprilTagLayoutField(AprilTagField::k2022RapidReact);
+
+  // Blue Hangar Truss - Hub
+  auto expectedPose =
+      Pose3d{127.272_in, 216.01_in, 67.932_in, Rotation3d{0_deg, 0_deg, 0_deg}};
+  auto maybePose = layout.GetTagPose(1);
+  EXPECT_TRUE(maybePose);
+  EXPECT_EQ(expectedPose, *maybePose);
+
+  // Blue Terminal Near Station
+  expectedPose = Pose3d{4.768_in, 67.631_in, 35.063_in,
+                        Rotation3d{0_deg, 0_deg, 46.25_deg}};
+  maybePose = layout.GetTagPose(5);
+  EXPECT_TRUE(maybePose);
+  EXPECT_EQ(expectedPose, *maybePose);
+
+  // Upper Hub Blue-Near
+  expectedPose = Pose3d{332.321_in, 183.676_in, 95.186_in,
+                        Rotation3d{0_deg, 26.75_deg, 69_deg}};
+  maybePose = layout.GetTagPose(53);
+  EXPECT_TRUE(maybePose);
+  EXPECT_EQ(expectedPose, *maybePose);
+
+  // Doesn't exist
+  maybePose = layout.GetTagPose(54);
+  EXPECT_FALSE(maybePose);
+}
+
+// Test all of the fields in the enum
+class AllFieldsFixtureTest : public ::testing::TestWithParam<AprilTagField> {};
+
+TEST_P(AllFieldsFixtureTest, CheckEntireEnum) {
+  AprilTagField field = GetParam();
+  EXPECT_NO_THROW(LoadAprilTagLayoutField(field));
+}
+
+INSTANTIATE_TEST_SUITE_P(ValuesEnumTestInstTests, AllFieldsFixtureTest,
+                         ::testing::ValuesIn(GetAllFields()));
+
+}  // namespace frc

apriltag/src/test/native/cpp/main.cpp

@@ -0,0 +1,11 @@
+// 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 "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

build.gradle

@@ -13,10 +13,10 @@ buildscript {
 
 plugins {
     id 'base'
-    id 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' version '4.1.0'
+    id 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' version '2023.0.0'
     id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2020.2'
     id 'edu.wpi.first.NativeUtils' apply false
-    id 'edu.wpi.first.GradleJni' version '1.0.0'
+    id 'edu.wpi.first.GradleJni' version '1.1.0'
     id 'edu.wpi.first.GradleVsCode'
     id 'idea'
     id 'visual-studio'
@@ -136,8 +136,10 @@ subprojects {
     }
 
     // Sign outputs with Developer ID
-    if (project.hasProperty("developerID")) {
-        tasks.withType(AbstractLinkTask) { task ->
+    tasks.withType(AbstractLinkTask) { task ->
+        task.inputs.property "HasDeveloperId", project.hasProperty("developerID")
+
+        if (project.hasProperty("developerID")) {
             // Don't sign any executables because codesign complains
             // about relative rpath.
             if (!(task instanceof LinkExecutable)) {

buildSrc/build.gradle

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

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

@@ -140,6 +140,12 @@ int UsbCameraImpl::RawToPercentage(const UsbCameraProperty& rawProp,
     }
     return 100;
   }
+  // Arducam OV9281 exposure setting quirk
+  if (m_ov9281_exposure && rawProp.name == "raw_exposure_absolute" &&
+      rawProp.minimum == 1 && rawProp.maximum == 5000) {
+    // real range is 1-75
+    return 100.0 * (rawValue - 1) / (75 - 1);
+  }
   return 100.0 * (rawValue - rawProp.minimum) /
          (rawProp.maximum - rawProp.minimum);
 }
@@ -159,6 +165,12 @@ int UsbCameraImpl::PercentageToRaw(const UsbCameraProperty& rawProp,
     }
     return quirkLifeCamHd3000[ndx];
   }
+  // Arducam OV9281 exposure setting quirk
+  if (m_ov9281_exposure && rawProp.name == "raw_exposure_absolute" &&
+      rawProp.minimum == 1 && rawProp.maximum == 5000) {
+    // real range is 1-75
+    return 1 + (75 - 1) * (percentValue / 100.0);
+  }
   return rawProp.minimum +
          (rawProp.maximum - rawProp.minimum) * (percentValue / 100.0);
 }
@@ -1384,6 +1396,7 @@ void UsbCameraImpl::SetQuirks() {
   std::string_view desc = GetDescription(descbuf);
   m_lifecam_exposure = wpi::ends_with(desc, "LifeCam HD-3000") ||
                        wpi::ends_with(desc, "LifeCam Cinema (TM)");
+  m_ov9281_exposure = wpi::contains(desc, "OV9281");
   m_picamera = wpi::ends_with(desc, "mmal service");
 
   int deviceNum = GetDeviceNum(m_path.c_str());

cscore/src/main/native/linux/UsbCameraImpl.h

@@ -161,6 +161,7 @@ class UsbCameraImpl : public SourceImpl {
   // Quirks
   bool m_lifecam_exposure{false};    // Microsoft LifeCam exposure
   bool m_ps3eyecam_exposure{false};  // PS3 Eyecam exposure
+  bool m_ov9281_exposure{false};     // Arducam OV9281 exposure
   bool m_picamera{false};            // Raspberry Pi camera
 
   //

datalogtool/publish.gradle

@@ -37,6 +37,8 @@ model {
                             into("MacOS") { with copySpec { from binary.executable.file } }
                             into("Resources") { with copySpec { from icon } }
 
+                            inputs.property "HasDeveloperId", project.hasProperty("developerID")
+
                             doLast {
                                 if (project.hasProperty("developerID")) {
                                     // Get path to binary.

docs/build.gradle

@@ -126,6 +126,9 @@ doxygen {
         exclude 'units/**'
     }
 
+    //TODO: building memory docs causes search to break
+    exclude 'wpi/memory/**'
+
     aliases 'effects=\\par <i>Effects:</i>^^',
             'notes=\\par <i>Notes:</i>^^',
             'requires=\\par <i>Requires:</i>^^',

glass/publish.gradle

@@ -92,6 +92,8 @@ model {
                             into("MacOS") { with copySpec { from binary.executable.file } }
                             into("Resources") { with copySpec { from icon } }
 
+                            inputs.property "HasDeveloperId", project.hasProperty("developerID")
+
                             doLast {
                                 if (project.hasProperty("developerID")) {
                                     // Get path to binary.

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

@@ -53,11 +53,12 @@ struct PlotSeriesRef {
 };
 
 class PlotSeries {
-  explicit PlotSeries(Storage& storage, int yAxis = 0);
+  explicit PlotSeries(Storage& storage);
 
  public:
   PlotSeries(Storage& storage, std::string_view id);
-  PlotSeries(Storage& storage, DataSource* source, int yAxis = 0);
+  PlotSeries(Storage& storage, DataSource* source);
+  PlotSeries(Storage& storage, DataSource* source, int yAxis);
 
   const std::string& GetId() const { return m_id; }
 
@@ -195,7 +196,7 @@ class PlotView : public View {
 
 }  // namespace
 
-PlotSeries::PlotSeries(Storage& storage, int yAxis)
+PlotSeries::PlotSeries(Storage& storage)
     : m_id{storage.GetString("id")},
       m_name{storage.GetString("name")},
       m_yAxis{storage.GetInt("yAxis", 0)},
@@ -208,12 +209,10 @@ PlotSeries::PlotSeries(Storage& storage, int yAxis)
       m_digital{
           storage.GetString("digital"), kAuto, {"Auto", "Digital", "Analog"}},
       m_digitalBitHeight{storage.GetInt("digitalBitHeight", 8)},
-      m_digitalBitGap{storage.GetInt("digitalBitGap", 4)} {
-  m_yAxis = yAxis;
-}
+      m_digitalBitGap{storage.GetInt("digitalBitGap", 4)} {}
 
 PlotSeries::PlotSeries(Storage& storage, std::string_view id)
-    : PlotSeries{storage, 0} {
+    : PlotSeries{storage} {
   m_id = id;
   if (DataSource* source = DataSource::Find(id)) {
     SetSource(source);
@@ -222,12 +221,17 @@ PlotSeries::PlotSeries(Storage& storage, std::string_view id)
   CheckSource();
 }
 
-PlotSeries::PlotSeries(Storage& storage, DataSource* source, int yAxis)
-    : PlotSeries{storage, yAxis} {
+PlotSeries::PlotSeries(Storage& storage, DataSource* source)
+    : PlotSeries{storage} {
   SetSource(source);
   m_id = source->GetId();
 }
 
+PlotSeries::PlotSeries(Storage& storage, DataSource* source, int yAxis)
+    : PlotSeries{storage, source} {
+  m_yAxis = yAxis;
+}
+
 void PlotSeries::CheckSource() {
   if (!m_newValueConn.connected() && !m_sourceCreatedConn.connected()) {
     m_source = nullptr;

glass/src/libnt/native/cpp/NTMechanism2D.cpp

@@ -289,16 +289,13 @@ void NTMechanism2DModel::Update() {
         }
       }
     } else if (auto valueData = event.GetValueEventData()) {
-      // .name
       if (valueData->topic == m_nameTopic.GetHandle()) {
+        // .name
         if (valueData->value && valueData->value.IsString()) {
           m_nameValue = valueData->value.GetString();
         }
-        continue;
-      }
-
-      // dims
-      if (valueData->topic == m_dimensionsTopic.GetHandle()) {
+      } else if (valueData->topic == m_dimensionsTopic.GetHandle()) {
+        // dims
         if (valueData->value && valueData->value.IsDoubleArray()) {
           auto arr = valueData->value.GetDoubleArray();
           if (arr.size() == 2) {
@@ -306,13 +303,32 @@ void NTMechanism2DModel::Update() {
                                                    units::meter_t{arr[1]}};
           }
         }
-      }
-
-      // backgroundColor
-      if (valueData->topic == m_bgColorTopic.GetHandle()) {
+      } else if (valueData->topic == m_bgColorTopic.GetHandle()) {
+        // backgroundColor
         if (valueData->value && valueData->value.IsString()) {
           ConvertColor(valueData->value.GetString(), &m_bgColorValue);
         }
+      } else {
+        auto fullName = nt::Topic{valueData->topic}.GetName();
+        auto name = wpi::drop_front(fullName, m_path.size());
+        if (name.empty() || name[0] == '.') {
+          continue;
+        }
+        std::string_view childName;
+        std::tie(name, childName) = wpi::split(name, '/');
+        if (childName.empty()) {
+          continue;
+        }
+
+        auto it = std::lower_bound(m_roots.begin(), m_roots.end(), name,
+                                   [](const auto& e, std::string_view name) {
+                                     return e->GetName() < name;
+                                   });
+        if (it != m_roots.end() && (*it)->GetName() == name) {
+          if ((*it)->NTUpdate(event, childName)) {
+            m_roots.erase(it);
+          }
+        }
       }
     }
   }

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

@@ -19,6 +19,7 @@
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
 
+#include "HALInitializer.h"
 #include "hal/DriverStation.h"
 #include "hal/Errors.h"
 
@@ -446,6 +447,7 @@ void HAL_RefreshDSData(void) {
 }
 
 void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) {
+  hal::init::CheckInit();
   driverStation->newDataEvents.Add(handle);
 }
 

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

@@ -82,6 +82,7 @@ void InitializeHAL() {
 }  // namespace init
 
 void ReleaseFPGAInterrupt(int32_t interruptNumber) {
+  hal::init::CheckInit();
   if (!global) {
     return;
   }
@@ -252,6 +253,7 @@ HAL_RuntimeType HAL_GetRuntimeType(void) {
 }
 
 int32_t HAL_GetFPGAVersion(int32_t* status) {
+  hal::init::CheckInit();
   if (!global) {
     *status = NiFpga_Status_ResourceNotInitialized;
     return 0;
@@ -260,6 +262,7 @@ int32_t HAL_GetFPGAVersion(int32_t* status) {
 }
 
 int64_t HAL_GetFPGARevision(int32_t* status) {
+  hal::init::CheckInit();
   if (!global) {
     *status = NiFpga_Status_ResourceNotInitialized;
     return 0;
@@ -268,6 +271,7 @@ int64_t HAL_GetFPGARevision(int32_t* status) {
 }
 
 uint64_t HAL_GetFPGATime(int32_t* status) {
+  hal::init::CheckInit();
   if (!global) {
     *status = NiFpga_Status_ResourceNotInitialized;
     return 0;
@@ -314,6 +318,7 @@ uint64_t HAL_ExpandFPGATime(uint32_t unexpandedLower, int32_t* status) {
 }
 
 HAL_Bool HAL_GetFPGAButton(int32_t* status) {
+  hal::init::CheckInit();
   if (!global) {
     *status = NiFpga_Status_ResourceNotInitialized;
     return false;
@@ -322,6 +327,7 @@ HAL_Bool HAL_GetFPGAButton(int32_t* status) {
 }
 
 HAL_Bool HAL_GetSystemActive(int32_t* status) {
+  hal::init::CheckInit();
   if (!watchdog) {
     *status = NiFpga_Status_ResourceNotInitialized;
     return false;
@@ -330,6 +336,7 @@ HAL_Bool HAL_GetSystemActive(int32_t* status) {
 }
 
 HAL_Bool HAL_GetBrownedOut(int32_t* status) {
+  hal::init::CheckInit();
   if (!watchdog) {
     *status = NiFpga_Status_ResourceNotInitialized;
     return false;

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

@@ -326,6 +326,7 @@ void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) {
   if (gShutdown) {
     return;
   }
+  hal::init::CheckInit();
   driverStation->newDataEvents.Add(handle);
 }
 

myRobot/build.gradle

@@ -53,6 +53,10 @@ dependencies {
     implementation project(':wpilibNewCommands')
 }
 
+tasks.withType(com.github.spotbugs.snom.SpotBugsTask).configureEach {
+    onlyIf { false }
+}
+
 def simProjects = ['halsim_gui']
 
 deploy {

ntcore/src/main/native/cpp/InstanceImpl.cpp

@@ -174,3 +174,15 @@ std::shared_ptr<INetworkClient> InstanceImpl::GetClient() {
   std::scoped_lock lock{m_mutex};
   return m_networkClient;
 }
+
+void InstanceImpl::Reset() {
+  std::scoped_lock lock{m_mutex};
+  m_networkServer.reset();
+  m_networkClient.reset();
+  m_servers.clear();
+  networkMode = NT_NET_MODE_NONE;
+
+  listenerStorage.Reset();
+  // connectionList should have been cleared by destroying networkClient/server
+  localStorage.Reset();
+}

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

@@ -56,6 +56,8 @@ class InstanceImpl {
   std::shared_ptr<NetworkServer> GetServer();
   std::shared_ptr<INetworkClient> GetClient();
 
+  void Reset();
+
   ListenerStorage listenerStorage;
   LoggerImpl logger_impl;
   wpi::Logger logger;

ntcore/src/main/native/cpp/ListenerStorage.cpp

@@ -36,17 +36,16 @@ void ListenerStorage::Thread::Main() {
     }
     // call all the way back out to the C++ API to ensure valid handle
     auto events = nt::ReadListenerQueue(m_poller);
-    if (events.empty()) {
-      continue;
-    }
-    std::unique_lock lock{m_mutex};
-    for (auto&& event : events) {
-      auto callbackIt = m_callbacks.find(event.listener);
-      if (callbackIt != m_callbacks.end()) {
-        auto callback = callbackIt->second;
-        lock.unlock();
-        callback(event);
-        lock.lock();
+    if (!events.empty()) {
+      std::unique_lock lock{m_mutex};
+      for (auto&& event : events) {
+        auto callbackIt = m_callbacks.find(event.listener);
+        if (callbackIt != m_callbacks.end()) {
+          auto callback = callbackIt->second;
+          lock.unlock();
+          callback(event);
+          lock.lock();
+        }
       }
     }
     if (std::find(signaled.begin(), signaled.end(),
@@ -308,16 +307,32 @@ ListenerStorage::RemoveListener(NT_Listener listenerHandle) {
 }
 
 bool ListenerStorage::WaitForListenerQueue(double timeout) {
-  std::scoped_lock lock{m_mutex};
   WPI_EventHandle h;
-  if (auto thr = m_thread.GetThread()) {
-    h = thr->m_waitQueueWaiter.GetHandle();
-    thr->m_waitQueueWakeup.Set();
-  } else {
-    return false;
+  {
+    std::scoped_lock lock{m_mutex};
+    if (auto thr = m_thread.GetThread()) {
+      h = thr->m_waitQueueWaiter.GetHandle();
+      thr->m_waitQueueWakeup.Set();
+    } else {
+      return false;
+    }
   }
   bool timedOut;
-  return wpi::WaitForObject(h, timeout, &timedOut);
+  wpi::WaitForObject(h, timeout, &timedOut);
+  return !timedOut;
+}
+
+void ListenerStorage::Reset() {
+  std::scoped_lock lock{m_mutex};
+  m_pollers.clear();
+  m_listeners.clear();
+  m_connListeners.clear();
+  m_topicListeners.clear();
+  m_valueListeners.clear();
+  m_logListeners.clear();
+  if (m_thread) {
+    m_thread.Stop();
+  }
 }
 
 std::vector<std::pair<NT_Listener, unsigned int>>

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

@@ -59,6 +59,8 @@ class ListenerStorage final : public IListenerStorage {
 
   bool WaitForListenerQueue(double timeout);
 
+  void Reset();
+
  private:
   // these assume the mutex is already held
   NT_Listener DoAddListener(NT_ListenerPoller pollerHandle);

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

@@ -20,6 +20,7 @@
 #include "Log.h"
 #include "PubSubOptions.h"
 #include "Types_internal.h"
+#include "Value_internal.h"
 #include "networktables/NetworkTableValue.h"
 #include "ntcore_c.h"
 
@@ -259,8 +260,9 @@ struct LSImpl {
 
   void CheckReset(TopicData* topic);
 
-  bool SetValue(TopicData* topic, const Value& value, unsigned int eventFlags);
-  void NotifyValue(TopicData* topic, unsigned int eventFlags);
+  bool SetValue(TopicData* topic, const Value& value, unsigned int eventFlags,
+                bool isDuplicate);
+  void NotifyValue(TopicData* topic, unsigned int eventFlags, bool isDuplicate);
 
   void SetFlags(TopicData* topic, unsigned int flags);
   void SetPersistent(TopicData* topic, bool value);
@@ -271,7 +273,7 @@ struct LSImpl {
                          unsigned int eventFlags, bool sendNetwork,
                          bool updateFlags = true);
 
-  void RefreshPubSubActive(TopicData* topic);
+  void RefreshPubSubActive(TopicData* topic, bool warnOnSubMismatch);
 
   void NetworkAnnounce(TopicData* topic, std::string_view typeStr,
                        const wpi::json& properties, NT_Publisher pubHandle);
@@ -322,7 +324,8 @@ struct LSImpl {
   PublisherData* PublishEntry(EntryData* entry, NT_Type type);
   Value* GetSubEntryValue(NT_Handle subentryHandle);
 
-  bool PublishLocalValue(PublisherData* publisher, const Value& value);
+  bool PublishLocalValue(PublisherData* publisher, const Value& value,
+                         bool force = false);
 
   bool SetEntryValue(NT_Handle pubentryHandle, const Value& value);
   bool SetDefaultEntryValue(NT_Handle pubsubentryHandle, const Value& value);
@@ -394,17 +397,9 @@ void PublisherData::UpdateActive() {
 void SubscriberData::UpdateActive() {
   // for subscribers, unassigned is a wildcard
   // also allow numerically compatible subscribers
-  active =
-      config.type == NT_UNASSIGNED ||
-      (config.type == topic->type && config.typeStr == topic->typeStr) ||
-      ((config.type & (NT_INTEGER | NT_FLOAT | NT_DOUBLE)) != 0 &&
-       (config.type & (NT_INTEGER | NT_FLOAT | NT_DOUBLE)) ==
-           (topic->type & (NT_INTEGER | NT_FLOAT | NT_DOUBLE))) ||
-      ((config.type & (NT_INTEGER_ARRAY | NT_FLOAT_ARRAY | NT_DOUBLE_ARRAY)) !=
-           0 &&
-       (config.type & (NT_INTEGER_ARRAY | NT_FLOAT_ARRAY | NT_DOUBLE_ARRAY)) ==
-           (topic->type &
-            (NT_INTEGER_ARRAY | NT_FLOAT_ARRAY | NT_DOUBLE_ARRAY)));
+  active = config.type == NT_UNASSIGNED ||
+           (config.type == topic->type && config.typeStr == topic->typeStr) ||
+           IsNumericCompatible(config.type, topic->type);
 }
 
 void LSImpl::NotifyTopic(TopicData* topic, unsigned int eventFlags) {
@@ -475,7 +470,7 @@ void LSImpl::CheckReset(TopicData* topic) {
 }
 
 bool LSImpl::SetValue(TopicData* topic, const Value& value,
-                      unsigned int eventFlags) {
+                      unsigned int eventFlags, bool isDuplicate) {
   if (topic->type != NT_UNASSIGNED && topic->type != value.type()) {
     return false;
   }
@@ -486,9 +481,9 @@ bool LSImpl::SetValue(TopicData* topic, const Value& value,
     topic->type = value.type();
     topic->lastValue = value;
     topic->lastValueNetwork = isNetwork;
-    NotifyValue(topic, eventFlags);
+    NotifyValue(topic, eventFlags, isDuplicate);
   }
-  if (topic->datalogType == value.type()) {
+  if (!isDuplicate && topic->datalogType == value.type()) {
     for (auto&& datalog : topic->datalogs) {
       datalog.Append(value);
     }
@@ -496,9 +491,11 @@ bool LSImpl::SetValue(TopicData* topic, const Value& value,
   return true;
 }
 
-void LSImpl::NotifyValue(TopicData* topic, unsigned int eventFlags) {
+void LSImpl::NotifyValue(TopicData* topic, unsigned int eventFlags,
+                         bool isDuplicate) {
   for (auto&& subscriber : topic->localSubscribers) {
-    if (subscriber->active) {
+    if (subscriber->active &&
+        (subscriber->config.keepDuplicates || !isDuplicate)) {
       subscriber->pollStorage.emplace_back(topic->lastValue);
       subscriber->handle.Set();
       if (!subscriber->valueListeners.empty()) {
@@ -509,10 +506,12 @@ void LSImpl::NotifyValue(TopicData* topic, unsigned int eventFlags) {
   }
 
   for (auto&& subscriber : topic->multiSubscribers) {
-    subscriber->handle.Set();
-    if (!subscriber->valueListeners.empty()) {
-      m_listenerStorage.Notify(subscriber->valueListeners, eventFlags,
-                               topic->handle, 0, topic->lastValue);
+    if (subscriber->options.keepDuplicates || !isDuplicate) {
+      subscriber->handle.Set();
+      if (!subscriber->valueListeners.empty()) {
+        m_listenerStorage.Notify(subscriber->valueListeners, eventFlags,
+                                 topic->handle, 0, topic->lastValue);
+      }
     }
   }
 }
@@ -620,12 +619,19 @@ void LSImpl::PropertiesUpdated(TopicData* topic, const wpi::json& update,
   }
 }
 
-void LSImpl::RefreshPubSubActive(TopicData* topic) {
+void LSImpl::RefreshPubSubActive(TopicData* topic, bool warnOnSubMismatch) {
   for (auto&& publisher : topic->localPublishers) {
     publisher->UpdateActive();
   }
   for (auto&& subscriber : topic->localSubscribers) {
     subscriber->UpdateActive();
+    if (warnOnSubMismatch && topic->Exists() && !subscriber->active) {
+      // warn on type mismatch
+      INFO(
+          "local subscribe to '{}' disabled due to type mismatch (wanted '{}', "
+          "published as '{}')",
+          topic->name, subscriber->config.typeStr, topic->typeStr);
+    }
   }
 }
 
@@ -653,7 +659,7 @@ void LSImpl::NetworkAnnounce(TopicData* topic, std::string_view typeStr,
     }
     topic->type = type;
     topic->typeStr = typeStr;
-    RefreshPubSubActive(topic);
+    RefreshPubSubActive(topic, true);
   }
   if (!didExist) {
     event |= NT_EVENT_PUBLISH;
@@ -702,7 +708,7 @@ void LSImpl::RemoveNetworkPublisher(TopicData* topic) {
         nextPub->config.typeStr != topic->typeStr) {
       topic->type = nextPub->config.type;
       topic->typeStr = nextPub->config.typeStr;
-      RefreshPubSubActive(topic);
+      RefreshPubSubActive(topic, false);
       // this may result in a duplicate publish warning on the server side,
       // but send one anyway in this case just to be sure
       if (nextPub->active && m_network) {
@@ -730,19 +736,20 @@ PublisherData* LSImpl::AddLocalPublisher(TopicData* topic,
   topic->localPublishers.Add(publisher);
 
   if (!didExist) {
+    DEBUG4("AddLocalPublisher: setting {} type {} typestr {}", topic->name,
+           static_cast<int>(config.type), config.typeStr);
     // set the type to the published type
     topic->type = config.type;
     topic->typeStr = config.typeStr;
-    RefreshPubSubActive(topic);
+    RefreshPubSubActive(topic, true);
 
     if (properties.is_null()) {
       topic->properties = wpi::json::object();
     } else if (properties.is_object()) {
       topic->properties = properties;
     } else {
-      WPI_WARNING(m_logger,
-                  "ignoring non-object properties when publishing '{}'",
-                  topic->name);
+      WARNING("ignoring non-object properties when publishing '{}'",
+              topic->name);
       topic->properties = wpi::json::object();
     }
 
@@ -794,7 +801,7 @@ std::unique_ptr<PublisherData> LSImpl::RemoveLocalPublisher(
           nextPub->config.typeStr != topic->typeStr) {
         topic->type = nextPub->config.type;
         topic->typeStr = nextPub->config.typeStr;
-        RefreshPubSubActive(topic);
+        RefreshPubSubActive(topic, false);
         if (nextPub->active && m_network) {
           m_network->Publish(nextPub->handle, topic->handle, topic->name,
                              topic->typeStr, topic->properties,
@@ -817,7 +824,7 @@ SubscriberData* LSImpl::AddLocalSubscriber(TopicData* topic,
     // warn on type mismatch
     INFO(
         "local subscribe to '{}' disabled due to type mismatch (wanted '{}', "
-        "currently '{}')",
+        "published as '{}')",
         topic->name, config.typeStr, topic->typeStr);
   }
   if (m_network) {
@@ -1176,8 +1183,12 @@ PublisherData* LSImpl::PublishEntry(EntryData* entry, NT_Type type) {
     entry->subscriber->config.typeStr = typeStr;
   } else if (entry->subscriber->config.type != type ||
              entry->subscriber->config.typeStr != typeStr) {
-    // don't allow dynamically changing the type of an entry
-    return nullptr;
+    if (!IsNumericCompatible(type, entry->subscriber->config.type)) {
+      // don't allow dynamically changing the type of an entry
+      ERROR("cannot publish entry {} as type {}, previously subscribed as {}",
+            entry->topic->name, typeStr, entry->subscriber->config.typeStr);
+      return nullptr;
+    }
   }
   // create publisher
   entry->publisher = AddLocalPublisher(entry->topic, wpi::json::object(),
@@ -1193,19 +1204,30 @@ Value* LSImpl::GetSubEntryValue(NT_Handle subentryHandle) {
   }
 }
 
-bool LSImpl::PublishLocalValue(PublisherData* publisher, const Value& value) {
+bool LSImpl::PublishLocalValue(PublisherData* publisher, const Value& value,
+                               bool force) {
   if (!value) {
     return false;
   }
   if (publisher->topic->type != NT_UNASSIGNED &&
       publisher->topic->type != value.type()) {
+    if (IsNumericCompatible(publisher->topic->type, value.type())) {
+      return PublishLocalValue(
+          publisher, ConvertNumericValue(value, publisher->topic->type));
+    }
     return false;
   }
   if (publisher->active) {
-    if (m_network) {
+    bool isDuplicate;
+    if (force || publisher->config.keepDuplicates) {
+      isDuplicate = false;
+    } else {
+      isDuplicate = (publisher->topic->lastValue == value);
+    }
+    if (!isDuplicate && m_network) {
       m_network->SetValue(publisher->handle, value);
     }
-    return SetValue(publisher->topic, value, NT_EVENT_VALUE_LOCAL);
+    return SetValue(publisher->topic, value, NT_EVENT_VALUE_LOCAL, isDuplicate);
   } else {
     return false;
   }
@@ -1229,28 +1251,38 @@ bool LSImpl::SetEntryValue(NT_Handle pubentryHandle, const Value& value) {
 
 bool LSImpl::SetDefaultEntryValue(NT_Handle pubsubentryHandle,
                                   const Value& value) {
+  DEBUG4("SetDefaultEntryValue({}, {})", pubsubentryHandle,
+         static_cast<int>(value.type()));
   if (!value) {
     return false;
   }
   if (auto topic = GetTopic(pubsubentryHandle)) {
-    if (topic->type == NT_UNASSIGNED || topic->type == value.type()) {
-      // set without notifying
-      topic->type = value.type();
-      topic->lastValue = value;
-      topic->lastValue.SetTime(0);
-      topic->lastValue.SetServerTime(0);
-
+    if (!topic->lastValue &&
+        (topic->type == NT_UNASSIGNED || topic->type == value.type() ||
+         IsNumericCompatible(topic->type, value.type()))) {
+      // publish if we haven't yet
       auto publisher = m_publishers.Get(pubsubentryHandle);
       if (!publisher) {
         if (auto entry = m_entries.Get(pubsubentryHandle)) {
           publisher = PublishEntry(entry, value.type());
         }
-        if (!publisher) {
-          return true;
-        }
       }
-      if (publisher->active && m_network) {
-        m_network->SetValue(publisher->handle, value);
+
+      // force value timestamps to 0
+      if (topic->type == NT_UNASSIGNED) {
+        topic->type = value.type();
+      }
+      if (topic->type == value.type()) {
+        topic->lastValue = value;
+      } else if (IsNumericCompatible(topic->type, value.type())) {
+        topic->lastValue = ConvertNumericValue(value, topic->type);
+      } else {
+        return true;
+      }
+      topic->lastValue.SetTime(0);
+      topic->lastValue.SetServerTime(0);
+      if (publisher) {
+        PublishLocalValue(publisher, topic->lastValue, true);
       }
       return true;
     }
@@ -1314,7 +1346,8 @@ void LocalStorage::NetworkPropertiesUpdate(std::string_view name,
 void LocalStorage::NetworkSetValue(NT_Topic topicHandle, const Value& value) {
   std::scoped_lock lock{m_mutex};
   if (auto topic = m_impl->m_topics.Get(topicHandle)) {
-    m_impl->SetValue(topic, value, NT_EVENT_VALUE_REMOTE);
+    m_impl->SetValue(topic, value, NT_EVENT_VALUE_REMOTE,
+                     value == topic->lastValue);
   }
 }
 
@@ -2047,10 +2080,17 @@ READ_QUEUE_NUMBER(Double)
 Value LocalStorage::GetEntryValue(NT_Handle subentryHandle) {
   std::scoped_lock lock{m_mutex};
   if (auto subscriber = m_impl->GetSubEntry(subentryHandle)) {
-    return subscriber->topic->lastValue;
-  } else {
-    return {};
+    if (subscriber->config.type == NT_UNASSIGNED ||
+        !subscriber->topic->lastValue ||
+        subscriber->config.type == subscriber->topic->lastValue.type()) {
+      return subscriber->topic->lastValue;
+    } else if (IsNumericCompatible(subscriber->config.type,
+                                   subscriber->topic->lastValue.type())) {
+      return ConvertNumericValue(subscriber->topic->lastValue,
+                                 subscriber->config.type);
+    }
   }
+  return {};
 }
 
 void LocalStorage::SetEntryFlags(NT_Entry entryHandle, unsigned int flags) {
@@ -2189,3 +2229,9 @@ void LocalStorage::StopDataLog(NT_DataLogger logger) {
     }
   }
 }
+
+void LocalStorage::Reset() {
+  std::scoped_lock lock{m_mutex};
+  m_impl = std::make_unique<Impl>(m_impl->m_inst, m_impl->m_listenerStorage,
+                                  m_impl->m_logger);
+}

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

@@ -209,6 +209,8 @@ class LocalStorage final : public net::ILocalStorage {
                              std::string_view logPrefix);
   void StopDataLog(NT_DataLogger logger);
 
+  void Reset();
+
  private:
   class Impl;
   std::unique_ptr<Impl> m_impl;

ntcore/src/main/native/cpp/NetworkServer.cpp

@@ -111,6 +111,7 @@ class NSImpl {
 
   void HandleLocal();
   void LoadPersistent();
+  void SavePersistent(std::string_view filename, std::string_view data);
   void Init();
   void AddConnection(ServerConnection* conn, const ConnectionInfo& info);
   void RemoveConnection(ServerConnection* conn);
@@ -372,11 +373,16 @@ void NSImpl::LoadPersistent() {
   }
 }
 
-static void SavePersistent(std::string_view filename, std::string_view data) {
+void NSImpl::SavePersistent(std::string_view filename, std::string_view data) {
   // write to temporary file
   auto tmp = fmt::format("{}.tmp", filename);
   std::error_code ec;
   wpi::raw_fd_ostream os{tmp, ec, fs::F_Text};
+  if (ec.value() != 0) {
+    INFO("could not open persistent file '{}' for write: {}", tmp,
+         ec.message());
+    return;
+  }
   os << data;
   os.close();
   if (os.has_error()) {
@@ -414,9 +420,8 @@ void NSImpl::Init() {
     if (m_serverImpl.PersistentChanged()) {
       uv::QueueWork(
           m_loop,
-          [fn = m_persistentFilename, data = m_serverImpl.DumpPersistent()] {
-            SavePersistent(fn, data);
-          },
+          [this, fn = m_persistentFilename,
+           data = m_serverImpl.DumpPersistent()] { SavePersistent(fn, data); },
           nullptr);
     }
   });

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

@@ -14,4 +14,17 @@ std::string_view TypeToString(NT_Type type);
 NT_Type StringToType(std::string_view typeStr);
 NT_Type StringToType3(std::string_view typeStr);
 
+constexpr bool IsNumeric(NT_Type type) {
+  return (type & (NT_INTEGER | NT_FLOAT | NT_DOUBLE)) != 0;
+}
+
+constexpr bool IsNumericArray(NT_Type type) {
+  return (type & (NT_INTEGER_ARRAY | NT_FLOAT_ARRAY | NT_DOUBLE_ARRAY)) != 0;
+}
+
+constexpr bool IsNumericCompatible(NT_Type a, NT_Type b) {
+  return (IsNumeric(a) && IsNumeric(b)) ||
+         (IsNumericArray(a) && IsNumericArray(b));
+}
+
 }  // namespace nt

ntcore/src/main/native/cpp/Value.cpp

@@ -89,6 +89,15 @@ Value Value::MakeBooleanArray(std::span<const int> value, int64_t time) {
   return val;
 }
 
+Value Value::MakeBooleanArray(std::vector<int>&& value, int64_t time) {
+  Value val{NT_BOOLEAN_ARRAY, time, private_init{}};
+  auto data = std::make_shared<std::vector<int>>(std::move(value));
+  val.m_val.data.arr_boolean.arr = data->data();
+  val.m_val.data.arr_boolean.size = data->size();
+  val.m_storage = std::move(data);
+  return val;
+}
+
 Value Value::MakeIntegerArray(std::span<const int64_t> value, int64_t time) {
   Value val{NT_INTEGER_ARRAY, time, private_init{}};
   auto data =
@@ -99,6 +108,15 @@ Value Value::MakeIntegerArray(std::span<const int64_t> value, int64_t time) {
   return val;
 }
 
+Value Value::MakeIntegerArray(std::vector<int64_t>&& value, int64_t time) {
+  Value val{NT_INTEGER_ARRAY, time, private_init{}};
+  auto data = std::make_shared<std::vector<int64_t>>(std::move(value));
+  val.m_val.data.arr_int.arr = data->data();
+  val.m_val.data.arr_int.size = data->size();
+  val.m_storage = std::move(data);
+  return val;
+}
+
 Value Value::MakeFloatArray(std::span<const float> value, int64_t time) {
   Value val{NT_FLOAT_ARRAY, time, private_init{}};
   auto data = std::make_shared<std::vector<float>>(value.begin(), value.end());
@@ -108,6 +126,15 @@ Value Value::MakeFloatArray(std::span<const float> value, int64_t time) {
   return val;
 }
 
+Value Value::MakeFloatArray(std::vector<float>&& value, int64_t time) {
+  Value val{NT_FLOAT_ARRAY, time, private_init{}};
+  auto data = std::make_shared<std::vector<float>>(std::move(value));
+  val.m_val.data.arr_float.arr = data->data();
+  val.m_val.data.arr_float.size = data->size();
+  val.m_storage = std::move(data);
+  return val;
+}
+
 Value Value::MakeDoubleArray(std::span<const double> value, int64_t time) {
   Value val{NT_DOUBLE_ARRAY, time, private_init{}};
   auto data = std::make_shared<std::vector<double>>(value.begin(), value.end());
@@ -117,6 +144,15 @@ Value Value::MakeDoubleArray(std::span<const double> value, int64_t time) {
   return val;
 }
 
+Value Value::MakeDoubleArray(std::vector<double>&& value, int64_t time) {
+  Value val{NT_DOUBLE_ARRAY, time, private_init{}};
+  auto data = std::make_shared<std::vector<double>>(std::move(value));
+  val.m_val.data.arr_double.arr = data->data();
+  val.m_val.data.arr_double.size = data->size();
+  val.m_storage = std::move(data);
+  return val;
+}
+
 Value Value::MakeStringArray(std::span<const std::string> value, int64_t time) {
   Value val{NT_STRING_ARRAY, time, private_init{}};
   auto data = std::make_shared<StringArrayStorage>(value);

ntcore/src/main/native/cpp/Value_internal.cpp

@@ -0,0 +1,49 @@
+// 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 "Value_internal.h"
+
+using namespace nt;
+
+Value nt::ConvertNumericValue(const Value& value, NT_Type type) {
+  switch (type) {
+    case NT_INTEGER: {
+      Value newval =
+          Value::MakeInteger(GetNumericAs<int64_t>(value), value.time());
+      newval.SetServerTime(value.server_time());
+      return newval;
+    }
+    case NT_FLOAT: {
+      Value newval = Value::MakeFloat(GetNumericAs<float>(value), value.time());
+      newval.SetServerTime(value.server_time());
+      return newval;
+    }
+    case NT_DOUBLE: {
+      Value newval =
+          Value::MakeDouble(GetNumericAs<double>(value), value.time());
+      newval.SetServerTime(value.server_time());
+      return newval;
+    }
+    case NT_INTEGER_ARRAY: {
+      Value newval = Value::MakeIntegerArray(GetNumericArrayAs<int64_t>(value),
+                                             value.time());
+      newval.SetServerTime(value.server_time());
+      return newval;
+    }
+    case NT_FLOAT_ARRAY: {
+      Value newval =
+          Value::MakeFloatArray(GetNumericArrayAs<float>(value), value.time());
+      newval.SetServerTime(value.server_time());
+      return newval;
+    }
+    case NT_DOUBLE_ARRAY: {
+      Value newval = Value::MakeDoubleArray(GetNumericArrayAs<double>(value),
+                                            value.time());
+      newval.SetServerTime(value.server_time());
+      return newval;
+    }
+    default:
+      return {};
+  }
+}

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

@@ -12,6 +12,7 @@
 
 #include <wpi/MemAlloc.h>
 
+#include "networktables/NetworkTableValue.h"
 #include "ntcore_c.h"
 
 namespace nt {
@@ -56,4 +57,35 @@ O* ConvertToC(const std::basic_string<I>& in, size_t* out_len) {
   return out;
 }
 
+template <typename T>
+T GetNumericAs(const Value& value) {
+  if (value.IsInteger()) {
+    return static_cast<T>(value.GetInteger());
+  } else if (value.IsFloat()) {
+    return static_cast<T>(value.GetFloat());
+  } else if (value.IsDouble()) {
+    return static_cast<T>(value.GetDouble());
+  } else {
+    return {};
+  }
+}
+
+template <typename T>
+std::vector<T> GetNumericArrayAs(const Value& value) {
+  if (value.IsIntegerArray()) {
+    auto arr = value.GetIntegerArray();
+    return {arr.begin(), arr.end()};
+  } else if (value.IsFloatArray()) {
+    auto arr = value.GetFloatArray();
+    return {arr.begin(), arr.end()};
+  } else if (value.IsDoubleArray()) {
+    auto arr = value.GetDoubleArray();
+    return {arr.begin(), arr.end()};
+  } else {
+    return {};
+  }
+}
+
+Value ConvertNumericValue(const Value& value, NT_Type type);
+
 }  // namespace nt

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

@@ -45,6 +45,12 @@ NT_Inst CreateInstance() {
   return Handle{InstanceImpl::Alloc(), 0, Handle::kInstance};
 }
 
+void ResetInstance(NT_Inst inst) {
+  if (auto ii = InstanceImpl::GetTyped(inst, Handle::kInstance)) {
+    ii->Reset();
+  }
+}
+
 void DestroyInstance(NT_Inst inst) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   if (i < 0) {

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

@@ -471,6 +471,18 @@ class Value final {
     return MakeBooleanArray(std::span(value.begin(), value.end()), time);
   }
 
+  /**
+   * Creates a boolean array entry value.
+   *
+   * @param value the value
+   * @param time if nonzero, the creation time to use (instead of the current
+   *             time)
+   * @return The entry value
+   *
+   * @note This function moves the values out of the vector.
+   */
+  static Value MakeBooleanArray(std::vector<int>&& value, int64_t time = 0);
+
   /**
    * Creates an integer array entry value.
    *
@@ -495,6 +507,18 @@ class Value final {
     return MakeIntegerArray(std::span(value.begin(), value.end()), time);
   }
 
+  /**
+   * Creates an integer array entry value.
+   *
+   * @param value the value
+   * @param time if nonzero, the creation time to use (instead of the current
+   *             time)
+   * @return The entry value
+   *
+   * @note This function moves the values out of the vector.
+   */
+  static Value MakeIntegerArray(std::vector<int64_t>&& value, int64_t time = 0);
+
   /**
    * Creates a float array entry value.
    *
@@ -518,6 +542,18 @@ class Value final {
     return MakeFloatArray(std::span(value.begin(), value.end()), time);
   }
 
+  /**
+   * Creates a float array entry value.
+   *
+   * @param value the value
+   * @param time if nonzero, the creation time to use (instead of the current
+   *             time)
+   * @return The entry value
+   *
+   * @note This function moves the values out of the vector.
+   */
+  static Value MakeFloatArray(std::vector<float>&& value, int64_t time = 0);
+
   /**
    * Creates a double array entry value.
    *
@@ -541,6 +577,18 @@ class Value final {
     return MakeDoubleArray(std::span(value.begin(), value.end()), time);
   }
 
+  /**
+   * Creates a double array entry value.
+   *
+   * @param value the value
+   * @param time if nonzero, the creation time to use (instead of the current
+   *             time)
+   * @return The entry value
+   *
+   * @note This function moves the values out of the vector.
+   */
+  static Value MakeDoubleArray(std::vector<double>&& value, int64_t time = 0);
+
   /**
    * Creates a string array entry value.
    *

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

@@ -348,6 +348,13 @@ NT_Inst GetDefaultInstance();
  */
 NT_Inst CreateInstance();
 
+/**
+ * Reset the internals of an instance. Every handle previously associated
+ * with this instance will no longer be valid, except for the instance
+ * handle.
+ */
+void ResetInstance(NT_Inst inst);
+
 /**
  * Destroy an instance.
  * The default instance cannot be destroyed.

ntcore/src/test/native/cpp/LocalStorageTest.cpp

@@ -13,21 +13,26 @@
 #include "gtest/gtest.h"
 #include "net/MockNetworkInterface.h"
 #include "ntcore_c.h"
+#include "ntcore_cpp.h"
 
 using ::testing::_;
+using ::testing::AllOf;
+using ::testing::ElementsAre;
+using ::testing::Field;
+using ::testing::IsEmpty;
+using ::testing::Property;
 using ::testing::Return;
 
 namespace nt {
 
 ::testing::Matcher<const PubSubOptions&> IsPubSubOptions(
     const PubSubOptions& good) {
-  return ::testing::AllOf(
-      ::testing::Field("periodic", &PubSubOptions::periodic, good.periodic),
-      ::testing::Field("pollStorageSize", &PubSubOptions::pollStorageSize,
-                       good.pollStorageSize),
-      ::testing::Field("logging", &PubSubOptions::sendAll, good.sendAll),
-      ::testing::Field("keepDuplicates", &PubSubOptions::keepDuplicates,
-                       good.keepDuplicates));
+  return AllOf(Field("periodic", &PubSubOptions::periodic, good.periodic),
+               Field("pollStorageSize", &PubSubOptions::pollStorageSize,
+                     good.pollStorageSize),
+               Field("logging", &PubSubOptions::sendAll, good.sendAll),
+               Field("keepDuplicates", &PubSubOptions::keepDuplicates,
+                     good.keepDuplicates));
 }
 
 class LocalStorageTest : public ::testing::Test {
@@ -158,7 +163,7 @@ TEST_F(LocalStorageTest, SubscribeNoTypeLocalPubPost) {
   EXPECT_EQ(vals[0].value, true);
   EXPECT_EQ(vals[0].time, 5);
 
-  val = Value::MakeBoolean(true, 6);
+  val = Value::MakeBoolean(false, 6);
   EXPECT_CALL(network, SetValue(pub, val));
   storage.SetEntryValue(pub, val);
 
@@ -221,7 +226,7 @@ TEST_F(LocalStorageTest, EntryNoTypeLocalSet) {
   EXPECT_EQ(vals[0].time, 5);
 
   // normal set with same type
-  val = Value::MakeBoolean(true, 6);
+  val = Value::MakeBoolean(false, 6);
   EXPECT_CALL(network, SetValue(_, val));
   EXPECT_TRUE(storage.SetEntryValue(entry, val));
 
@@ -247,6 +252,9 @@ TEST_F(LocalStorageTest, PubUnpubPub) {
   EXPECT_CALL(network, Publish(_, fooTopic, std::string_view{"foo"},
                                std::string_view{"boolean"}, wpi::json::object(),
                                IsPubSubOptions({})));
+  EXPECT_CALL(logger, Call(NT_LOG_INFO, _, _,
+                           "local subscribe to 'foo' disabled due to type "
+                           "mismatch (wanted 'int', published as 'boolean')"));
   auto pub = storage.Publish(fooTopic, NT_BOOLEAN, "boolean", {}, {});
 
   auto val = Value::MakeBoolean(true, 5);
@@ -329,7 +337,7 @@ TEST_F(LocalStorageTest, LocalSubConflict) {
                                  IsPubSubOptions({})));
   EXPECT_CALL(logger, Call(NT_LOG_INFO, _, _,
                            "local subscribe to 'foo' disabled due to type "
-                           "mismatch (wanted 'int', currently 'boolean')"));
+                           "mismatch (wanted 'int', published as 'boolean')"));
   storage.Subscribe(fooTopic, NT_INTEGER, "int", {});
 }
 
@@ -466,6 +474,12 @@ TEST_F(LocalStorageTest, SetValueEmptyUntypedEntry) {
 }
 
 TEST_F(LocalStorageTest, PublishUntyped) {
+  EXPECT_CALL(
+      logger,
+      Call(
+          NT_LOG_ERROR, _, _,
+          "cannot publish 'foo' with an unassigned type or empty type string"));
+
   EXPECT_EQ(storage.Publish(fooTopic, NT_UNASSIGNED, "", {}, {}), 0u);
 }
 
@@ -473,4 +487,306 @@ TEST_F(LocalStorageTest, SetValueInvalidHandle) {
   EXPECT_FALSE(storage.SetEntryValue(0u, {}));
 }
 
+class LocalStorageDuplicatesTest : public LocalStorageTest {
+ public:
+  void SetupPubSub(bool keepPub, bool keepSub);
+  void SetValues();
+
+  NT_Publisher pub;
+  NT_Subscriber sub;
+  Value val1 = Value::MakeDouble(1.0, 10);
+  Value val2 = Value::MakeDouble(1.0, 20);  // duplicate value
+  Value val3 = Value::MakeDouble(2.0, 30);
+};
+
+void LocalStorageDuplicatesTest::SetupPubSub(bool keepPub, bool keepSub) {
+  PubSubOptions pubOptions;
+  pubOptions.keepDuplicates = keepPub;
+  EXPECT_CALL(network, Publish(_, fooTopic, std::string_view{"foo"},
+                               std::string_view{"double"}, wpi::json::object(),
+                               IsPubSubOptions(pubOptions)));
+  pub = storage.Publish(fooTopic, NT_DOUBLE, "double", {},
+                        {{PubSubOption::KeepDuplicates(keepPub)}});
+
+  PubSubOptions subOptions;
+  subOptions.pollStorageSize = 10;
+  subOptions.keepDuplicates = keepSub;
+  EXPECT_CALL(network, Subscribe(_, wpi::SpanEq({std::string{"foo"}}),
+                                 IsPubSubOptions(subOptions)));
+  sub = storage.Subscribe(
+      fooTopic, NT_DOUBLE, "double",
+      {{PubSubOption::KeepDuplicates(keepSub), PubSubOption::PollStorage(10)}});
+}
+
+void LocalStorageDuplicatesTest::SetValues() {
+  storage.SetEntryValue(pub, val1);
+  storage.SetEntryValue(pub, val2);
+  // verify the timestamp was updated
+  EXPECT_EQ(storage.GetEntryLastChange(sub), val2.time());
+  storage.SetEntryValue(pub, val3);
+}
+
+TEST_F(LocalStorageDuplicatesTest, Defaults) {
+  SetupPubSub(false, false);
+
+  EXPECT_CALL(network, SetValue(pub, val1));
+  EXPECT_CALL(network, SetValue(pub, val3));
+  SetValues();
+
+  // verify 2nd update was dropped locally
+  auto values = storage.ReadQueueDouble(sub);
+  ASSERT_EQ(values.size(), 2u);
+  ASSERT_EQ(values[0].value, val1.GetDouble());
+  ASSERT_EQ(values[0].time, val1.time());
+  ASSERT_EQ(values[1].value, val3.GetDouble());
+  ASSERT_EQ(values[1].time, val3.time());
+}
+
+TEST_F(LocalStorageDuplicatesTest, KeepPub) {
+  SetupPubSub(true, false);
+
+  EXPECT_CALL(network, SetValue(pub, val1)).Times(2);
+  // EXPECT_CALL(network, SetValue(pub, val2));
+  EXPECT_CALL(network, SetValue(pub, val3));
+  SetValues();
+
+  // verify all 3 updates were received locally
+  auto values = storage.ReadQueueDouble(sub);
+  ASSERT_EQ(values.size(), 3u);
+}
+
+TEST_F(LocalStorageDuplicatesTest, KeepSub) {
+  SetupPubSub(false, true);
+
+  // second update should NOT go to the network
+  EXPECT_CALL(network, SetValue(pub, val1));
+  EXPECT_CALL(network, SetValue(pub, val3));
+  SetValues();
+
+  // verify all 3 updates were received locally
+  auto values = storage.ReadQueueDouble(sub);
+  ASSERT_EQ(values.size(), 3u);
+}
+
+TEST_F(LocalStorageDuplicatesTest, FromNetwork) {
+  SetupPubSub(false, false);
+
+  // incoming from the network are treated like a normal local publish
+  auto topic = storage.NetworkAnnounce("foo", "double", {{}}, 0);
+  storage.NetworkSetValue(topic, val1);
+  storage.NetworkSetValue(topic, val2);
+  // verify the timestamp was updated
+  EXPECT_EQ(storage.GetEntryLastChange(sub), val2.time());
+  storage.NetworkSetValue(topic, val3);
+
+  // verify 2nd update was dropped locally
+  auto values = storage.ReadQueueDouble(sub);
+  ASSERT_EQ(values.size(), 2u);
+  ASSERT_EQ(values[0].value, val1.GetDouble());
+  ASSERT_EQ(values[0].time, val1.time());
+  ASSERT_EQ(values[1].value, val3.GetDouble());
+  ASSERT_EQ(values[1].time, val3.time());
+}
+
+class LocalStorageNumberVariantsTest : public LocalStorageTest {
+ public:
+  void CreateSubscriber(NT_Handle* handle, std::string_view name, NT_Type type,
+                        std::string_view typeStr);
+  void CreateSubscribers();
+  void CreateSubscribersArray();
+
+  NT_Subscriber sub1, sub2, sub3, sub4;
+  NT_Entry entry;
+
+  struct SubEntry {
+    SubEntry(NT_Handle subentry, NT_Type type, std::string_view name)
+        : subentry{subentry}, type{type}, name{name} {}
+    NT_Handle subentry;
+    NT_Type type;
+    std::string name;
+  };
+  std::vector<SubEntry> subentries;
+};
+
+void LocalStorageNumberVariantsTest::CreateSubscriber(
+    NT_Handle* handle, std::string_view name, NT_Type type,
+    std::string_view typeStr) {
+  *handle = storage.Subscribe(fooTopic, type, typeStr, {});
+  subentries.emplace_back(*handle, type, name);
+}
+
+void LocalStorageNumberVariantsTest::CreateSubscribers() {
+  EXPECT_CALL(logger,
+              Call(NT_LOG_INFO, _, _,
+                   "local subscribe to 'foo' disabled due to type "
+                   "mismatch (wanted 'boolean', published as 'double')"));
+  CreateSubscriber(&sub1, "subDouble", NT_DOUBLE, "double");
+  CreateSubscriber(&sub2, "subInteger", NT_INTEGER, "int");
+  CreateSubscriber(&sub3, "subFloat", NT_FLOAT, "float");
+  CreateSubscriber(&sub4, "subBoolean", NT_BOOLEAN, "boolean");
+  entry = storage.GetEntry("foo");
+  subentries.emplace_back(entry, NT_UNASSIGNED, "entry");
+}
+
+void LocalStorageNumberVariantsTest::CreateSubscribersArray() {
+  EXPECT_CALL(logger,
+              Call(NT_LOG_INFO, _, _,
+                   "local subscribe to 'foo' disabled due to type "
+                   "mismatch (wanted 'boolean[]', published as 'double[]')"));
+  CreateSubscriber(&sub1, "subDouble", NT_DOUBLE_ARRAY, "double[]");
+  CreateSubscriber(&sub2, "subInteger", NT_INTEGER_ARRAY, "int[]");
+  CreateSubscriber(&sub3, "subFloat", NT_FLOAT_ARRAY, "float[]");
+  CreateSubscriber(&sub4, "subBoolean", NT_BOOLEAN_ARRAY, "boolean[]");
+  entry = storage.GetEntry("foo");
+  subentries.emplace_back(entry, NT_UNASSIGNED, "entry");
+}
+
+TEST_F(LocalStorageNumberVariantsTest, GetEntryPubAfter) {
+  EXPECT_CALL(network, Subscribe(_, _, _)).Times(5);
+  EXPECT_CALL(network, Publish(_, _, _, _, _, _)).Times(1);
+  EXPECT_CALL(network, SetValue(_, _)).Times(1);
+  CreateSubscribers();
+  auto pub = storage.Publish(fooTopic, NT_DOUBLE, "double", {}, {});
+  storage.SetEntryValue(pub, Value::MakeDouble(1.0, 50));
+  // all subscribers get the actual type and time
+  for (auto&& subentry : subentries) {
+    SCOPED_TRACE(subentry.name);
+    EXPECT_EQ(storage.GetEntryType(subentry.subentry), NT_DOUBLE);
+    EXPECT_EQ(storage.GetEntryLastChange(subentry.subentry), 50);
+  }
+  // for subscribers, they get a converted value or nothing on mismatch
+  EXPECT_EQ(storage.GetEntryValue(sub1), Value::MakeDouble(1.0, 50));
+  EXPECT_EQ(storage.GetEntryValue(sub2), Value::MakeInteger(1, 50));
+  EXPECT_EQ(storage.GetEntryValue(sub3), Value::MakeFloat(1.0, 50));
+  EXPECT_EQ(storage.GetEntryValue(sub4), Value{});
+  // entrys just get whatever the value is
+  EXPECT_EQ(storage.GetEntryValue(entry), Value::MakeDouble(1.0, 50));
+}
+
+TEST_F(LocalStorageNumberVariantsTest, GetEntryPubBefore) {
+  EXPECT_CALL(network, Subscribe(_, _, _)).Times(5);
+  EXPECT_CALL(network, Publish(_, _, _, _, _, _)).Times(1);
+  EXPECT_CALL(network, SetValue(_, _)).Times(1);
+  auto pub = storage.Publish(fooTopic, NT_DOUBLE, "double", {}, {});
+  CreateSubscribers();
+  storage.SetEntryValue(pub, Value::MakeDouble(1.0, 50));
+  // all subscribers get the actual type and time
+  for (auto&& subentry : subentries) {
+    SCOPED_TRACE(subentry.name);
+    EXPECT_EQ(storage.GetEntryType(subentry.subentry), NT_DOUBLE);
+    EXPECT_EQ(storage.GetEntryLastChange(subentry.subentry), 50);
+  }
+  // for subscribers, they get a converted value or nothing on mismatch
+  EXPECT_EQ(storage.GetEntryValue(sub1), Value::MakeDouble(1.0, 50));
+  EXPECT_EQ(storage.GetEntryValue(sub2), Value::MakeInteger(1, 50));
+  EXPECT_EQ(storage.GetEntryValue(sub3), Value::MakeFloat(1.0, 50));
+  EXPECT_EQ(storage.GetEntryValue(sub4), Value{});
+  // entrys just get whatever the value is
+  EXPECT_EQ(storage.GetEntryValue(entry), Value::MakeDouble(1.0, 50));
+}
+
+template <typename T>
+::testing::Matcher<const T&> TSEq(auto value, int64_t time) {
+  return AllOf(Field("value", &T::value, value), Field("time", &T::time, time));
+}
+
+TEST_F(LocalStorageNumberVariantsTest, GetAtomic) {
+  EXPECT_CALL(network, Subscribe(_, _, _)).Times(5);
+  EXPECT_CALL(network, Publish(_, _, _, _, _, _)).Times(1);
+  EXPECT_CALL(network, SetValue(_, _)).Times(1);
+  auto pub = storage.Publish(fooTopic, NT_DOUBLE, "double", {}, {});
+  CreateSubscribers();
+  storage.SetEntryValue(pub, Value::MakeDouble(1.0, 50));
+
+  for (auto&& subentry : subentries) {
+    SCOPED_TRACE(subentry.name);
+    EXPECT_THAT(storage.GetAtomicDouble(subentry.subentry, 0),
+                TSEq<TimestampedDouble>(1.0, 50));
+    EXPECT_THAT(storage.GetAtomicInteger(subentry.subentry, 0),
+                TSEq<TimestampedInteger>(1, 50));
+    EXPECT_THAT(storage.GetAtomicFloat(subentry.subentry, 0),
+                TSEq<TimestampedFloat>(1.0, 50));
+    EXPECT_THAT(storage.GetAtomicBoolean(subentry.subentry, false),
+                TSEq<TimestampedBoolean>(false, 0));
+  }
+}
+
+template <typename T, typename U>
+::testing::Matcher<const T&> TSSpanEq(std::span<U> value, int64_t time) {
+  return AllOf(
+      Field("value", &T::value, wpi::SpanEq(std::span<const U>(value))),
+      Field("time", &T::time, time));
+}
+
+TEST_F(LocalStorageNumberVariantsTest, GetAtomicArray) {
+  EXPECT_CALL(network, Subscribe(_, _, _)).Times(5);
+  EXPECT_CALL(network, Publish(_, _, _, _, _, _)).Times(1);
+  EXPECT_CALL(network, SetValue(_, _)).Times(1);
+  auto pub = storage.Publish(fooTopic, NT_DOUBLE_ARRAY, "double[]", {}, {});
+  CreateSubscribersArray();
+  storage.SetEntryValue(pub, Value::MakeDoubleArray({1.0}, 50));
+
+  for (auto&& subentry : subentries) {
+    SCOPED_TRACE(subentry.name);
+    double doubleVal = 1.0;
+    EXPECT_THAT(storage.GetAtomicDoubleArray(subentry.subentry, {}),
+                TSSpanEq<TimestampedDoubleArray>(std::span{&doubleVal, 1}, 50));
+    int64_t intVal = 1;
+    EXPECT_THAT(storage.GetAtomicIntegerArray(subentry.subentry, {}),
+                TSSpanEq<TimestampedIntegerArray>(std::span{&intVal, 1}, 50));
+    float floatVal = 1.0;
+    EXPECT_THAT(storage.GetAtomicFloatArray(subentry.subentry, {}),
+                TSSpanEq<TimestampedFloatArray>(std::span{&floatVal, 1}, 50));
+    EXPECT_THAT(storage.GetAtomicBooleanArray(subentry.subentry, {}),
+                TSSpanEq<TimestampedBooleanArray>(std::span<int>{}, 0));
+  }
+}
+
+TEST_F(LocalStorageNumberVariantsTest, ReadQueue) {
+  EXPECT_CALL(network, Subscribe(_, _, _)).Times(5);
+  EXPECT_CALL(network, Publish(_, _, _, _, _, _)).Times(1);
+  EXPECT_CALL(network, SetValue(_, _)).Times(4);
+  auto pub = storage.Publish(fooTopic, NT_DOUBLE, "double", {}, {});
+  CreateSubscribers();
+
+  storage.SetEntryValue(pub, Value::MakeDouble(1.0, 50));
+  for (auto&& subentry : subentries) {
+    SCOPED_TRACE(subentry.name);
+    if (subentry.type == NT_BOOLEAN) {
+      EXPECT_THAT(storage.ReadQueueDouble(subentry.subentry), IsEmpty());
+    } else {
+      EXPECT_THAT(storage.ReadQueueDouble(subentry.subentry),
+                  ElementsAre(TSEq<TimestampedDouble>(1.0, 50)));
+    }
+  }
+
+  storage.SetEntryValue(pub, Value::MakeDouble(2.0, 50));
+  for (auto&& subentry : subentries) {
+    SCOPED_TRACE(subentry.name);
+    if (subentry.type == NT_BOOLEAN) {
+      EXPECT_THAT(storage.ReadQueueInteger(subentry.subentry), IsEmpty());
+    } else {
+      EXPECT_THAT(storage.ReadQueueInteger(subentry.subentry),
+                  ElementsAre(TSEq<TimestampedInteger>(2, 50)));
+    }
+  }
+
+  storage.SetEntryValue(pub, Value::MakeDouble(3.0, 50));
+  for (auto&& subentry : subentries) {
+    SCOPED_TRACE(subentry.name);
+    if (subentry.type == NT_BOOLEAN) {
+      EXPECT_THAT(storage.ReadQueueFloat(subentry.subentry), IsEmpty());
+    } else {
+      EXPECT_THAT(storage.ReadQueueFloat(subentry.subentry),
+                  ElementsAre(TSEq<TimestampedFloat>(3.0, 50)));
+    }
+  }
+
+  storage.SetEntryValue(pub, Value::MakeDouble(4.0, 50));
+  for (auto&& subentry : subentries) {
+    SCOPED_TRACE(subentry.name);
+    EXPECT_THAT(storage.ReadQueueBoolean(subentry.subentry), IsEmpty());
+  }
+}
+
 }  // namespace nt

ntcore/src/test/native/cpp/NetworkTableTest.cpp

@@ -89,3 +89,15 @@ TEST_F(NetworkTableTest, EmptyOrNoSlash) {
   ASSERT_TRUE(inst.GetEntry("/testkey").Exists());
   nt::NetworkTableInstance::Destroy(inst);
 }
+
+TEST_F(NetworkTableTest, ResetInstance) {
+  auto inst = nt::NetworkTableInstance::Create();
+  auto nt = inst.GetTable("containskey");
+  ASSERT_FALSE(nt->ContainsKey("testkey"));
+  nt->PutNumber("testkey", 5);
+  ASSERT_TRUE(nt->ContainsKey("testkey"));
+  ASSERT_TRUE(inst.GetEntry("/containskey/testkey").Exists());
+  nt::ResetInstance(inst.GetHandle());
+  ASSERT_FALSE(nt->ContainsKey("testkey"));
+  nt::NetworkTableInstance::Destroy(inst);
+}

ntcore/src/test/native/cpp/TestPrinters.cpp

@@ -121,37 +121,37 @@ void PrintTo(const Value& value, std::ostream* os) {
     case NT_UNASSIGNED:
       break;
     case NT_BOOLEAN:
-      *os << (value.GetBoolean() ? "true" : "false");
+      *os << "boolean, " << (value.GetBoolean() ? "true" : "false");
       break;
     case NT_DOUBLE:
-      *os << value.GetDouble();
+      *os << "double, " << value.GetDouble();
       break;
     case NT_FLOAT:
-      *os << value.GetFloat();
+      *os << "float, " << value.GetFloat();
       break;
     case NT_INTEGER:
-      *os << value.GetInteger();
+      *os << "int, " << value.GetInteger();
       break;
     case NT_STRING:
-      *os << '"' << value.GetString() << '"';
+      *os << "string, \"" << value.GetString() << '"';
       break;
     case NT_RAW:
-      *os << ::testing::PrintToString(value.GetRaw());
+      *os << "raw, " << ::testing::PrintToString(value.GetRaw());
       break;
     case NT_BOOLEAN_ARRAY:
-      *os << ::testing::PrintToString(value.GetBooleanArray());
+      *os << "boolean[], " << ::testing::PrintToString(value.GetBooleanArray());
       break;
     case NT_DOUBLE_ARRAY:
-      *os << ::testing::PrintToString(value.GetDoubleArray());
+      *os << "double[], " << ::testing::PrintToString(value.GetDoubleArray());
       break;
     case NT_FLOAT_ARRAY:
-      *os << ::testing::PrintToString(value.GetFloatArray());
+      *os << "float[], " << ::testing::PrintToString(value.GetFloatArray());
       break;
     case NT_INTEGER_ARRAY:
-      *os << ::testing::PrintToString(value.GetIntegerArray());
+      *os << "int[], " << ::testing::PrintToString(value.GetIntegerArray());
       break;
     case NT_STRING_ARRAY:
-      *os << ::testing::PrintToString(value.GetStringArray());
+      *os << "string[], " << ::testing::PrintToString(value.GetStringArray());
       break;
     default:
       *os << "UNKNOWN TYPE " << value.type();

outlineviewer/publish.gradle

@@ -37,6 +37,8 @@ model {
                             into("MacOS") { with copySpec { from binary.executable.file } }
                             into("Resources") { with copySpec { from icon } }
 
+                            inputs.property "HasDeveloperId", project.hasProperty("developerID")
+
                             doLast {
                                 if (project.hasProperty("developerID")) {
                                     // Get path to binary.

roborioteamnumbersetter/publish.gradle

@@ -37,6 +37,8 @@ model {
                             into("MacOS") { with copySpec { from binary.executable.file } }
                             into("Resources") { with copySpec { from icon } }
 
+                            inputs.property "HasDeveloperId", project.hasProperty("developerID")
+
                             doLast {
                                 if (project.hasProperty("developerID")) {
                                     // Get path to binary.

settings.gradle

@@ -52,6 +52,7 @@ include 'myRobot'
 include 'docs'
 include 'msvcruntime'
 include 'ntcoreffi'
+include 'apriltag'
 
 buildCache {
     def cred = {

shared/config.gradle

@@ -15,9 +15,9 @@ nativeUtils {
         configureDependencies {
             wpiVersion = "-1"
             niLibVersion = "2023.1.0"
-            opencvVersion = "4.6.0-2"
-            googleTestVersion = "1.11.0-3"
-            imguiVersion = "1.88-8"
+            opencvVersion = "4.6.0-3"
+            googleTestVersion = "1.11.0-4"
+            imguiVersion = "1.88-9"
             wpimathVersion = "-1"
         }
     }
@@ -34,14 +34,6 @@ nativeUtils.setSinglePrintPerPlatform()
 nativeUtils.enableSourceLink()
 
 nativeUtils.platformConfigs.each {
-    if (it.name.contains('windows')) {
-        it.cppCompiler.args.remove("/std:c++17")
-        it.cppCompiler.args.add("/std:c++20")
-        return
-    } else if (it.name == 'linuxx86-64' || it.name == 'osxx86-64' || it.name == 'osxarm64') {
-        it.cppCompiler.args.remove("-std=c++17")
-        it.cppCompiler.args.add("-std=c++20")
-    }
     if (it.name.contains('osx')) {
         it.linker.args << '-Wl,-rpath,\'@loader_path\''
         it.linker.args << "-headerpad_max_install_names"

shared/jni/setupBuild.gradle

@@ -104,6 +104,10 @@ model {
                 baseName = nativeName + 'jni'
             }
 
+            if (project.hasProperty('skipJniSymbols')) {
+                checkSkipSymbols = skipJniSymbols
+            }
+
             enableCheckTask !project.hasProperty('skipJniCheck')
             javaCompileTasks << compileJava
             jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.roborio)
@@ -148,6 +152,10 @@ model {
                 baseName = nativeName + 'jni'
             }
 
+            if (project.hasProperty('skipJniSymbols')) {
+                checkSkipSymbols = skipJniSymbols
+            }
+
             enableCheckTask !project.hasProperty('skipJniCheck')
             javaCompileTasks << compileJava
             jniCrossCompileOptions << JniCrossCompileOptions(nativeUtils.wpi.platforms.roborio)

shared/libssh.gradle

@@ -6,7 +6,7 @@ nativeUtils {
             headerClassifier = "headers"
             sourceClassifier = "sources"
             ext = "zip"
-            version = '0.95-5'
+            version = '0.95-6'
             targetPlatforms.addAll(nativeUtils.wpi.platforms.allPlatforms)
         }
     }

upstream_utils/update_eigen.py

@@ -73,12 +73,8 @@ def eigen_inclusions(dp, f):
     ]
     modules_rgx = r"|".join("/" + m for m in modules)
 
-    # "Std" matches StdDeque, StdList, and StdVector headers
-    if re.search(modules_rgx, abspath) or "Std" in f:
-        return True
-    else:
-        # Exclude all other modules
-        return False
+    # "Std" matches StdDeque, StdList, and StdVector headers. Other modules are excluded.
+    return bool(re.search(modules_rgx, abspath) or "Std" in f)
 
 
 def unsupported_inclusions(dp, f):

upstream_utils/update_llvm.py

@@ -126,12 +126,12 @@ def overwrite_files(wpiutil_files, llvm_files):
 
     for wpi_file in wpiutil_files:
         wpi_base_name = os.path.basename(wpi_file)
-        if wpi_base_name not in llvm_files:
-            if wpi_base_name not in unmatched_files_whitelist:
-                print(f"No file match for {wpi_file}, check if LLVM deleted it")
-        else:
+        if wpi_base_name in llvm_files:
             shutil.copyfile(llvm_files[wpi_base_name], wpi_file)
 
+        elif wpi_base_name not in unmatched_files_whitelist:
+            print(f"No file match for {wpi_file}, check if LLVM deleted it")
+
 
 def overwrite_source(wpiutil_root, llvm_root):
     llvm_files = flattened_llvm_files(

wpilib-config.cmake.in

@@ -5,6 +5,7 @@ find_dependency(Threads)
 @LIBUV_SYSTEM_REPLACE@
 @EIGEN_SYSTEM_REPLACE@
 @WPIUTIL_DEP_REPLACE@
+@WPINET_DEP_REPLACE@
 @NTCORE_DEP_REPLACE@
 @CSCORE_DEP_REPLACE@
 @CAMERASERVER_DEP_REPLACE@

wpilibNewCommands/WPILibNewCommands.json

@@ -30,8 +30,7 @@
               "windowsx86-64",
               "windowsx86",
               "linuxx86-64",
-              "osxx86-64",
-              "osxarm64"
+              "osxuniversal"
           ]
       }
   ]

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

@@ -56,6 +56,12 @@ public abstract class CommandBase implements Sendable, Command {
     SendableRegistry.setName(this, name);
   }
 
+  @Override
+  public CommandBase withName(String name) {
+    this.setName(name);
+    return this;
+  }
+
   /**
    * Gets the subsystem name of this Command.
    *

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

@@ -405,10 +405,6 @@ public final class CommandScheduler implements NTSendable, AutoCloseable {
       throw new IllegalArgumentException("Default commands must require their subsystem!");
     }
 
-    if (defaultCommand.isFinished()) {
-      throw new IllegalArgumentException("Default commands should not end!");
-    }
-
     if (defaultCommand.getInterruptionBehavior() == InterruptionBehavior.kCancelIncoming) {
       DriverStation.reportWarning(
           "Registering a non-interruptible default command!\n"

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

@@ -21,7 +21,7 @@ public final class Commands {
    *
    * @return the command
    */
-  public static Command none() {
+  public static CommandBase none() {
     return new InstantCommand();
   }
 
@@ -35,7 +35,7 @@ public final class Commands {
    * @return the command
    * @see InstantCommand
    */
-  public static Command runOnce(Runnable action, Subsystem... requirements) {
+  public static CommandBase runOnce(Runnable action, Subsystem... requirements) {
     return new InstantCommand(action, requirements);
   }
 
@@ -47,7 +47,7 @@ public final class Commands {
    * @return the command
    * @see RunCommand
    */
-  public static Command run(Runnable action, Subsystem... requirements) {
+  public static CommandBase run(Runnable action, Subsystem... requirements) {
     return new RunCommand(action, requirements);
   }
 
@@ -61,7 +61,7 @@ public final class Commands {
    * @return the command
    * @see StartEndCommand
    */
-  public static Command startEnd(Runnable start, Runnable end, Subsystem... requirements) {
+  public static CommandBase startEnd(Runnable start, Runnable end, Subsystem... requirements) {
     return new StartEndCommand(start, end, requirements);
   }
 
@@ -74,7 +74,7 @@ public final class Commands {
    * @param requirements subsystems the action requires
    * @return the command
    */
-  public static Command runEnd(Runnable run, Runnable end, Subsystem... requirements) {
+  public static CommandBase runEnd(Runnable run, Runnable end, Subsystem... requirements) {
     requireNonNullParam(end, "end", "Command.runEnd");
     return new FunctionalCommand(
         () -> {}, run, interrupted -> end.run(), () -> false, requirements);
@@ -87,7 +87,7 @@ public final class Commands {
    * @return the command
    * @see PrintCommand
    */
-  public static Command print(String message) {
+  public static CommandBase print(String message) {
     return new PrintCommand(message);
   }
 
@@ -100,7 +100,7 @@ public final class Commands {
    * @return the command
    * @see WaitCommand
    */
-  public static Command wait(double seconds) {
+  public static CommandBase wait(double seconds) {
     return new WaitCommand(seconds);
   }
 
@@ -111,7 +111,7 @@ public final class Commands {
    * @return the command
    * @see WaitUntilCommand
    */
-  public static Command waitUntil(BooleanSupplier condition) {
+  public static CommandBase waitUntil(BooleanSupplier condition) {
     return new WaitUntilCommand(condition);
   }
 
@@ -126,7 +126,7 @@ public final class Commands {
    * @return the command
    * @see ConditionalCommand
    */
-  public static Command either(Command onTrue, Command onFalse, BooleanSupplier selector) {
+  public static CommandBase either(Command onTrue, Command onFalse, BooleanSupplier selector) {
     return new ConditionalCommand(onTrue, onFalse, selector);
   }
 
@@ -138,7 +138,7 @@ public final class Commands {
    * @return the command
    * @see SelectCommand
    */
-  public static Command select(Map<Object, Command> commands, Supplier<Object> selector) {
+  public static CommandBase select(Map<Object, Command> commands, Supplier<Object> selector) {
     return new SelectCommand(commands, selector);
   }
 
@@ -149,7 +149,7 @@ public final class Commands {
    * @return the command group
    * @see SequentialCommandGroup
    */
-  public static Command sequence(Command... commands) {
+  public static CommandBase sequence(Command... commands) {
     return new SequentialCommandGroup(commands);
   }
 
@@ -164,7 +164,7 @@ public final class Commands {
    * @see SequentialCommandGroup
    * @see Command#repeatedly()
    */
-  public static Command repeatingSequence(Command... commands) {
+  public static CommandBase repeatingSequence(Command... commands) {
     return sequence(commands).repeatedly();
   }
 
@@ -175,7 +175,7 @@ public final class Commands {
    * @return the command
    * @see ParallelCommandGroup
    */
-  public static Command parallel(Command... commands) {
+  public static CommandBase parallel(Command... commands) {
     return new ParallelCommandGroup(commands);
   }
 
@@ -187,7 +187,7 @@ public final class Commands {
    * @return the command group
    * @see ParallelRaceGroup
    */
-  public static Command race(Command... commands) {
+  public static CommandBase race(Command... commands) {
     return new ParallelRaceGroup(commands);
   }
 
@@ -200,7 +200,7 @@ public final class Commands {
    * @return the command group
    * @see ParallelDeadlineGroup
    */
-  public static Command deadline(Command deadline, Command... commands) {
+  public static CommandBase deadline(Command deadline, Command... commands) {
     return new ParallelDeadlineGroup(deadline, commands);
   }
 

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

@@ -205,6 +205,27 @@ public class CommandPS4Controller extends CommandGenericHID {
     return m_hid.cross(loop).castTo(Trigger::new);
   }
 
+  /**
+   * Constructs an event instance around the triangle button's digital signal.
+   *
+   * @return an event instance representing the triangle button's digital signal attached to the
+   *     {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   */
+  public Trigger triangle() {
+    return triangle(CommandScheduler.getInstance().getDefaultButtonLoop());
+  }
+
+  /**
+   * Constructs an event instance around the triangle button's digital signal.
+   *
+   * @param loop the event loop instance to attach the event to.
+   * @return an event instance representing the triangle button's digital signal attached to the
+   *     given loop.
+   */
+  public Trigger triangle(EventLoop loop) {
+    return m_hid.triangle(loop).castTo(Trigger::new);
+  }
+
   /**
    * Constructs an event instance around the circle button's digital signal.
    *

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

@@ -17,8 +17,13 @@ import java.util.function.BiFunction;
 import java.util.function.BooleanSupplier;
 
 /**
- * This class is a wrapper around {@link BooleanEvent}, providing an easy way to link commands to
- * digital inputs.
+ * This class provides an easy way to link commands to conditions.
+ *
+ * <p>It is very easy to link a button to a command. For instance, you could link the trigger button
+ * of a joystick to a "score" command.
+ *
+ * <p>Triggers can easily be composed for advanced functionality using the {@link
+ * #and(BooleanSupplier)}, {@link #or(BooleanSupplier)}, {@link #negate()} operators.
  *
  * <p>This class is provided by the NewCommands VendorDep
  */
@@ -26,13 +31,13 @@ public class Trigger implements BooleanSupplier {
   private final BooleanEvent m_event;
 
   /**
-   * Creates a new trigger with the given condition/digital signal.
+   * Creates a new trigger based on the given condition.
    *
-   * @param loop the loop that polls this trigger
-   * @param signal the digital signal represented.
+   * @param loop The loop instance that polls this trigger.
+   * @param condition the condition represented by this trigger
    */
-  public Trigger(EventLoop loop, BooleanSupplier signal) {
-    m_event = new BooleanEvent(loop, signal);
+  public Trigger(EventLoop loop, BooleanSupplier condition) {
+    m_event = new BooleanEvent(loop, condition);
   }
 
   /**
@@ -47,24 +52,24 @@ public class Trigger implements BooleanSupplier {
   }
 
   /**
-   * Creates a new trigger with the given condition/digital signal.
+   * Creates a new trigger based on the given condition.
    *
-   * <p>Polled by the {@link CommandScheduler#getDefaultButtonLoop() default scheduler button loop}.
+   * <p>Polled by the default scheduler button loop.
    *
-   * @param signal the digital signal represented.
+   * @param condition the condition represented by this trigger
    */
-  public Trigger(BooleanSupplier signal) {
-    this(CommandScheduler.getInstance().getDefaultButtonLoop(), signal);
+  public Trigger(BooleanSupplier condition) {
+    this(CommandScheduler.getInstance().getDefaultButtonLoop(), condition);
   }
 
-  /** Creates a new trigger that is always inactive. */
+  /** Creates a new trigger that is always `false`. */
   @Deprecated
   public Trigger() {
     this(() -> false);
   }
 
   /**
-   * Starts the given command whenever the signal rises from the low state to the high state.
+   * Starts the given command whenever the condition changes from `false` to `true`.
    *
    * @param command the command to start
    * @return this trigger, so calls can be chained
@@ -77,7 +82,7 @@ public class Trigger implements BooleanSupplier {
   }
 
   /**
-   * Starts the given command whenever the signal falls from the high state to the low state.
+   * Starts the given command whenever the condition changes from `true` to `false`.
    *
    * @param command the command to start
    * @return this trigger, so calls can be chained
@@ -90,10 +95,11 @@ public class Trigger implements BooleanSupplier {
   }
 
   /**
-   * Starts the given command when the signal rises to the high state and cancels it when the signal
-   * falls.
+   * Starts the given command when the condition changes to `true` and cancels it when the condition
+   * changes to `false`.
    *
-   * <p>Doesn't re-start the command in-between.
+   * <p>Doesn't re-start the command if it ends while the condition is still `true`. If the command
+   * should restart, see {@link edu.wpi.first.wpilibj2.command.RepeatCommand}.
    *
    * @param command the command to start
    * @return this trigger, so calls can be chained
@@ -106,10 +112,11 @@ public class Trigger implements BooleanSupplier {
   }
 
   /**
-   * Starts the given command when the signal falls to the low state and cancels it when the signal
-   * rises.
+   * Starts the given command when the condition changes to `false` and cancels it when the
+   * condition changes to `true`.
    *
-   * <p>Does not re-start the command in-between.
+   * <p>Doesn't re-start the command if it ends while the condition is still `false`. If the command
+   * should restart, see {@link edu.wpi.first.wpilibj2.command.RepeatCommand}.
    *
    * @param command the command to start
    * @return this trigger, so calls can be chained
@@ -122,7 +129,7 @@ public class Trigger implements BooleanSupplier {
   }
 
   /**
-   * Toggles a command when the signal rises from the low state to the high state.
+   * Toggles a command when the condition changes from `false` to `true`.
    *
    * @param command the command to toggle
    * @return this trigger, so calls can be chained
@@ -143,7 +150,7 @@ public class Trigger implements BooleanSupplier {
   }
 
   /**
-   * Toggles a command when the signal rises from the low state to the high state.
+   * Toggles a command when the condition changes from `true` to the low state.
    *
    * @param command the command to toggle
    * @return this trigger, so calls can be chained

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

@@ -4,6 +4,8 @@
 
 #include "frc2/command/CommandPtr.h"
 
+#include <frc/Errors.h>
+
 #include "frc2/command/CommandScheduler.h"
 #include "frc2/command/ConditionalCommand.h"
 #include "frc2/command/InstantCommand.h"
@@ -20,12 +22,21 @@
 
 using namespace frc2;
 
+void CommandPtr::AssertValid() const {
+  if (!m_ptr) {
+    throw FRC_MakeError(frc::err::CommandIllegalUse,
+                        "Moved-from CommandPtr object used!");
+  }
+}
+
 CommandPtr CommandPtr::Repeatedly() && {
+  AssertValid();
   m_ptr = std::make_unique<RepeatCommand>(std::move(m_ptr));
   return std::move(*this);
 }
 
 CommandPtr CommandPtr::AsProxy() && {
+  AssertValid();
   m_ptr = std::make_unique<ProxyScheduleCommand>(std::move(m_ptr));
   return std::move(*this);
 }
@@ -44,6 +55,7 @@ class RunsWhenDisabledCommand : public WrapperCommand {
 };
 
 CommandPtr CommandPtr::IgnoringDisable(bool doesRunWhenDisabled) && {
+  AssertValid();
   m_ptr = std::make_unique<RunsWhenDisabledCommand>(std::move(m_ptr),
                                                     doesRunWhenDisabled);
   return std::move(*this);
@@ -67,6 +79,7 @@ class InterruptBehaviorCommand : public WrapperCommand {
 
 CommandPtr CommandPtr::WithInterruptBehavior(
     InterruptionBehavior interruptBehavior) && {
+  AssertValid();
   m_ptr = std::make_unique<InterruptBehaviorCommand>(std::move(m_ptr),
                                                      interruptBehavior);
   return std::move(*this);
@@ -74,6 +87,7 @@ CommandPtr CommandPtr::WithInterruptBehavior(
 
 CommandPtr CommandPtr::AndThen(std::function<void()> toRun,
                                std::span<Subsystem* const> requirements) && {
+  AssertValid();
   return std::move(*this).AndThen(CommandPtr(
       std::make_unique<InstantCommand>(std::move(toRun), requirements)));
 }
@@ -81,11 +95,13 @@ CommandPtr CommandPtr::AndThen(std::function<void()> toRun,
 CommandPtr CommandPtr::AndThen(
     std::function<void()> toRun,
     std::initializer_list<Subsystem*> requirements) && {
+  AssertValid();
   return std::move(*this).AndThen(CommandPtr(
       std::make_unique<InstantCommand>(std::move(toRun), requirements)));
 }
 
 CommandPtr CommandPtr::AndThen(CommandPtr&& next) && {
+  AssertValid();
   std::vector<std::unique_ptr<Command>> temp;
   temp.emplace_back(std::move(m_ptr));
   temp.emplace_back(std::move(next).Unwrap());
@@ -95,6 +111,7 @@ CommandPtr CommandPtr::AndThen(CommandPtr&& next) && {
 
 CommandPtr CommandPtr::BeforeStarting(
     std::function<void()> toRun, std::span<Subsystem* const> requirements) && {
+  AssertValid();
   return std::move(*this).BeforeStarting(CommandPtr(
       std::make_unique<InstantCommand>(std::move(toRun), requirements)));
 }
@@ -102,11 +119,13 @@ CommandPtr CommandPtr::BeforeStarting(
 CommandPtr CommandPtr::BeforeStarting(
     std::function<void()> toRun,
     std::initializer_list<Subsystem*> requirements) && {
+  AssertValid();
   return std::move(*this).BeforeStarting(CommandPtr(
       std::make_unique<InstantCommand>(std::move(toRun), requirements)));
 }
 
 CommandPtr CommandPtr::BeforeStarting(CommandPtr&& before) && {
+  AssertValid();
   std::vector<std::unique_ptr<Command>> temp;
   temp.emplace_back(std::move(before).Unwrap());
   temp.emplace_back(std::move(m_ptr));
@@ -115,6 +134,7 @@ CommandPtr CommandPtr::BeforeStarting(CommandPtr&& before) && {
 }
 
 CommandPtr CommandPtr::WithTimeout(units::second_t duration) && {
+  AssertValid();
   std::vector<std::unique_ptr<Command>> temp;
   temp.emplace_back(std::make_unique<WaitCommand>(duration));
   temp.emplace_back(std::move(m_ptr));
@@ -123,6 +143,7 @@ CommandPtr CommandPtr::WithTimeout(units::second_t duration) && {
 }
 
 CommandPtr CommandPtr::Until(std::function<bool()> condition) && {
+  AssertValid();
   std::vector<std::unique_ptr<Command>> temp;
   temp.emplace_back(std::make_unique<WaitUntilCommand>(std::move(condition)));
   temp.emplace_back(std::move(m_ptr));
@@ -131,6 +152,7 @@ CommandPtr CommandPtr::Until(std::function<bool()> condition) && {
 }
 
 CommandPtr CommandPtr::Unless(std::function<bool()> condition) && {
+  AssertValid();
   m_ptr = std::make_unique<ConditionalCommand>(
       std::make_unique<InstantCommand>(), std::move(m_ptr),
       std::move(condition));
@@ -138,6 +160,7 @@ CommandPtr CommandPtr::Unless(std::function<bool()> condition) && {
 }
 
 CommandPtr CommandPtr::DeadlineWith(CommandPtr&& parallel) && {
+  AssertValid();
   std::vector<std::unique_ptr<Command>> vec;
   vec.emplace_back(std::move(parallel).Unwrap());
   m_ptr =
@@ -146,6 +169,7 @@ CommandPtr CommandPtr::DeadlineWith(CommandPtr&& parallel) && {
 }
 
 CommandPtr CommandPtr::AlongWith(CommandPtr&& parallel) && {
+  AssertValid();
   std::vector<std::unique_ptr<Command>> vec;
   vec.emplace_back(std::move(m_ptr));
   vec.emplace_back(std::move(parallel).Unwrap());
@@ -154,6 +178,7 @@ CommandPtr CommandPtr::AlongWith(CommandPtr&& parallel) && {
 }
 
 CommandPtr CommandPtr::RaceWith(CommandPtr&& parallel) && {
+  AssertValid();
   std::vector<std::unique_ptr<Command>> vec;
   vec.emplace_back(std::move(m_ptr));
   vec.emplace_back(std::move(parallel).Unwrap());
@@ -179,11 +204,13 @@ class FinallyCommand : public WrapperCommand {
 }  // namespace
 
 CommandPtr CommandPtr::FinallyDo(std::function<void(bool)> end) && {
+  AssertValid();
   m_ptr = std::make_unique<FinallyCommand>(std::move(m_ptr), std::move(end));
   return std::move(*this);
 }
 
 CommandPtr CommandPtr::HandleInterrupt(std::function<void(void)> handler) && {
+  AssertValid();
   return std::move(*this).FinallyDo(
       [handler = std::move(handler)](bool interrupted) {
         if (interrupted) {
@@ -193,29 +220,39 @@ CommandPtr CommandPtr::HandleInterrupt(std::function<void(void)> handler) && {
 }
 
 Command* CommandPtr::get() const {
+  AssertValid();
   return m_ptr.get();
 }
 
 std::unique_ptr<Command> CommandPtr::Unwrap() && {
+  AssertValid();
   return std::move(m_ptr);
 }
 
 void CommandPtr::Schedule() const {
+  AssertValid();
   CommandScheduler::GetInstance().Schedule(*this);
 }
 
 void CommandPtr::Cancel() const {
+  AssertValid();
   CommandScheduler::GetInstance().Cancel(*this);
 }
 
 bool CommandPtr::IsScheduled() const {
+  AssertValid();
   return CommandScheduler::GetInstance().IsScheduled(*this);
 }
 
 bool CommandPtr::HasRequirement(Subsystem* requirement) const {
+  AssertValid();
   return m_ptr->HasRequirement(requirement);
 }
 
+CommandPtr::operator bool() const {
+  return m_ptr.operator bool();
+}
+
 std::vector<std::unique_ptr<Command>> CommandPtr::UnwrapVector(
     std::vector<CommandPtr>&& vec) {
   std::vector<std::unique_ptr<Command>> ptrs;

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

@@ -14,7 +14,6 @@
 #include "frc2/command/PrintCommand.h"
 #include "frc2/command/ProxyScheduleCommand.h"
 #include "frc2/command/RunCommand.h"
-#include "frc2/command/SelectCommand.h"
 #include "frc2/command/SequentialCommandGroup.h"
 #include "frc2/command/WaitCommand.h"
 #include "frc2/command/WaitUntilCommand.h"
@@ -100,14 +99,6 @@ CommandPtr cmd::Either(CommandPtr&& onTrue, CommandPtr&& onFalse,
       .ToPtr();
 }
 
-template <typename Key>
-CommandPtr cmd::Select(std::function<Key()> selector,
-                       std::vector<std::pair<Key, CommandPtr>> commands) {
-  return SelectCommand(std::move(selector),
-                       CommandPtr::UnwrapVector(std::move(commands)))
-      .ToPtr();
-}
-
 CommandPtr cmd::Sequence(std::vector<CommandPtr>&& commands) {
   return SequentialCommandGroup(CommandPtr::UnwrapVector(std::move(commands)))
       .ToPtr();

wpilibNewCommands/src/main/native/include/frc2/command/CommandPtr.h

@@ -259,6 +259,11 @@ class CommandPtr final {
    */
   bool HasRequirement(Subsystem* requirement) const;
 
+  /**
+   * Check if this CommandPtr object is valid and wasn't moved-from.
+   */
+  explicit operator bool() const;
+
   /**
    * Convert a vector of CommandPtr objects to their underlying unique_ptrs.
    */
@@ -267,6 +272,7 @@ class CommandPtr final {
 
  private:
   std::unique_ptr<Command> m_ptr;
+  void AssertValid() const;
 };
 
 }  // namespace frc2

wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h

@@ -182,10 +182,6 @@ class CommandScheduler final : public nt::NTSendable,
       throw FRC_MakeError(frc::err::CommandIllegalUse,
                           "Default commands must require their subsystem!");
     }
-    if (defaultCommand.IsFinished()) {
-      throw FRC_MakeError(frc::err::CommandIllegalUse,
-                          "Default commands should not end!");
-    }
     SetDefaultCommandImpl(subsystem,
                           std::make_unique<std::remove_reference_t<T>>(
                               std::forward<T>(defaultCommand)));

wpilibNewCommands/src/main/native/include/frc2/command/Commands.h

@@ -12,6 +12,7 @@
 #include <vector>
 
 #include "frc2/command/CommandPtr.h"
+#include "frc2/command/SelectCommand.h"
 
 namespace frc2 {
 class Subsystem;
@@ -158,7 +159,11 @@ namespace cmd {
 template <typename Key>
 [[nodiscard]] CommandPtr Select(
     std::function<Key()> selector,
-    std::vector<std::pair<Key, CommandPtr>> commands);
+    std::vector<std::pair<Key, CommandPtr>> commands) {
+  return SelectCommand(std::move(selector),
+                       CommandPtr::UnwrapVector(std::move(commands)))
+      .ToPtr();
+}
 
 // Command Groups
 

wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h

@@ -22,46 +22,48 @@
 namespace frc2 {
 class Command;
 /**
- * This class is a command-based wrapper around {@link frc::BooleanEvent},
- * providing an easy way to link commands to inputs.
+ * This class provides an easy way to link commands to conditions.
  *
- * This class is provided by the NewCommands VendorDep
+ * <p>It is very easy to link a button to a command. For instance, you could
+ * link the trigger button of a joystick to a "score" command.
  *
- * @see Button
+ * <p>Triggers can easily be composed for advanced functionality using the
+ * {@link #operator!}, {@link #operator||}, {@link #operator&&} operators.
+ *
+ * <p>This class is provided by the NewCommands VendorDep
  */
 class Trigger {
  public:
   /**
-   * Creates a new trigger with the given condition determining whether it is
-   * active.
+   * Creates a new trigger based on the given condition.
    *
    * <p>Polled by the default scheduler button loop.
    *
-   * @param isActive returns whether or not the trigger should be active
+   * @param condition the condition represented by this trigger
    */
-  explicit Trigger(std::function<bool()> isActive)
+  explicit Trigger(std::function<bool()> condition)
       : m_event{CommandScheduler::GetInstance().GetDefaultButtonLoop(),
-                std::move(isActive)} {}
+                std::move(condition)} {}
 
   /**
-   * Create a new trigger that is active when the given condition is true.
+   * Creates a new trigger based on the given condition.
    *
    * @param loop The loop instance that polls this trigger.
-   * @param isActive Whether the trigger is active.
+   * @param condition the condition represented by this trigger
    */
-  Trigger(frc::EventLoop* loop, std::function<bool()> isActive)
-      : m_event{loop, std::move(isActive)} {}
+  Trigger(frc::EventLoop* loop, std::function<bool()> condition)
+      : m_event{loop, std::move(condition)} {}
 
   /**
-   * Create a new trigger that is never active (default constructor) - activity
-   *  can be further determined by subclass code.
+   * Create a new trigger that is always `false`.
    */
   Trigger() : Trigger([] { return false; }) {}
 
   Trigger(const Trigger& other);
 
   /**
-   * Starts the given command whenever the signal rises from `false` to `true`.
+   * Starts the given command whenever the condition changes from `false` to
+   * `true`.
    *
    * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
    * lifespan of the command.
@@ -73,8 +75,8 @@ class Trigger {
   Trigger OnTrue(Command* command);
 
   /**
-   * Starts the given command whenever the signal rises from `false` to `true`.
-   * Moves command ownership to the button scheduler.
+   * Starts the given command whenever the condition changes from `false` to
+   * `true`. Moves command ownership to the button scheduler.
    *
    * @param command The command to bind.
    * @return The trigger, for chained calls.
@@ -82,7 +84,8 @@ class Trigger {
   Trigger OnTrue(CommandPtr&& command);
 
   /**
-   * Starts the given command whenever the signal falls from `true` to `false`.
+   * Starts the given command whenever the condition changes from `true` to
+   * `false`.
    *
    * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
    * lifespan of the command.
@@ -94,7 +97,8 @@ class Trigger {
   Trigger OnFalse(Command* command);
 
   /**
-   * Starts the given command whenever the signal falls from `true` to `false`.
+   * Starts the given command whenever the condition changes from `true` to
+   * `false`.
    *
    * @param command The command to bind.
    * @return The trigger, for chained calls.
@@ -102,10 +106,11 @@ class Trigger {
   Trigger OnFalse(CommandPtr&& command);
 
   /**
-   * Starts the given command when the signal rises to `true` and cancels it
-   * when the signal falls to `false`.
+   * Starts the given command when the condition changes to `true` and cancels
+   * it when the condition changes to `false`.
    *
-   * <p>Doesn't re-start the command in-between.
+   * <p>Doesn't re-start the command if it ends while the condition is still
+   * `true`. If the command should restart, see RepeatCommand.
    *
    * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
    * lifespan of the command.
@@ -116,9 +121,12 @@ class Trigger {
   Trigger WhileTrue(Command* command);
 
   /**
-   * Starts the given command when the signal rises to `true` and cancels it
-   * when the signal falls to `false`. Moves command ownership to the button
-   * scheduler.
+   * Starts the given command when the condition changes to `true` and cancels
+   * it when the condition changes to `false`. Moves command ownership to the
+   * button scheduler.
+   *
+   * <p>Doesn't re-start the command if it ends while the condition is still
+   * `true`. If the command should restart, see RepeatCommand.
    *
    * @param command The command to bind.
    * @return The trigger, for chained calls.
@@ -126,10 +134,11 @@ class Trigger {
   Trigger WhileTrue(CommandPtr&& command);
 
   /**
-   * Starts the given command when the signal falls to `false` and cancels
-   * it when the signal rises.
+   * Starts the given command when the condition changes to `false` and cancels
+   * it when the condition changes to `true`.
    *
-   * <p>Doesn't re-start the command in-between.
+   * <p>Doesn't re-start the command if it ends while the condition is still
+   * `true`. If the command should restart, see RepeatCommand.
    *
    * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
    * lifespan of the command.
@@ -140,9 +149,12 @@ class Trigger {
   Trigger WhileFalse(Command* command);
 
   /**
-   * Starts the given command when the signal falls to `false` and cancels
-   * it when the signal rises. Moves command ownership to the button
-   * scheduler.
+   * Starts the given command when the condition changes to `false` and cancels
+   * it when the condition changes to `true`. Moves command ownership to the
+   * button scheduler.
+   *
+   * <p>Doesn't re-start the command if it ends while the condition is still
+   * `false`. If the command should restart, see RepeatCommand.
    *
    * @param command The command to bind.
    * @return The trigger, for chained calls.
@@ -150,8 +162,7 @@ class Trigger {
   Trigger WhileFalse(CommandPtr&& command);
 
   /**
-   * Toggles a command when the signal rises from `false` to the high
-   * state.
+   * Toggles a command when the condition changes from `false` to `true`.
    *
    * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
    * lifespan of the command.
@@ -162,8 +173,7 @@ class Trigger {
   Trigger ToggleOnTrue(Command* command);
 
   /**
-   * Toggles a command when the signal rises from `false` to the high
-   * state.
+   * Toggles a command when the condition changes from `false` to `true`.
    *
    * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
    * lifespan of the command.
@@ -174,7 +184,7 @@ class Trigger {
   Trigger ToggleOnTrue(CommandPtr&& command);
 
   /**
-   * Toggles a command when the signal falls from `true` to the low
+   * Toggles a command when the condition changes from `true` to the low
    * state.
    *
    * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
@@ -186,7 +196,7 @@ class Trigger {
   Trigger ToggleOnFalse(Command* command);
 
   /**
-   * Toggles a command when the signal falls from `true` to the low
+   * Toggles a command when the condition changes from `true` to the low
    * state.
    *
    * <p>Takes a raw pointer, and so is non-owning; users are responsible for the
@@ -487,6 +497,15 @@ class Trigger {
     return m_event.operator&&(rhs).CastTo<Trigger>();
   }
 
+  /**
+   * Composes two triggers with logical AND.
+   *
+   * @return A trigger which is active when both component triggers are active.
+   */
+  Trigger operator&&(Trigger& rhs) {
+    return (m_event && rhs.m_event).CastTo<Trigger>();
+  }
+
   /**
    * Composes two triggers with logical OR.
    *
@@ -496,6 +515,15 @@ class Trigger {
     return m_event.operator||(rhs).CastTo<Trigger>();
   }
 
+  /**
+   * Composes two triggers with logical OR.
+   *
+   * @return A trigger which is active when either component trigger is active.
+   */
+  Trigger operator||(Trigger& rhs) {
+    return (m_event || rhs.m_event).CastTo<Trigger>();
+  }
+
   /**
    * Composes a trigger with logical NOT.
    *

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java

@@ -64,12 +64,10 @@ class CommandRequirementsTest extends CommandTestBase {
       Subsystem system = new SubsystemBase() {};
 
       Command missingRequirement = new WaitUntilCommand(() -> false);
-      Command ends = new InstantCommand(() -> {}, system);
 
       assertThrows(
           IllegalArgumentException.class,
           () -> scheduler.setDefaultCommand(system, missingRequirement));
-      assertThrows(IllegalArgumentException.class, () -> scheduler.setDefaultCommand(system, ends));
     }
   }
 }

wpilibNewCommands/src/test/native/cpp/frc2/command/CommandPtrTest.cpp

@@ -0,0 +1,35 @@
+// 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/Errors.h>
+
+#include "CommandTestBase.h"
+#include "frc2/command/CommandPtr.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/Commands.h"
+
+using namespace frc2;
+class CommandPtrTest : public CommandTestBase {};
+
+TEST_F(CommandPtrTest, MovedFrom) {
+  CommandScheduler scheduler = GetScheduler();
+
+  int counter = 0;
+
+  CommandPtr movedFrom = cmd::Run([&counter] { counter++; });
+  CommandPtr movedTo = std::move(movedFrom);
+
+  EXPECT_NO_FATAL_FAILURE(scheduler.Schedule(movedTo));
+  EXPECT_NO_FATAL_FAILURE(scheduler.Run());
+
+  EXPECT_EQ(1, counter);
+  EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(movedTo));
+
+  EXPECT_THROW(scheduler.Schedule(movedFrom), frc::RuntimeError);
+  EXPECT_THROW(movedFrom.IsScheduled(), frc::RuntimeError);
+  EXPECT_THROW(static_cast<void>(std::move(movedFrom).Repeatedly()),
+               frc::RuntimeError);
+
+  EXPECT_EQ(1, counter);
+}

wpilibc/build.gradle

@@ -85,8 +85,7 @@ model {
                 cpp {
                     source {
                         srcDirs = [
-                            'src/main/native/cpp',
-                            "$buildDir/generated/cpp"
+                            'src/main/native/cpp'
                         ]
                         include '**/*.cpp'
                     }
@@ -100,6 +99,22 @@ model {
                     it.buildable = false
                     return
                 }
+
+                it.sources {
+                    versionSources(CppSourceSet) {
+                        source {
+                            srcDirs = [
+                                "${rootDir}/shared/singlelib",
+                                "$buildDir/generated/cpp"
+                            ]
+                            include '**/*.cpp'
+                        }
+                        exportedHeaders {
+                            srcDirs 'src/main/native/include'
+                        }
+                    }
+                }
+
                 cppCompiler.define 'DYNAMIC_CAMERA_SERVER'
                 project(':ntcore').addNtcoreDependency(it, 'shared')
                 project(':hal').addHalDependency(it, 'shared')

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

@@ -26,7 +26,8 @@ void SendableBuilderImpl::PropertyImpl<Topic>::Update(bool controllable,
                                                       int64_t time) {
   if (controllable && sub && updateLocal) {
     updateLocal(sub);
-  } else if (pub && updateNetwork) {
+  }
+  if (pub && updateNetwork) {
     updateNetwork(pub, time);
   }
 }

wpilibc/src/main/native/include/frc/event/EventLoop.h

@@ -16,6 +16,9 @@ class EventLoop {
  public:
   EventLoop();
 
+  EventLoop(const EventLoop&) = delete;
+  EventLoop& operator=(const EventLoop&) = delete;
+
   /**
    * Bind a new action to run whenever the condition is true.
    *

wpilibc/src/main/native/include/frc/interfaces/Gyro.h

@@ -66,10 +66,6 @@ class Gyro {
   /**
    * Return the heading of the robot as a Rotation2d.
    *
-   * The angle is continuous, that is it will continue from 360 to 361 degrees.
-   * This allows algorithms that wouldn't want to see a discontinuity in the
-   * gyro output as it sweeps past from 360 to 0 on the second time around.
-   *
    * The angle is expected to increase as the gyro turns counterclockwise when
    * looked at from the top. It needs to follow the NWU axis convention.
    *

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

@@ -37,8 +37,7 @@ class ShuffleboardComponent : public ShuffleboardComponentBase {
    * @param properties the properties for this component
    * @return this component
    */
-  Derived& WithProperties(
-      const wpi::StringMap<std::shared_ptr<nt::Value>>& properties);
+  Derived& WithProperties(const wpi::StringMap<nt::Value>& properties);
 
   /**
    * Sets the position of this component in the tab. This has no effect if this

wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponent.inc

@@ -21,7 +21,7 @@ ShuffleboardComponent<Derived>::ShuffleboardComponent(
 
 template <typename Derived>
 Derived& ShuffleboardComponent<Derived>::WithProperties(
-    const wpi::StringMap<std::shared_ptr<nt::Value>>& properties) {
+    const wpi::StringMap<nt::Value>& properties) {
   m_properties = properties;
   m_metadataDirty = true;
   return *static_cast<Derived*>(this);

wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h

@@ -75,7 +75,9 @@ class Drivetrain {
   frc::AnalogGyro m_gyro{0};
 
   frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
-  frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
+  frc::DifferentialDriveOdometry m_odometry{
+      m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
+      units::meter_t{m_rightEncoder.GetDistance()}};
 
   // Gains are for example purposes only - must be determined for your own
   // robot!

wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h

@@ -79,7 +79,9 @@ class Drivetrain {
   // Gains are for example purposes only - must be determined for your own
   // robot!
   frc::DifferentialDrivePoseEstimator m_poseEstimator{
-      frc::Rotation2d{},
+      m_gyro.GetRotation2d(),
+      units::meter_t{m_leftEncoder.GetDistance()},
+      units::meter_t{m_rightEncoder.GetDistance()},
       frc::Pose2d{},
       {0.01, 0.01, 0.01, 0.01, 0.01},
       {0.1, 0.1, 0.1},

wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp

@@ -126,6 +126,6 @@ frc::Pose2d DriveSubsystem::GetPose() {
 }
 
 void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
-  m_odometry.ResetPosition(pose, m_gyro.GetRotation2d(),
-                           getCurrentWheelDistances());
+  m_odometry.ResetPosition(m_gyro.GetRotation2d(), getCurrentWheelDistances(),
+                           pose);
 }

wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h

@@ -77,9 +77,9 @@ class Drivetrain {
   // Gains are for example purposes only - must be determined for your own
   // robot!
   frc::MecanumDrivePoseEstimator m_poseEstimator{
-      0_deg,
-      frc::Pose2d{},
+      m_gyro.GetRotation2d(),
       GetCurrentDistances(),
+      frc::Pose2d{},
       m_kinematics,
       {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1},
       {0.05, 0.05, 0.05, 0.05, 0.05},

wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp

@@ -16,7 +16,7 @@ DriveSubsystem::DriveSubsystem()
       m_right2{kRightMotor2Port},
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
-      m_odometry{m_gyro.GetRotation2d()} {
+      m_odometry{m_gyro.GetRotation2d(), units::meter_t{0}, units::meter_t{0}} {
   // We need to invert one side of the drivetrain so that positive voltages
   // result in both sides moving forward. Depending on how your robot's
   // gearbox is constructed, you might have to invert the left side instead.
@@ -86,5 +86,7 @@ frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
 
 void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
   ResetEncoders();
-  m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
+  m_odometry.ResetPosition(m_gyro.GetRotation2d(),
+                           units::meter_t{m_leftEncoder.GetDistance()},
+                           units::meter_t{m_rightEncoder.GetDistance()}, pose);
 }

wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp

@@ -28,7 +28,9 @@ void Drivetrain::UpdateOdometry() {
 }
 
 void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
-  m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
+  m_odometry.ResetPosition(m_gyro.GetRotation2d(),
+                           units::meter_t{m_leftEncoder.GetDistance()},
+                           units::meter_t{m_rightEncoder.GetDistance()}, pose);
 }
 
 frc::Pose2d Drivetrain::GetPose() const {

wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h

@@ -76,7 +76,9 @@ class Drivetrain {
   frc::AnalogGyro m_gyro{0};
 
   frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
-  frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
+  frc::DifferentialDriveOdometry m_odometry{
+      m_gyro.GetRotation2d(), units::meter_t{m_leftEncoder.GetDistance()},
+      units::meter_t{m_rightEncoder.GetDistance()}};
 
   // Gains are for example purposes only - must be determined for your own
   // robot!

wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp

@@ -33,7 +33,9 @@ void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
   m_leftEncoder.Reset();
   m_rightEncoder.Reset();
   m_drivetrainSimulator.SetPose(pose);
-  m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
+  m_odometry.ResetPosition(m_gyro.GetRotation2d(),
+                           units::meter_t{m_leftEncoder.GetDistance()},
+                           units::meter_t{m_rightEncoder.GetDistance()}, pose);
 }
 
 void Drivetrain::SimulationPeriodic() {