[commands] Disable regularly failing unit test (#4824)

Using an atomic here means we are never going against a lock that is touchable from user code. That should make reading the DS data from the DS callback even safer.

.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

@@ -4,9 +4,9 @@ on:
     types: [ created ]
 
 jobs:
-  wpiformat:
-    if: github.event.issue.pull_request && startsWith(github.event.comment.body, '/wpiformat')
-    runs-on: ubuntu-latest
+  format:
+    if: github.event.issue.pull_request && startsWith(github.event.comment.body, '/format')
+    runs-on: ubuntu-22.04
     steps:
       - name: React Rocket
         uses: actions/github-script@v4
@@ -37,21 +37,28 @@ 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
           git config user.name "github-actions[bot]"
           git config user.email "41898282+github-actions[bot]@users.noreply.github.com"
           # Commit
-          git commit -am "wpiformat"
+          git commit -am "Formatting fixes"
           git push

.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

@@ -1,19 +0,0 @@
-name: Gazebo
-
-on: [pull_request, push]
-
-concurrency:
-  group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
-  cancel-in-progress: true
-
-jobs:
-  build:
-    name: "Build"
-    runs-on: ubuntu-latest
-    container: wpilib/gazebo-ubuntu:22.04
-    steps:
-      - uses: actions/checkout@v3
-        with:
-          fetch-depth: 0
-      - name: Build with Gradle
-        run: ./gradlew simulation:frc_gazebo_plugins:build simulation:halsim_gazebo:build -PbuildServer -PforceGazebo

.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
@@ -83,7 +82,7 @@ jobs:
       - uses: actions/setup-java@v3
         with:
           distribution: 'zulu'
-          java-version: 11
+          java-version: 17
           architecture: ${{ matrix.architecture }}
       - name: Import Developer ID Certificate
         uses: wpilibsuite/import-signing-certificate@v1
@@ -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

.gitignore

@@ -5,6 +5,10 @@ doxygen.log
 build*/
 !buildSrc/
 
+simgui-ds.json
+simgui-window.json
+simgui.json
+
 # Created by the jenkins test script
 test-reports
 

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/)
+[![Gradle](https://github.com/wpilibsuite/allwpilib/actions/workflows/gradle.yml/badge.svg?branch=main)](https://github.com/wpilibsuite/allwpilib/actions/workflows/gradle.yml)
+[![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.
 
@@ -15,7 +15,6 @@ Welcome to the WPILib project. This repository contains the HAL, WPILibJ, and WP
     - [Faster builds](#faster-builds)
     - [Using Development Builds](#using-development-builds)
     - [Custom toolchain location](#custom-toolchain-location)
-    - [Gazebo simulation](#gazebo-simulation)
     - [Formatting/Linting](#formattinglinting)
     - [CMake](#cmake)
   - [Publishing](#publishing)
@@ -33,7 +32,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 +103,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
 
@@ -114,33 +113,14 @@ If you have installed the FRC Toolchain to a directory other than the default, o
 ./gradlew build -PtoolChainPath=some/path/to/frc/toolchain/bin
 ```
 
-### Gazebo simulation
-
-If you also want to force building Gazebo simulation support, add -PforceGazebo. This requires gazebo_transport. We have tested on 14.04 and 15.05, but any correct install of Gazebo should work, even on Windows if you build Gazebo from source. Correct means CMake needs to be able to find gazebo-config.cmake. See [The Gazebo website](https://gazebosim.org/) for installation instructions.
-
-```bash
-./gradlew build -PforceGazebo
-```
-
-If you prefer to use CMake directly, the you can still do so.
-The common CMake tasks are wpilibcSim, frc_gazebo_plugins, and gz_msgs
-
-```bash
-mkdir build #run this in the root of allwpilib
-cd build
-cmake ..
-make
-```
-
-
 ### Formatting/linting
 
+Once a PR has been submitted, formatting can be run in CI by commenting `/format` on the PR. A new commit will be pushed with the formatting changes.
+
 #### wpiformat
 
 wpiformat can be executed anywhere in the repository via `py -3 -m wpiformat -clang 14` on Windows or `python3 -m wpiformat -clang 14` on other platforms.
 
-Once a PR has been submitted, formatting can be run in CI by commenting `/wpiformat` on the PR. A new commit will be pushed with the formatting changes.
-
 #### Java Code Quality Tools
 
 The Java code quality tools Checkstyle, PMD, and Spotless can be run via `./gradlew javaFormat`. SpotBugs can be run via the `spotbugsMain`, `spotbugsTest`, and `spotbugsDev` tasks. These tools will all be run automatically by the `build` task. To disable this behavior, pass the `-PskipJavaFormat` flag.
@@ -164,9 +144,7 @@ The maven artifacts are described in [MavenArtifacts.md](MavenArtifacts.md)
 
 ## Structure and Organization
 
-The main WPILib code you're probably looking for is in WPILibJ and WPILibC. Those directories are split into shared, sim, and athena. Athena contains the WPILib code meant to run on your roboRIO. Sim is WPILib code meant to run on your computer with Gazebo, and shared is code shared between the two. Shared code must be platform-independent, since it will be compiled with both the ARM cross-compiler and whatever desktop compiler you are using (g++, msvc, etc...).
-
-The Simulation directory contains extra simulation tools and libraries, such as gz_msgs and JavaGazebo. See sub-directories for more information.
+The main WPILib code you're probably looking for is in WPILibJ and WPILibC. Those directories are split into shared, sim, and athena. Athena contains the WPILib code meant to run on your roboRIO. Sim is WPILib code meant to run on your computer, and shared is code shared between the two. Shared code must be platform-independent, since it will be compiled with both the ARM cross-compiler and whatever desktop compiler you are using (g++, msvc, etc...).
 
 The integration test directories for C++ and Java contain test code that runs on our test-system. When you submit code for review, it is tested by those programs. If you add new functionality you should make sure to write tests for it so we don't break it in the future.
 

apriltag/CMakeLists.txt

@@ -0,0 +1,118 @@
+project(apriltag)
+
+include(CompileWarnings)
+include(GenResources)
+include(FetchContent)
+
+FetchContent_Declare(
+    apriltaglib
+    GIT_REPOSITORY  https://github.com/wpilibsuite/apriltag.git
+    GIT_TAG         ad31e33d20f9782b7239cb15cde57c56c91383ad
+)
+
+# Don't use apriltag's CMakeLists.txt due to conflicting naming and JNI
+FetchContent_GetProperties(apriltaglib)
+if(NOT apriltaglib_POPULATED)
+    FetchContent_Populate(apriltaglib)
+endif()
+
+aux_source_directory(${apriltaglib_SOURCE_DIR}/common APRILTAGLIB_COMMON_SRC)
+file(GLOB TAG_FILES ${apriltaglib_SOURCE_DIR}/tag*.c)
+set(APRILTAGLIB_SRCS ${apriltaglib_SOURCE_DIR}/apriltag.c ${apriltaglib_SOURCE_DIR}/apriltag_pose.c ${apriltaglib_SOURCE_DIR}/apriltag_quad_thresh.c)
+
+file(GLOB apriltag_jni_src src/main/native/cpp/jni/AprilTagJNI.cpp)
+
+if (WITH_JAVA)
+  find_package(Java REQUIRED)
+  find_package(JNI REQUIRED)
+  include(UseJava)
+  set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
+
+  set(CMAKE_JNI_TARGET true)
+
+  file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
+  file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
+  find_file(OPENCV_JAR_FILE
+    NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar
+    PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin ${OpenCV_INSTALL_PATH}/share/java
+    NO_DEFAULT_PATH)
+
+  set(CMAKE_JAVA_INCLUDE_PATH apriltag.jar ${EJML_JARS} ${JACKSON_JARS})
+
+  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+  file(GLOB_RECURSE JAVA_RESOURCES src/main/native/resources/*.json)
+  add_jar(apriltag_jar
+    SOURCES ${JAVA_SOURCES}
+    RESOURCES NAMESPACE "edu/wpi/first/apriltag" ${JAVA_RESOURCES}
+    INCLUDE_JARS wpimath_jar ${EJML_JARS} wpiutil_jar ${OPENCV_JAR_FILE}
+    OUTPUT_NAME apriltag
+    GENERATE_NATIVE_HEADERS apriltag_jni_headers)
+
+  get_property(APRILTAG_JAR_FILE TARGET apriltag_jar PROPERTY JAR_FILE)
+  install(FILES ${APRILTAG_JAR_FILE} DESTINATION "${java_lib_dest}")
+
+  set_property(TARGET apriltag_jar PROPERTY FOLDER "java")
+
+  add_library(apriltagjni ${apriltag_jni_src})
+  wpilib_target_warnings(apriltagjni)
+  target_link_libraries(apriltagjni PUBLIC apriltag)
+
+  set_property(TARGET apriltagjni PROPERTY FOLDER "libraries")
+
+  target_link_libraries(apriltagjni PRIVATE apriltag_jni_headers)
+  add_dependencies(apriltagjni apriltag_jar)
+
+  if (MSVC)
+    install(TARGETS apriltagjni RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
+  endif()
+
+  install(TARGETS apriltagjni EXPORT apriltagjni DESTINATION "${main_lib_dest}")
+
+endif()
+
+GENERATE_RESOURCES(src/main/native/resources/edu/wpi/first/apriltag generated/main/cpp APRILTAG frc apriltag_resources_src)
+
+file(GLOB apriltag_native_src src/main/native/cpp/*.cpp)
+
+add_library(apriltag ${apriltag_native_src} ${apriltag_resources_src} ${APRILTAGLIB_SRCS} ${APRILTAGLIB_COMMON_SRC} ${TAG_FILES})
+set_target_properties(apriltag PROPERTIES DEBUG_POSTFIX "d")
+
+set_property(TARGET apriltag PROPERTY FOLDER "libraries")
+target_compile_features(apriltag PUBLIC cxx_std_20)
+wpilib_target_warnings(apriltag)
+# disable warnings that apriltaglib can't handle
+if (MSVC)
+  target_compile_options(apriltag PRIVATE /wd4018)
+else()
+  target_compile_options(apriltag PRIVATE -Wno-sign-compare -Wno-gnu-zero-variadic-macro-arguments)
+endif()
+
+target_link_libraries(apriltag wpimath)
+
+target_include_directories(apriltag PUBLIC
+  $<BUILD_INTERFACE:${apriltaglib_SOURCE_DIR}>
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+  $<INSTALL_INTERFACE:${include_dest}/apriltag>)
+
+install(TARGETS apriltag EXPORT apriltag DESTINATION "${main_lib_dest}")
+install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/apriltag")
+
+if (WITH_JAVA AND MSVC)
+    install(TARGETS apriltag RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
+endif()
+
+if (WITH_FLAT_INSTALL)
+    set (apriltag_config_dir ${wpilib_dest})
+else()
+    set (apriltag_config_dir share/apriltag)
+endif()
+
+configure_file(apriltag-config.cmake.in ${WPILIB_BINARY_DIR}/apriltag-config.cmake )
+install(FILES ${WPILIB_BINARY_DIR}/apriltag-config.cmake DESTINATION ${apriltag_config_dir})
+install(EXPORT apriltag DESTINATION ${apriltag_config_dir})
+
+if (WITH_TESTS)
+    wpilib_add_test(apriltag src/test/native/cpp)
+    target_include_directories(apriltag_test PRIVATE src/test/native/include)
+    target_link_libraries(apriltag_test apriltag gmock_main)
+endif()

apriltag/apriltag-config.cmake.in

@@ -0,0 +1,7 @@
+include(CMakeFindDependencyMacro)
+@FILENAME_DEP_REPLACE@
+@WPIMATH_DEP_REPLACE@
+@WPIUTIL_DEP_REPLACE@
+
+@FILENAME_DEP_REPLACE@
+include(${SELF_DIR}/apriltag.cmake)

apriltag/build.gradle

@@ -0,0 +1,84 @@
+apply from: "${rootDir}/shared/resources.gradle"
+
+ext {
+    nativeName = 'apriltag'
+    devMain = 'edu.wpi.first.apriltag.DevMain'
+    useJava = true
+
+    def generateTask = createGenerateResourcesTask('main', 'APRILTAG', 'frc', project)
+
+    tasks.withType(CppCompile) {
+        dependsOn generateTask
+    }
+    splitSetup = {
+        it.sources {
+            resourcesCpp(CppSourceSet) {
+                source {
+                    srcDirs "$buildDir/generated/main/cpp", "$rootDir/shared/singlelib"
+                    include '*.cpp'
+                }
+            }
+        }
+    }
+}
+
+evaluationDependsOn(':wpimath')
+
+apply from: "${rootDir}/shared/jni/setupBuild.gradle"
+apply from: "${rootDir}/shared/apriltaglib.gradle"
+apply from: "${rootDir}/shared/opencv.gradle"
+
+dependencies {
+    implementation project(':wpimath')
+    devImplementation 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'
+
+            if (it.component.name == "${nativeName}JNI") {
+                lib project: ':wpimath', library: 'wpimath', linkage: 'static'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+            } else {
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            }
+
+            nativeUtils.useRequiredLibrary(it, 'apriltaglib')
+        }
+    }
+    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,18 @@
+// 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 edu.wpi.first.apriltag.jni.AprilTagJNI;
+
+public final class DevMain {
+  /** Main entry point. */
+  public static void main(String[] args) {
+    System.out.println("Hello World!");
+    var detector = AprilTagJNI.aprilTagCreate("tag16h5", 2.0, 0.0, 1, false, false);
+    AprilTagJNI.aprilTagDestroy(detector);
+  }
+
+  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,28 @@ 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.
+ * <p>Pose3ds in the JSON are measured using the normal FRC coordinate system, NWU with the origin
+ * at the bottom-right corner of the blue alliance wall. {@link #setOrigin(OriginPosition)} can be
+ * used to change the poses returned from {@link AprilTagFieldLayout#getTagPose(int)} to be from the
+ * perspective of a specific alliance.
  */
 @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 +72,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 +93,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 +158,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 +185,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/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java

@@ -0,0 +1,89 @@
+// 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.jni;
+
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.IOException;
+import java.util.concurrent.atomic.AtomicBoolean;
+import org.opencv.core.Mat;
+
+public class AprilTagJNI {
+  static boolean libraryLoaded = false;
+
+  static RuntimeLoader<AprilTagJNI> loader = null;
+
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+  }
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        loader =
+            new RuntimeLoader<>(
+                "apriltagjni", RuntimeLoader.getDefaultExtractionRoot(), AprilTagJNI.class);
+        loader.loadLibrary();
+      } catch (IOException ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  // Returns a pointer to a apriltag_detector_t
+  public static native long aprilTagCreate(
+      String fam, double decimate, double blur, int threads, boolean debug, boolean refine_edges);
+
+  // Destroy and free a previously created detector.
+  public static native void aprilTagDestroy(long detector);
+
+  private static native Object[] aprilTagDetectInternal(
+      long detector,
+      long imgAddr,
+      int rows,
+      int cols,
+      boolean doPoseEstimation,
+      double tagWidth,
+      double fx,
+      double fy,
+      double cx,
+      double cy,
+      int nIters);
+
+  // Detect targets given a GRAY frame. Returns a pointer toa zarray
+  public static DetectionResult[] aprilTagDetect(
+      long detector,
+      Mat img,
+      boolean doPoseEstimation,
+      double tagWidth,
+      double fx,
+      double fy,
+      double cx,
+      double cy,
+      int nIters) {
+    return (DetectionResult[])
+        aprilTagDetectInternal(
+            detector,
+            img.dataAddr(),
+            img.rows(),
+            img.cols(),
+            doPoseEstimation,
+            tagWidth,
+            fx,
+            fy,
+            cx,
+            cy,
+            nIters);
+  }
+}

apriltag/src/main/java/edu/wpi/first/apriltag/jni/DetectionResult.java

@@ -0,0 +1,226 @@
+// 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.jni;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Arrays;
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
+import org.ejml.simple.SimpleMatrix;
+
+public class DetectionResult {
+  public int getId() {
+    return m_id;
+  }
+
+  public int getHamming() {
+    return m_hamming;
+  }
+
+  public float getDecisionMargin() {
+    return m_decisionMargin;
+  }
+
+  public void setDecisionMargin(float decisionMargin) {
+    this.m_decisionMargin = decisionMargin;
+  }
+
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
+  public double[] getHomography() {
+    return m_homography;
+  }
+
+  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
+  public void setHomography(double[] homography) {
+    this.m_homography = homography;
+  }
+
+  public double getCenterX() {
+    return m_centerX;
+  }
+
+  public void setCenterX(double centerX) {
+    this.m_centerX = centerX;
+  }
+
+  public double getCenterY() {
+    return m_centerY;
+  }
+
+  public void setCenterY(double centerY) {
+    this.m_centerY = centerY;
+  }
+
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
+  public double[] getCorners() {
+    return m_corners;
+  }
+
+  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
+  public void setCorners(double[] corners) {
+    this.m_corners = corners;
+  }
+
+  public double getError1() {
+    return m_error1;
+  }
+
+  public double getError2() {
+    return m_error2;
+  }
+
+  public Transform3d getPoseResult1() {
+    return m_poseResult1;
+  }
+
+  public Transform3d getPoseResult2() {
+    return m_poseResult2;
+  }
+
+  private final int m_id;
+  private final int m_hamming;
+  private float m_decisionMargin;
+  private double[] m_homography;
+  private double m_centerX;
+  private double m_centerY;
+  private double[] m_corners;
+
+  private final Transform3d m_poseResult1;
+  private final double m_error1;
+  private final Transform3d m_poseResult2;
+  private final double m_error2;
+
+  /**
+   * Constructs a new detection result. Used from JNI.
+   *
+   * @param id id
+   * @param hamming hamming
+   * @param decisionMargin dm
+   * @param homography homography
+   * @param centerX centerX
+   * @param centerY centerY
+   * @param corners corners
+   * @param pose1TransArr pose1TransArr
+   * @param pose1RotArr pose1RotArr
+   * @param err1 err1
+   * @param pose2TransArr pose2TransArr
+   * @param pose2RotArr pose2RotArr
+   * @param err2 err2
+   */
+  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
+  public DetectionResult(
+      int id,
+      int hamming,
+      float decisionMargin,
+      double[] homography,
+      double centerX,
+      double centerY,
+      double[] corners,
+      double[] pose1TransArr,
+      double[] pose1RotArr,
+      double err1,
+      double[] pose2TransArr,
+      double[] pose2RotArr,
+      double err2) {
+    this.m_id = id;
+    this.m_hamming = hamming;
+    this.m_decisionMargin = decisionMargin;
+    this.m_homography = homography;
+    this.m_centerX = centerX;
+    this.m_centerY = centerY;
+    this.m_corners = corners;
+
+    this.m_error1 = err1;
+    this.m_poseResult1 =
+        new Transform3d(
+            new Translation3d(pose1TransArr[0], pose1TransArr[1], pose1TransArr[2]),
+            new Rotation3d(
+                orthogonalizeRotationMatrix(
+                    new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose1RotArr))));
+    this.m_error2 = err2;
+    this.m_poseResult2 =
+        new Transform3d(
+            new Translation3d(pose2TransArr[0], pose2TransArr[1], pose2TransArr[2]),
+            new Rotation3d(
+                orthogonalizeRotationMatrix(
+                    new MatBuilder<>(Nat.N3(), Nat.N3()).fill(pose2RotArr))));
+  }
+
+  /**
+   * Get the ratio of pose reprojection errors, called ambiguity. Numbers above 0.2 are likely to be
+   * ambiguous.
+   *
+   * @return The ratio of pose reprojection errors.
+   */
+  public double getPoseAmbiguity() {
+    var min = Math.min(m_error1, m_error2);
+    var max = Math.max(m_error1, m_error2);
+
+    if (max > 0) {
+      return min / max;
+    } else {
+      return -1;
+    }
+  }
+
+  @Override
+  public String toString() {
+    return "DetectionResult [centerX="
+        + m_centerX
+        + ", centerY="
+        + m_centerY
+        + ", corners="
+        + Arrays.toString(m_corners)
+        + ", decisionMargin="
+        + m_decisionMargin
+        + ", error1="
+        + m_error1
+        + ", error2="
+        + m_error2
+        + ", hamming="
+        + m_hamming
+        + ", homography="
+        + Arrays.toString(m_homography)
+        + ", id="
+        + m_id
+        + ", poseResult1="
+        + m_poseResult1
+        + ", poseResult2="
+        + m_poseResult2
+        + "]";
+  }
+
+  private static Matrix<N3, N3> orthogonalizeRotationMatrix(Matrix<N3, N3> input) {
+    var a = DecompositionFactory_DDRM.qr(3, 3);
+    if (!a.decompose(input.getStorage().getDDRM())) {
+      // best we can do is return the input
+      return input;
+    }
+
+    // Grab results (thanks for this _great_ api, EJML)
+    var Q = new DMatrixRMaj(3, 3);
+    var R = new DMatrixRMaj(3, 3);
+    a.getQ(Q, false);
+    a.getR(R, false);
+
+    // Fix signs in R if they're < 0 so it's close to an identity matrix
+    // (our QR decomposition implementation sometimes flips the signs of columns)
+    for (int colR = 0; colR < 3; ++colR) {
+      if (R.get(colR, colR) < 0) {
+        for (int rowQ = 0; rowQ < 3; ++rowQ) {
+          Q.set(rowQ, colR, -Q.get(rowQ, colR));
+        }
+      }
+    }
+
+    return new Matrix<>(new SimpleMatrix(Q));
+  }
+}

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

@@ -8,14 +8,6 @@
 
 using namespace frc;
 
-bool AprilTag::operator==(const AprilTag& other) const {
-  return ID == other.ID && pose == other.pose;
-}
-
-bool AprilTag::operator!=(const AprilTag& other) const {
-  return !operator==(other);
-}
-
 void frc::to_json(wpi::json& json, const AprilTag& apriltag) {
   json = wpi::json{{"ID", apriltag.ID}, {"pose", apriltag.pose}};
 }

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,36 +25,47 @@ 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>()};
+  m_fieldLength = units::meter_t{json.at("field").at("length").get<double>()};
 }
 
 AprilTagFieldLayout::AprilTagFieldLayout(std::vector<AprilTag> apriltags,
                                          units::meter_t fieldLength,
                                          units::meter_t fieldWidth)
-    : m_apriltags(std::move(apriltags)),
-      m_fieldLength(std::move(fieldLength)),
-      m_fieldWidth(std::move(fieldWidth)) {}
+    : 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) {
@@ -71,25 +81,25 @@ void AprilTagFieldLayout::Serialize(std::string_view path) {
   output.flush();
 }
 
-bool AprilTagFieldLayout::operator==(const AprilTagFieldLayout& other) const {
-  return m_apriltags == other.m_apriltags && m_mirror == other.m_mirror &&
-         m_fieldLength == other.m_fieldLength &&
-         m_fieldWidth == other.m_fieldWidth;
-}
-
-bool AprilTagFieldLayout::operator!=(const AprilTagFieldLayout& other) const {
-  return !operator==(other);
-}
-
 void frc::to_json(wpi::json& json, const AprilTagFieldLayout& layout) {
+  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/cpp/jni/AprilTagJNI.cpp

@@ -0,0 +1,320 @@
+// 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 <wpi/jni_util.h>
+
+#include "edu_wpi_first_apriltag_jni_AprilTagJNI.h"
+
+#ifdef _WIN32
+#pragma warning(push)
+#pragma warning(disable : 4200)
+#endif
+
+#if defined(__GNUC__)
+#pragma GCC diagnostic ignored "-Wpedantic"
+#endif
+#include "apriltag.h"
+#ifdef _WIN32
+#pragma warning(pop)
+#endif
+
+#include "tag36h11.h"
+#include "tag25h9.h"
+#include "tag16h5.h"
+#include "tagCircle21h7.h"
+#include "tagCircle49h12.h"
+#include "tagCustom48h12.h"
+#include "tagStandard41h12.h"
+#include "tagStandard52h13.h"
+#include "apriltag_pose.h"
+
+#include <vector>
+#include <algorithm>
+#include <cmath>
+
+using namespace wpi::java;
+
+struct DetectorState {
+  int id;
+  apriltag_detector_t* td;
+  apriltag_family_t* tf;
+  void (*tf_destroy)(apriltag_family_t*);
+};
+
+static std::vector<DetectorState> detectors;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    aprilTagCreate
+ * Signature: (Ljava/lang/String;DDIZZ)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_aprilTagCreate
+  (JNIEnv* env, jclass cls, jstring jstr, jdouble decimate, jdouble blur,
+   jint threads, jboolean debug, jboolean refine_edges)
+{
+  // Initialize tag detector with options
+  apriltag_family_t* tf = nullptr;
+  // const char *famname = fam;
+  const char* famname = env->GetStringUTFChars(jstr, nullptr);
+
+  void (*tf_destroy_func)(apriltag_family_t*);
+
+  if (!strcmp(famname, "tag36h11")) {
+    tf = tag36h11_create();
+    tf_destroy_func = tag36h11_destroy;
+  } else if (!strcmp(famname, "tag25h9")) {
+    tf = tag25h9_create();
+    tf_destroy_func = tag25h9_destroy;
+  } else if (!strcmp(famname, "tag16h5")) {
+    tf = tag16h5_create();
+    tf_destroy_func = tag16h5_destroy;
+  } else if (!strcmp(famname, "tagCircle21h7")) {
+    tf = tagCircle21h7_create();
+    tf_destroy_func = tagCircle21h7_destroy;
+  } else if (!strcmp(famname, "tagCircle49h12")) {
+    tf = tagCircle49h12_create();
+    tf_destroy_func = tagCircle49h12_destroy;
+  } else if (!strcmp(famname, "tagStandard41h12")) {
+    tf = tagStandard41h12_create();
+    tf_destroy_func = tagStandard41h12_destroy;
+  } else if (!strcmp(famname, "tagStandard52h13")) {
+    tf = tagStandard52h13_create();
+    tf_destroy_func = tagStandard52h13_destroy;
+  } else if (!strcmp(famname, "tagCustom48h12")) {
+    tf = tagCustom48h12_create();
+    tf_destroy_func = tagCustom48h12_destroy;
+  } else {
+    std::printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");
+    env->ReleaseStringUTFChars(jstr, famname);
+    return 0;
+  }
+
+  apriltag_detector_t* td = apriltag_detector_create();
+  apriltag_detector_add_family(td, tf);
+  td->quad_decimate = static_cast<float>(decimate);
+  td->quad_sigma = static_cast<float>(blur);
+  td->nthreads = threads;
+  td->debug = debug;
+  td->refine_edges = refine_edges;
+
+  env->ReleaseStringUTFChars(jstr, famname);
+
+  // std::printf("Looking for max\n");
+  auto max = std::max_element(detectors.begin(), detectors.end(),
+                              [](DetectorState& a, DetectorState& b) {
+                                return a.id < b.id;
+                              });  // detectors.size();
+  int index = 0;
+  if (max != detectors.end())
+    index = max->id + 1;
+  detectors.push_back({index, td, tf, tf_destroy_func});
+  std::printf("Created detector at idx %i\n", index);
+  return (jlong)index;
+}
+
+static JClass detectionClass;
+
+JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
+  JNIEnv* env;
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
+    return JNI_ERR;
+  }
+
+  detectionClass = JClass(env, "edu/wpi/first/apriltag/jni/DetectionResult");
+
+  if (!detectionClass) {
+    std::printf("Couldn't find class!");
+    return JNI_ERR;
+  }
+
+  return JNI_VERSION_1_6;
+}
+
+static jobject MakeJObject(JNIEnv* env, const apriltag_detection_t* detect,
+                           apriltag_pose_t& pose1, apriltag_pose_t& pose2,
+                           double error1, double error2) {
+  // Constructor signature must match Java! I = int, F = float, [D = double
+  // array
+  static jmethodID constructor =
+      env->GetMethodID(detectionClass, "<init>", "(IIF[DDD[D[D[DD[D[DD)V");
+
+  if (!constructor) {
+    return nullptr;
+  }
+
+  if (!detect) {
+    return nullptr;
+  }
+
+  // We have to copy the homography matrix and coners into jdoubles
+  jdouble h[9];  // = new jdouble[9]{};
+  for (int i = 0; i < 9; i++) {
+    h[i] = detect->H->data[i];
+  }
+
+  jdouble corners[8];  // = new jdouble[8]{};
+  for (int i = 0; i < 4; i++) {
+    corners[i * 2] = detect->p[i][0];
+    corners[i * 2 + 1] = detect->p[i][1];
+  }
+
+  jdoubleArray harr = MakeJDoubleArray(env, {h, 9});
+  jdoubleArray carr = MakeJDoubleArray(env, {corners, 8});
+
+  // The rotation of the target is encoded as a 3 by 3 rotation matrix, we'll
+  // convert to a row-major array
+  jdouble pose1RotMat[9] = {0};
+  jdouble pose2RotMat[9] = {0};
+
+  for (int i = 0; i < 9; i++) {
+    if (pose1.R) {
+      pose1RotMat[i] = pose1.R->data[i];
+    }
+    if (pose2.R) {
+      pose2RotMat[i] = pose2.R->data[i];
+    }
+  }
+
+  // And translation a 3x1 vector (todo check axis order)
+  jdouble pose1Trans[3] = {0};
+  jdouble pose2Trans[3] = {0};
+  for (int i = 0; i < 3; i++) {
+    if (pose1.t) {
+      pose1Trans[i] = pose1.t->data[i];
+    }
+    if (pose2.t) {
+      pose2Trans[i] = pose2.t->data[i];
+    }
+  }
+
+  jdoubleArray pose1rotArr = MakeJDoubleArray(env, {pose1RotMat, 9});
+  jdoubleArray pose2rotArr = MakeJDoubleArray(env, {pose2RotMat, 9});
+  jdoubleArray pose1transArr = MakeJDoubleArray(env, {pose1Trans, 3});
+  jdoubleArray pose2transArr = MakeJDoubleArray(env, {pose2Trans, 3});
+  jdouble err1 = error1;
+  jdouble err2 = error2;
+
+  // Actually call the constructor
+  auto ret = env->NewObject(
+      detectionClass, constructor, (jint)detect->id, (jint)detect->hamming,
+      (jfloat)detect->decision_margin, harr, (jdouble)detect->c[0],
+      (jdouble)detect->c[1], carr, pose1transArr, pose1rotArr, err1,
+      pose2transArr, pose2rotArr, err2);
+
+  // TODO we don't seem to need this... or at least, it doesnt leak rn
+  // env->ReleaseDoubleArrayElements(harr, h, 0);
+  // env->ReleaseDoubleArrayElements(carr, corners, 0);
+
+  return ret;
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    aprilTagDetectInternal
+ * Signature: (JJIIZDDDDDI)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_aprilTagDetectInternal
+  (JNIEnv* env, jclass cls, jlong detectIdx, jlong pData, jint rows, jint cols,
+   jboolean doPoseEstimation, jdouble tagWidthMeters, jdouble fx, jdouble fy,
+   jdouble cx, jdouble cy, jint nIters)
+{
+  // No image, can't do anything
+  if (!pData) {
+    return nullptr;
+  }
+
+  // Make an image_u8_t header for the Mat data
+  image_u8_t im = {static_cast<int32_t>(cols), static_cast<int32_t>(rows),
+                   static_cast<int32_t>(cols),
+                   reinterpret_cast<uint8_t*>(pData)};
+
+  // Get our detector
+  auto state =
+      std::find_if(detectors.begin(), detectors.end(),
+                   [&](DetectorState& s) { return s.id == detectIdx; });
+  if (state == detectors.end()) {
+    return nullptr;
+  }
+
+  // And run the detector on our new image
+  zarray_t* detections = apriltag_detector_detect(state->td, &im);
+
+  int size = zarray_size(detections);
+
+  // Object array to return to Java
+  jobjectArray jarr = env->NewObjectArray(size, detectionClass, nullptr);
+  if (!jarr) {
+    std::printf("Couldn't make array\n");
+    return nullptr;
+  }
+
+  // Global pose
+  apriltag_pose_t pose1;
+  std::memset(&pose1, 0, sizeof(pose1));
+
+  apriltag_pose_t pose2;
+  std::memset(&pose2, 0, sizeof(pose2));
+
+  // std::printf("Created array %llu! Got %i targets!\n", &jarr, size);
+  //  Add our detected targets to the array
+  for (int i = 0; i < size; ++i) {
+    apriltag_detection_t* det = nullptr;
+    zarray_get(detections, i, &det);
+
+    if (det != nullptr) {
+      double err1 =
+          HUGE_VAL;  // Should get overwritten if pose estimation is happening
+      double err2 = HUGE_VAL;
+      if (doPoseEstimation) {
+        // Feed results to the pose estimator
+        apriltag_detection_info_t info{det, tagWidthMeters, fx, fy, cx, cy};
+        estimate_tag_pose_orthogonal_iteration(&info, &err1, &pose1, &err2,
+                                               &pose2, nIters);
+      }
+
+      jobject obj = MakeJObject(env, det, pose1, pose2, err1, err2);
+
+      env->SetObjectArrayElement(jarr, i, obj);
+    }
+  }
+
+  // Now that stuff's in our Java-side array, we can clean up native memory
+  apriltag_detections_destroy(detections);
+
+  return jarr;
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    aprilTagDestroy
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_aprilTagDestroy
+  (JNIEnv* env, jclass clazz, jlong detectIdx)
+{
+  auto state =
+      std::find_if(detectors.begin(), detectors.end(),
+                   [&](DetectorState& s) { return s.id == detectIdx; });
+
+  if (state == detectors.end()) {
+    return;
+  }
+
+  if (state->td) {
+    apriltag_detector_destroy(state->td);
+    state->td = nullptr;
+  }
+  if (state->tf) {
+    state->tf_destroy(state->tf);
+    state->tf = nullptr;
+  }
+
+  detectors.erase(detectors.begin() + detectIdx);
+}
+}  // extern "C"

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

@@ -21,19 +21,8 @@ struct WPILIB_DLLEXPORT AprilTag {
 
   /**
    * Checks equality between this AprilTag and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
    */
-  bool operator==(const AprilTag& other) const;
-
-  /**
-   * Checks inequality between this AprilTag and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const AprilTag& other) const;
+  bool operator==(const AprilTag&) const = default;
 };
 
 WPILIB_DLLEXPORT

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,20 @@ 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.
- */
+ * Pose3ds in the JSON are measured using the normal FRC coordinate system, NWU
+ * with the origin at the bottom-right corner of the blue alliance wall.
+ * SetOrigin(OriginPosition) can be used to change the poses returned from
+ * GetTagPose(int) to be from the perspective of a specific alliance. */
 class WPILIB_DLLEXPORT AprilTagFieldLayout {
  public:
+  enum class OriginPosition {
+    kBlueAllianceWallRightSide,
+    kRedAllianceWallRightSide,
+  };
+
   AprilTagFieldLayout() = default;
 
   /**
@@ -52,21 +55,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.
@@ -86,25 +100,14 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout {
 
   /*
    * Checks equality between this AprilTagFieldLayout and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
    */
-  bool operator==(const AprilTagFieldLayout& other) const;
-
-  /**
-   * Checks inequality between this AprilTagFieldLayout and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const AprilTagFieldLayout& other) const;
+  bool operator==(const AprilTagFieldLayout&) const = default;
 
  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" : 16.4592,
+    "width" : 8.2296
+  }
+}

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/java/edu/wpi/first/apriltag/jni/JNITest.java

@@ -0,0 +1,16 @@
+// 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.jni;
+
+import org.junit.jupiter.api.Test;
+
+public class JNITest {
+  @Test
+  void jniLinkTest() {
+    // Test to verify that the JNI test link works correctly.
+    var detector = AprilTagJNI.aprilTagCreate("tag16h5", 2.0, 0.0, 1, false, false);
+    AprilTagJNI.aprilTagDestroy(detector);
+  }
+}

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

@@ -2,12 +2,10 @@
 // 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 "gtest/gtest.h"
 
-class Switch {
- public:
-  virtual ~Switch() = default;
-
-  /// \brief Returns true when the switch is triggered.
-  virtual bool Get() = 0;
-};
+int main(int argc, char** argv) {
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

azure-pipelines-testbench.yaml

@@ -38,6 +38,7 @@ stages:
 
   - stage: TestBench
     displayName: Test Bench
+    condition: false
     jobs:
       - job: Cpp
         displayName: C++

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.1'
     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

@@ -1,9 +1,13 @@
 repositories {
     maven {
-        url "https://plugins.gradle.org/m2/"
         mavenLocal()
+        maven {
+            url = 'https://frcmaven.wpi.edu/artifactory/ex-gradle'
+        }
+        mavenCentral()
+        url "https://plugins.gradle.org/m2/"
     }
 }
 dependencies {
-    implementation "edu.wpi.first:native-utils:2023.6.0"
+    implementation "edu.wpi.first:native-utils:2023.9.0"
 }

cscore/build.gradle

@@ -61,8 +61,13 @@ model {
             if (!it.buildable || !(it instanceof NativeBinarySpec)) {
                 return
             }
-            lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
-            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            if (it.component.name == "${nativeName}JNI") {
+                lib project: ':wpinet', library: 'wpinet', linkage: 'static'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+            } else {
+                lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            }
         }
     }
 }
@@ -176,6 +181,8 @@ nativeUtils.exportsConfigs {
     }
 }
 
+apply from: "${rootDir}/shared/imgui.gradle"
+
 model {
     components {
         examplesMap.each { key, value ->
@@ -188,7 +195,7 @@ model {
                             lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
                             lib project: ':wpigui', library: 'wpigui', linkage: 'static'
                             lib library: 'cscore', linkage: 'shared'
-                            nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                            nativeUtils.useRequiredLibrary(it, 'imgui')
                             if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
                                 it.buildable = false
                                 return

cscore/src/main/java/edu/wpi/first/cscore/VideoMode.java

@@ -13,7 +13,9 @@ public class VideoMode {
     kYUYV(2),
     kRGB565(3),
     kBGR(4),
-    kGray(5);
+    kGray(5),
+    kY16(6),
+    kUYVY(7);
 
     private final int value;
 

cscore/src/main/native/cpp/Frame.cpp

@@ -217,7 +217,7 @@ Image* Frame::ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat,
   // Color convert
   switch (pixelFormat) {
     case VideoMode::kRGB565:
-      // If source is YUYV or Gray, need to convert to BGR first
+      // If source is YUYV, UYVY, Gray, or Y16, need to convert to BGR first
       if (cur->pixelFormat == VideoMode::kYUYV) {
         // Check to see if BGR version already exists...
         if (Image* newImage =
@@ -226,6 +226,14 @@ Image* Frame::ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat,
         } else {
           cur = ConvertYUYVToBGR(cur);
         }
+      } else if (cur->pixelFormat == VideoMode::kUYVY) {
+        // Check to see if BGR version already exists...
+        if (Image* newImage =
+                GetExistingImage(cur->width, cur->height, VideoMode::kBGR)) {
+          cur = newImage;
+        } else {
+          cur = ConvertUYVYToBGR(cur);
+        }
       } else if (cur->pixelFormat == VideoMode::kGray) {
         // Check to see if BGR version already exists...
         if (Image* newImage =
@@ -234,18 +242,35 @@ Image* Frame::ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat,
         } else {
           cur = ConvertGrayToBGR(cur);
         }
-      }
-      return ConvertBGRToRGB565(cur);
-    case VideoMode::kGray:
-      // If source is YUYV or RGB565, need to convert to BGR first
-      if (cur->pixelFormat == VideoMode::kYUYV) {
+      } else if (cur->pixelFormat == VideoMode::kY16) {
         // Check to see if BGR version already exists...
         if (Image* newImage =
                 GetExistingImage(cur->width, cur->height, VideoMode::kBGR)) {
           cur = newImage;
+        } else if (Image* newImage = GetExistingImage(cur->width, cur->height,
+                                                      VideoMode::kGray)) {
+          cur = ConvertGrayToBGR(newImage);
         } else {
-          cur = ConvertYUYVToBGR(cur);
+          cur = ConvertGrayToBGR(ConvertY16ToGray(cur));
         }
+      }
+      return ConvertBGRToRGB565(cur);
+    case VideoMode::kGray:
+    case VideoMode::kY16:
+      // If source is also grayscale, convert directly
+      if (pixelFormat == VideoMode::kGray &&
+          cur->pixelFormat == VideoMode::kY16) {
+        return ConvertY16ToGray(cur);
+      } else if (pixelFormat == VideoMode::kY16 &&
+                 cur->pixelFormat == VideoMode::kGray) {
+        return ConvertGrayToY16(cur);
+      }
+      // If source is YUYV, UYVY, convert directly to Gray
+      // If RGB565, need to convert to BGR first
+      if (cur->pixelFormat == VideoMode::kYUYV) {
+        cur = ConvertYUYVToGray(cur);
+      } else if (cur->pixelFormat == VideoMode::kUYVY) {
+        cur = ConvertUYVYToGray(cur);
       } else if (cur->pixelFormat == VideoMode::kRGB565) {
         // Check to see if BGR version already exists...
         if (Image* newImage =
@@ -254,12 +279,18 @@ Image* Frame::ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat,
         } else {
           cur = ConvertRGB565ToBGR(cur);
         }
+        cur = ConvertBGRToGray(cur);
       }
-      return ConvertBGRToGray(cur);
+      if (pixelFormat == VideoMode::kY16) {
+        cur = ConvertGrayToY16(cur);
+      }
+      return cur;
     case VideoMode::kBGR:
     case VideoMode::kMJPEG:
       if (cur->pixelFormat == VideoMode::kYUYV) {
         cur = ConvertYUYVToBGR(cur);
+      } else if (cur->pixelFormat == VideoMode::kUYVY) {
+        cur = ConvertUYVYToBGR(cur);
       } else if (cur->pixelFormat == VideoMode::kRGB565) {
         cur = ConvertRGB565ToBGR(cur);
       } else if (cur->pixelFormat == VideoMode::kGray) {
@@ -268,9 +299,23 @@ Image* Frame::ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat,
         } else {
           return ConvertGrayToMJPEG(cur, defaultJpegQuality);
         }
+      } else if (cur->pixelFormat == VideoMode::kY16) {
+        // Check to see if Gray version already exists...
+        if (Image* newImage =
+                GetExistingImage(cur->width, cur->height, VideoMode::kGray)) {
+          cur = newImage;
+        } else {
+          cur = ConvertY16ToGray(cur);
+        }
+        if (pixelFormat == VideoMode::kBGR) {
+          return ConvertGrayToBGR(cur);
+        } else {
+          return ConvertGrayToMJPEG(cur, defaultJpegQuality);
+        }
       }
       break;
     case VideoMode::kYUYV:
+    case VideoMode::kUYVY:
     default:
       return nullptr;  // Unsupported
   }
@@ -351,6 +396,72 @@ Image* Frame::ConvertYUYVToBGR(Image* image) {
   return rv;
 }
 
+Image* Frame::ConvertYUYVToGray(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kYUYV) {
+    return nullptr;
+  }
+
+  // Allocate a grayscale image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kGray, image->width, image->height,
+                                image->width * image->height);
+
+  // Convert
+  cv::cvtColor(image->AsMat(), newImage->AsMat(), cv::COLOR_YUV2GRAY_YUYV);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::scoped_lock lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
+Image* Frame::ConvertUYVYToBGR(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kUYVY) {
+    return nullptr;
+  }
+
+  // Allocate a BGR image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kBGR, image->width, image->height,
+                                image->width * image->height * 3);
+
+  // Convert
+  cv::cvtColor(image->AsMat(), newImage->AsMat(), cv::COLOR_YUV2BGR_UYVY);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::scoped_lock lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
+Image* Frame::ConvertUYVYToGray(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kUYVY) {
+    return nullptr;
+  }
+
+  // Allocate a grayscale image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kGray, image->width, image->height,
+                                image->width * image->height);
+
+  // Convert
+  cv::cvtColor(image->AsMat(), newImage->AsMat(), cv::COLOR_YUV2GRAY_UYVY);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::scoped_lock lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
 Image* Frame::ConvertBGRToRGB565(Image* image) {
   if (!image || image->pixelFormat != VideoMode::kBGR) {
     return nullptr;
@@ -509,6 +620,50 @@ Image* Frame::ConvertGrayToMJPEG(Image* image, int quality) {
   return rv;
 }
 
+Image* Frame::ConvertGrayToY16(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kGray) {
+    return nullptr;
+  }
+
+  // Allocate a Y16 image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kY16, image->width, image->height,
+                                image->width * image->height * 2);
+
+  // Convert with linear scaling
+  image->AsMat().convertTo(newImage->AsMat(), CV_16U, 256);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::scoped_lock lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
+Image* Frame::ConvertY16ToGray(Image* image) {
+  if (!image || image->pixelFormat != VideoMode::kY16) {
+    return nullptr;
+  }
+
+  // Allocate a Grayscale image
+  auto newImage =
+      m_impl->source.AllocImage(VideoMode::kGray, image->width, image->height,
+                                image->width * image->height);
+
+  // Scale min to 0 and max to 255
+  cv::normalize(image->AsMat(), newImage->AsMat(), 255, 0, cv::NORM_MINMAX);
+
+  // Save the result
+  Image* rv = newImage.release();
+  if (m_impl) {
+    std::scoped_lock lock(m_impl->mutex);
+    m_impl->images.push_back(rv);
+  }
+  return rv;
+}
+
 Image* Frame::GetImageImpl(int width, int height,
                            VideoMode::PixelFormat pixelFormat,
                            int requiredJpegQuality, int defaultJpegQuality) {

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

@@ -195,12 +195,17 @@ class Frame {
   Image* ConvertMJPEGToBGR(Image* image);
   Image* ConvertMJPEGToGray(Image* image);
   Image* ConvertYUYVToBGR(Image* image);
+  Image* ConvertYUYVToGray(Image* image);
+  Image* ConvertUYVYToBGR(Image* image);
+  Image* ConvertUYVYToGray(Image* image);
   Image* ConvertBGRToRGB565(Image* image);
   Image* ConvertRGB565ToBGR(Image* image);
   Image* ConvertBGRToGray(Image* image);
   Image* ConvertGrayToBGR(Image* image);
   Image* ConvertBGRToMJPEG(Image* image, int quality);
   Image* ConvertGrayToMJPEG(Image* image, int quality);
+  Image* ConvertGrayToY16(Image* image);
+  Image* ConvertY16ToGray(Image* image);
 
   Image* GetImage(int width, int height, VideoMode::PixelFormat pixelFormat) {
     if (pixelFormat == VideoMode::kMJPEG) {

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

@@ -74,6 +74,8 @@ class Image {
     switch (pixelFormat) {
       case VideoMode::kYUYV:
       case VideoMode::kRGB565:
+      case VideoMode::kY16:
+      case VideoMode::kUYVY:
         type = CV_8UC2;
         break;
       case VideoMode::kBGR:

cscore/src/main/native/cpp/MjpegServerImpl.cpp

@@ -460,6 +460,12 @@ void MjpegServerImpl::ConnThread::SendHTML(wpi::raw_ostream& os,
       case VideoMode::kGray:
         os << "gray";
         break;
+      case VideoMode::kY16:
+        os << "Y16";
+        break;
+      case VideoMode::kUYVY:
+        os << "UYVY";
+        break;
       default:
         os << "unknown";
         break;
@@ -569,6 +575,12 @@ void MjpegServerImpl::ConnThread::SendJSON(wpi::raw_ostream& os,
       case VideoMode::kGray:
         os << "gray";
         break;
+      case VideoMode::kY16:
+        os << "Y16";
+        break;
+      case VideoMode::kUYVY:
+        os << "UYVY";
+        break;
       default:
         os << "unknown";
         break;
@@ -740,8 +752,10 @@ void MjpegServerImpl::ConnThread::SendStream(wpi::raw_socket_ostream& os) {
         // for adding it if required.
         addDHT = JpegNeedsDHT(data, &size, &locSOF);
         break;
-      case VideoMode::kYUYV:
+      case VideoMode::kUYVY:
       case VideoMode::kRGB565:
+      case VideoMode::kYUYV:
+      case VideoMode::kY16:
       default:
         // Bad frame; sleep for 10 ms so we don't consume all processor time.
         std::this_thread::sleep_for(std::chrono::milliseconds(10));

cscore/src/main/native/cpp/RawSourceImpl.cpp

@@ -26,6 +26,8 @@ void RawSourceImpl::PutFrame(const CS_RawFrame& image) {
   switch (image.pixelFormat) {
     case VideoMode::kYUYV:
     case VideoMode::kRGB565:
+    case VideoMode::kY16:
+    case VideoMode::kUYVY:
       type = CV_8UC2;
       break;
     case VideoMode::kBGR:

cscore/src/main/native/cpp/SourceImpl.cpp

@@ -198,6 +198,10 @@ bool SourceImpl::SetConfigJson(const wpi::json& config, CS_Status* status) {
         mode.pixelFormat = cs::VideoMode::kBGR;
       } else if (wpi::equals_lower(str, "gray")) {
         mode.pixelFormat = cs::VideoMode::kGray;
+      } else if (wpi::equals_lower(str, "y16")) {
+        mode.pixelFormat = cs::VideoMode::kY16;
+      } else if (wpi::equals_lower(str, "uyvy")) {
+        mode.pixelFormat = cs::VideoMode::kUYVY;
       } else {
         SWARNING("SetConfigJson: could not understand pixel format value '{}'",
                  str);
@@ -360,6 +364,12 @@ wpi::json SourceImpl::GetConfigJsonObject(CS_Status* status) {
     case VideoMode::kGray:
       pixelFormat = "gray";
       break;
+    case VideoMode::kY16:
+      pixelFormat = "y16";
+      break;
+    case VideoMode::kUYVY:
+      pixelFormat = "uyvy";
+      break;
     default:
       break;
   }

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

@@ -93,7 +93,9 @@ enum CS_PixelFormat {
   CS_PIXFMT_YUYV,
   CS_PIXFMT_RGB565,
   CS_PIXFMT_BGR,
-  CS_PIXFMT_GRAY
+  CS_PIXFMT_GRAY,
+  CS_PIXFMT_Y16,
+  CS_PIXFMT_UYVY
 };
 
 /**

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

@@ -68,7 +68,9 @@ struct VideoMode : public CS_VideoMode {
     kYUYV = CS_PIXFMT_YUYV,
     kRGB565 = CS_PIXFMT_RGB565,
     kBGR = CS_PIXFMT_BGR,
-    kGray = CS_PIXFMT_GRAY
+    kGray = CS_PIXFMT_GRAY,
+    kY16 = CS_PIXFMT_Y16,
+    kUYVY = CS_PIXFMT_UYVY
   };
   VideoMode() {
     pixelFormat = 0;
@@ -88,8 +90,6 @@ struct VideoMode : public CS_VideoMode {
     return pixelFormat == other.pixelFormat && width == other.width &&
            height == other.height && fps == other.fps;
   }
-
-  bool operator!=(const VideoMode& other) const { return !(*this == other); }
 };
 
 /**

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

@@ -140,8 +140,6 @@ class VideoSource {
     return m_handle == other.m_handle;
   }
 
-  bool operator!=(const VideoSource& other) const { return !(*this == other); }
-
   /**
    * Get the kind of the source.
    */
@@ -736,8 +734,6 @@ class VideoSink {
     return m_handle == other.m_handle;
   }
 
-  bool operator!=(const VideoSink& other) const { return !(*this == other); }
-
   /**
    * Get the kind of the sink.
    */

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

@@ -82,6 +82,10 @@ static VideoMode::PixelFormat ToPixelFormat(__u32 pixelFormat) {
       return VideoMode::kBGR;
     case V4L2_PIX_FMT_GREY:
       return VideoMode::kGray;
+    case V4L2_PIX_FMT_Y16:
+      return VideoMode::kY16;
+    case V4L2_PIX_FMT_UYVY:
+      return VideoMode::kUYVY;
     default:
       return VideoMode::kUnknown;
   }
@@ -100,6 +104,10 @@ static __u32 FromPixelFormat(VideoMode::PixelFormat pixelFormat) {
       return V4L2_PIX_FMT_BGR24;
     case VideoMode::kGray:
       return V4L2_PIX_FMT_GREY;
+    case VideoMode::kY16:
+      return V4L2_PIX_FMT_Y16;
+    case VideoMode::kUYVY:
+      return V4L2_PIX_FMT_UYVY;
     default:
       return 0;
   }
@@ -140,6 +148,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 +173,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 +1404,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
 
   //

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

@@ -2,6 +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.
 
+#include "Instance.h"
 #include "cscore_cpp.h"
 
 namespace cs {
@@ -9,12 +10,16 @@ namespace cs {
 CS_Source CreateUsbCameraDev(std::string_view name, int dev,
                              CS_Status* status) {
   *status = CS_INVALID_HANDLE;
+  WPI_ERROR(Instance::GetInstance().logger,
+            "USB Camera support not implemented for macOS");
   return 0;
 }
 
 CS_Source CreateUsbCameraPath(std::string_view name, std::string_view path,
                               CS_Status* status) {
   *status = CS_INVALID_HANDLE;
+  WPI_ERROR(Instance::GetInstance().logger,
+            "USB Camera support not implemented for macOS");
   return 0;
 }
 
@@ -35,6 +40,8 @@ UsbCameraInfo GetUsbCameraInfo(CS_Source source, CS_Status* status) {
 
 std::vector<UsbCameraInfo> EnumerateUsbCameras(CS_Status* status) {
   *status = CS_INVALID_HANDLE;
+  WPI_ERROR(Instance::GetInstance().logger,
+            "USB Camera support not implemented for macOS");
   return std::vector<UsbCameraInfo>{};
 }
 

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

@@ -356,6 +356,12 @@ void UsbCameraImpl::ProcessFrame(IMFSample* videoSample,
                         tmpMat.total());
       tmpMat.copyTo(dest->AsMat());
       break;
+    case cs::VideoMode::PixelFormat::kY16:
+      tmpMat = cv::Mat(mode.height, mode.width, CV_8UC2, ptr, pitch);
+      dest =
+          AllocImage(VideoMode::kY16, tmpMat.cols, tmpMat.rows, tmpMat.total());
+      tmpMat.copyTo(dest->AsMat());
+      break;
     case cs::VideoMode::PixelFormat::kBGR:
       tmpMat = cv::Mat(mode.height, mode.width, CV_8UC3, ptr, pitch);
       dest = AllocImage(VideoMode::kBGR, tmpMat.cols, tmpMat.rows,
@@ -368,6 +374,12 @@ void UsbCameraImpl::ProcessFrame(IMFSample* videoSample,
                         tmpMat.total() * 2);
       tmpMat.copyTo(dest->AsMat());
       break;
+    case cs::VideoMode::PixelFormat::kUYVY:
+      tmpMat = cv::Mat(mode.height, mode.width, CV_8UC2, ptr, pitch);
+      dest = AllocImage(VideoMode::kUYVY, tmpMat.cols, tmpMat.rows,
+                        tmpMat.total() * 2);
+      tmpMat.copyTo(dest->AsMat());
+      break;
     default:
       doFinalSet = false;
       break;
@@ -461,9 +473,10 @@ LRESULT UsbCameraImpl::PumpMain(HWND hwnd, UINT uiMsg, WPARAM wParam,
 
 static cs::VideoMode::PixelFormat GetFromGUID(const GUID& guid) {
   // Compare GUID to one of the supported ones
-  if (IsEqualGUID(guid, MFVideoFormat_NV12)) {
-    // GrayScale
+  if (IsEqualGUID(guid, MFVideoFormat_L8)) {
     return cs::VideoMode::PixelFormat::kGray;
+  } else if (IsEqualGUID(guid, MFVideoFormat_L16)) {
+    return cs::VideoMode::PixelFormat::kY16;
   } else if (IsEqualGUID(guid, MFVideoFormat_YUY2)) {
     return cs::VideoMode::PixelFormat::kYUYV;
   } else if (IsEqualGUID(guid, MFVideoFormat_RGB24)) {
@@ -472,6 +485,8 @@ static cs::VideoMode::PixelFormat GetFromGUID(const GUID& guid) {
     return cs::VideoMode::PixelFormat::kMJPEG;
   } else if (IsEqualGUID(guid, MFVideoFormat_RGB565)) {
     return cs::VideoMode::PixelFormat::kRGB565;
+  } else if (IsEqualGUID(guid, MFVideoFormat_UYVY)) {
+    return cs::VideoMode::PixelFormat::kUYVY;
   } else {
     return cs::VideoMode::PixelFormat::kUnknown;
   }

datalogtool/build.gradle

@@ -25,6 +25,7 @@ if (!project.hasProperty('onlylinuxathena')) {
     def wpilibVersionFileOutput = file("$buildDir/generated/main/cpp/WPILibVersion.cpp")
 
     apply from: "${rootDir}/shared/libssh.gradle"
+    apply from: "${rootDir}/shared/imgui.gradle"
 
     task generateCppVersion() {
         description = 'Generates the wpilib version class'
@@ -103,7 +104,7 @@ if (!project.hasProperty('onlylinuxathena')) {
                     lib project: ':glass', library: 'glass', linkage: 'static'
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
                     lib project: ':wpigui', library: 'wpigui', linkage: 'static'
-                    nativeUtils.useRequiredLibrary(it, 'imgui_static', 'libssh')
+                    nativeUtils.useRequiredLibrary(it, 'imgui', 'libssh')
                     if (it.targetPlatform.operatingSystem.isWindows()) {
                         it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib'
                         it.linker.args << 'ws2_32.lib' << 'advapi32.lib' << 'crypt32.lib' << 'user32.lib'

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

@@ -3,16 +3,17 @@ plugins {
     id "org.ysb33r.doxygen" version "0.7.0"
 }
 
-evaluationDependsOn(':wpiutil')
-evaluationDependsOn(':wpinet')
-evaluationDependsOn(':ntcore')
+evaluationDependsOn(':apriltag')
+evaluationDependsOn(':cameraserver')
 evaluationDependsOn(':cscore')
 evaluationDependsOn(':hal')
-evaluationDependsOn(':cameraserver')
-evaluationDependsOn(':wpimath')
+evaluationDependsOn(':ntcore')
+evaluationDependsOn(':wpilibNewCommands')
 evaluationDependsOn(':wpilibc')
 evaluationDependsOn(':wpilibj')
-evaluationDependsOn(':wpilibNewCommands')
+evaluationDependsOn(':wpimath')
+evaluationDependsOn(':wpinet')
+evaluationDependsOn(':wpiutil')
 
 def baseArtifactIdCpp = 'documentation'
 def artifactGroupIdCpp = 'edu.wpi.first.wpilibc'
@@ -27,15 +28,16 @@ def outputsFolder = file("$project.buildDir/outputs")
 def cppProjectZips = []
 def cppIncludeRoots = []
 
-cppProjectZips.add(project(':hal').cppHeadersZip)
-cppProjectZips.add(project(':wpiutil').cppHeadersZip)
-cppProjectZips.add(project(':wpinet').cppHeadersZip)
-cppProjectZips.add(project(':ntcore').cppHeadersZip)
-cppProjectZips.add(project(':cscore').cppHeadersZip)
+cppProjectZips.add(project(':apriltag').cppHeadersZip)
 cppProjectZips.add(project(':cameraserver').cppHeadersZip)
-cppProjectZips.add(project(':wpimath').cppHeadersZip)
-cppProjectZips.add(project(':wpilibc').cppHeadersZip)
+cppProjectZips.add(project(':cscore').cppHeadersZip)
+cppProjectZips.add(project(':hal').cppHeadersZip)
+cppProjectZips.add(project(':ntcore').cppHeadersZip)
 cppProjectZips.add(project(':wpilibNewCommands').cppHeadersZip)
+cppProjectZips.add(project(':wpilibc').cppHeadersZip)
+cppProjectZips.add(project(':wpimath').cppHeadersZip)
+cppProjectZips.add(project(':wpinet').cppHeadersZip)
+cppProjectZips.add(project(':wpiutil').cppHeadersZip)
 
 doxygen {
     executables {
@@ -126,6 +128,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>^^',
@@ -158,7 +163,7 @@ doxygen {
     warn_if_undocumented false
     warn_no_paramdoc true
 
-    //enable doxygen preprocessor expansion of WPI_DEPRECATED to fix SpeedController docs
+    //enable doxygen preprocessor expansion of WPI_DEPRECATED to fix MotorController docs
     enable_preprocessing true
     macro_expansion true
     expand_only_predef true
@@ -202,19 +207,20 @@ task generateJavaDocs(type: Javadoc) {
     options.addBooleanOption("Xdoclint:html,missing,reference,syntax", true)
     options.addBooleanOption('html5', true)
     options.linkSource(true)
-    dependsOn project(':wpilibj').generateJavaVersion
     dependsOn project(':hal').generateUsageReporting
-    dependsOn project(':wpimath').generateNat
     dependsOn project(':ntcore').ntcoreGenerateJavaTypes
-    source project(':hal').sourceSets.main.java
-    source project(':wpiutil').sourceSets.main.java
-    source project(':wpinet').sourceSets.main.java
-    source project(':cscore').sourceSets.main.java
-    source project(':ntcore').sourceSets.main.java
-    source project(':wpimath').sourceSets.main.java
-    source project(':wpilibj').sourceSets.main.java
+    dependsOn project(':wpilibj').generateJavaVersion
+    dependsOn project(':wpimath').generateNat
+    source project(':apriltag').sourceSets.main.java
     source project(':cameraserver').sourceSets.main.java
+    source project(':cscore').sourceSets.main.java
+    source project(':hal').sourceSets.main.java
+    source project(':ntcore').sourceSets.main.java
     source project(':wpilibNewCommands').sourceSets.main.java
+    source project(':wpilibj').sourceSets.main.java
+    source project(':wpimath').sourceSets.main.java
+    source project(':wpinet').sourceSets.main.java
+    source project(':wpiutil').sourceSets.main.java
     source configurations.javaSource.collect { zipTree(it) }
     include '**/*.java'
     failOnError = true

glass/build.gradle

@@ -24,6 +24,8 @@ if (!project.hasProperty('onlylinuxathena')) {
     def wpilibVersionFileInput = file("src/app/generate/WPILibVersion.cpp.in")
     def wpilibVersionFileOutput = file("$buildDir/generated/app/cpp/WPILibVersion.cpp")
 
+    apply from: "${rootDir}/shared/imgui.gradle"
+
     task generateCppVersion() {
         description = 'Generates the wpilib version class'
         group = 'WPILib'
@@ -111,7 +113,7 @@ if (!project.hasProperty('onlylinuxathena')) {
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                     lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                     lib project: ':wpigui', library: 'wpigui', linkage: 'static'
-                    nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                    nativeUtils.useRequiredLibrary(it, 'imgui')
                 }
                 appendDebugPathToBinaries(binaries)
             }
@@ -142,7 +144,7 @@ if (!project.hasProperty('onlylinuxathena')) {
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                     lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
                     lib project: ':wpigui', library: 'wpigui', linkage: 'static'
-                    nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                    nativeUtils.useRequiredLibrary(it, 'imgui')
                 }
                 appendDebugPathToBinaries(binaries)
             }
@@ -182,7 +184,7 @@ if (!project.hasProperty('onlylinuxathena')) {
                     lib project: ':wpimath', library: 'wpimath', linkage: 'static'
                     lib project: ':wpigui', library: 'wpigui', linkage: 'static'
                     nativeUtils.useRequiredLibrary(it, 'opencv_static')
-                    nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                    nativeUtils.useRequiredLibrary(it, 'imgui')
                     if (it.targetPlatform.operatingSystem.isWindows()) {
                         it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib'
                         it.linker.args << '/DELAYLOAD:MF.dll' << '/DELAYLOAD:MFReadWrite.dll' << '/DELAYLOAD:MFPlat.dll' << '/delay:nobind'

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/hardware/MotorController.cpp

@@ -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.
 
-#include "glass/hardware/SpeedController.h"
+#include "glass/hardware/MotorController.h"
 
 #include <imgui.h>
 #include <imgui_internal.h>
@@ -12,13 +12,13 @@
 
 using namespace glass;
 
-void glass::DisplaySpeedController(SpeedControllerModel* m) {
+void glass::DisplayMotorController(MotorControllerModel* m) {
   // Get duty cycle data from the model and do not display anything if the data
   // is null.
   auto dc = m->GetPercentData();
   if (!dc || !m->Exists()) {
     ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
-    ImGui::Text("Unknown SpeedController");
+    ImGui::Text("Unknown MotorController");
     ImGui::PopStyleColor();
     return;
   }

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;
@@ -987,7 +991,7 @@ void PlotProvider::DisplayMenu() {
     for (size_t i = 0; i <= numWindows; ++i) {
       std::snprintf(id, sizeof(id), "Plot <%d>", static_cast<int>(i));
       bool match = false;
-      for (size_t j = i; j < numWindows; ++j) {
+      for (size_t j = 0; j < numWindows; ++j) {
         if (m_windows[j]->GetId() == id) {
           match = true;
           break;

glass/src/lib/native/include/glass/DataSource.h

@@ -11,6 +11,7 @@
 
 #include <imgui.h>
 #include <wpi/Signal.h>
+#include <wpi/spinlock.h>
 
 namespace glass {
 
@@ -37,12 +38,21 @@ class DataSource {
   bool IsDigital() const { return m_digital; }
 
   void SetValue(double value, int64_t time = 0) {
+    std::scoped_lock lock{m_valueMutex};
     m_value = value;
     m_valueTime = time;
     valueChanged(value, time);
   }
-  double GetValue() const { return m_value; }
-  int64_t GetValueTime() const { return m_valueTime; }
+
+  double GetValue() const {
+    std::scoped_lock lock{m_valueMutex};
+    return m_value;
+  }
+
+  int64_t GetValueTime() const {
+    std::scoped_lock lock{m_valueMutex};
+    return m_valueTime;
+  }
 
   // drag source helpers
   void LabelText(const char* label, const char* fmt, ...) const IM_FMTARGS(3);
@@ -59,7 +69,7 @@ class DataSource {
                 ImGuiInputTextFlags flags = 0) const;
   void EmitDrag(ImGuiDragDropFlags flags = 0) const;
 
-  wpi::sig::Signal<double, int64_t> valueChanged;
+  wpi::sig::SignalBase<wpi::spinlock, double, int64_t> valueChanged;
 
   static DataSource* Find(std::string_view id);
 
@@ -69,6 +79,7 @@ class DataSource {
   std::string m_id;
   std::string& m_name;
   bool m_digital = false;
+  mutable wpi::spinlock m_valueMutex;
   double m_value = 0;
   int64_t m_valueTime = 0;
 };

glass/src/lib/native/include/glass/hardware/MotorController.h

@@ -8,12 +8,12 @@
 
 namespace glass {
 class DataSource;
-class SpeedControllerModel : public Model {
+class MotorControllerModel : public Model {
  public:
   virtual const char* GetName() const = 0;
   virtual const char* GetSimDevice() const = 0;
   virtual DataSource* GetPercentData() = 0;
   virtual void SetPercent(double value) = 0;
 };
-void DisplaySpeedController(SpeedControllerModel* m);
+void DisplayMotorController(MotorControllerModel* m);
 }  // namespace glass

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

@@ -38,7 +38,7 @@ class FMSModel : public Model {
   virtual void SetEnabled(bool val) = 0;
   virtual void SetTest(bool val) = 0;
   virtual void SetAutonomous(bool val) = 0;
-  virtual void SetGameSpecificMessage(const char* val) = 0;
+  virtual void SetGameSpecificMessage(std::string_view val) = 0;
 };
 
 /**

glass/src/libnt/native/cpp/NTCommandScheduler.cpp

@@ -43,5 +43,5 @@ void NTCommandSchedulerModel::Update() {
 }
 
 bool NTCommandSchedulerModel::Exists() {
-  return m_inst.IsConnected() && m_commands.Exists();
+  return m_commands.Exists();
 }

glass/src/libnt/native/cpp/NTCommandSelector.cpp

@@ -37,5 +37,5 @@ void NTCommandSelectorModel::Update() {
 }
 
 bool NTCommandSelectorModel::Exists() {
-  return m_inst.IsConnected() && m_running.Exists();
+  return m_running.Exists();
 }

glass/src/libnt/native/cpp/NTDifferentialDrive.cpp

@@ -56,5 +56,5 @@ void NTDifferentialDriveModel::Update() {
 }
 
 bool NTDifferentialDriveModel::Exists() {
-  return m_inst.IsConnected() && m_lPercent.Exists();
+  return m_lPercent.Exists();
 }

glass/src/libnt/native/cpp/NTDigitalInput.cpp

@@ -15,8 +15,8 @@ NTDigitalInputModel::NTDigitalInputModel(std::string_view path)
 NTDigitalInputModel::NTDigitalInputModel(nt::NetworkTableInstance inst,
                                          std::string_view path)
     : m_inst{inst},
-      m_value{inst.GetBooleanTopic(fmt::format("{}/Value", path))
-                  .Subscribe(false, {{nt::PubSubOption::SendAll(true)}})},
+      m_value{
+          inst.GetBooleanTopic(fmt::format("{}/Value", path)).Subscribe(false)},
       m_name{inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe("")},
       m_valueData{fmt::format("NT_DIn:{}", path)},
       m_nameValue{wpi::rsplit(path, '/').second} {
@@ -33,5 +33,5 @@ void NTDigitalInputModel::Update() {
 }
 
 bool NTDigitalInputModel::Exists() {
-  return m_inst.IsConnected() && m_value.Exists();
+  return m_value.Exists();
 }

glass/src/libnt/native/cpp/NTDigitalOutput.cpp

@@ -14,8 +14,8 @@ NTDigitalOutputModel::NTDigitalOutputModel(std::string_view path)
 NTDigitalOutputModel::NTDigitalOutputModel(nt::NetworkTableInstance inst,
                                            std::string_view path)
     : m_inst{inst},
-      m_value{inst.GetBooleanTopic(fmt::format("{}/Value", path))
-                  .GetEntry(false, {{nt::PubSubOption::SendAll(true)}})},
+      m_value{
+          inst.GetBooleanTopic(fmt::format("{}/Value", path)).GetEntry(false)},
       m_name{inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe("")},
       m_controllable{inst.GetBooleanTopic(fmt::format("{}/.controllable", path))
                          .Subscribe(false)},
@@ -40,5 +40,5 @@ void NTDigitalOutputModel::Update() {
 }
 
 bool NTDigitalOutputModel::Exists() {
-  return m_inst.IsConnected() && m_value.Exists();
+  return m_value.Exists();
 }

glass/src/libnt/native/cpp/NTFMS.cpp

@@ -21,11 +21,11 @@ NTFMSModel::NTFMSModel(nt::NetworkTableInstance inst, std::string_view path)
           inst.GetStringTopic(fmt::format("{}/GameSpecificMessage", path))
               .Subscribe("")},
       m_alliance{inst.GetBooleanTopic(fmt::format("{}/IsRedAlliance", path))
-                     .Subscribe(false, {{nt::PubSubOption::SendAll(true)}})},
+                     .Subscribe(false)},
       m_station{inst.GetIntegerTopic(fmt::format("{}/StationNumber", path))
-                    .Subscribe(0, {{nt::PubSubOption::SendAll(true)}})},
+                    .Subscribe(0)},
       m_controlWord{inst.GetIntegerTopic(fmt::format("{}/FMSControlData", path))
-                        .Subscribe(0, {{nt::PubSubOption::SendAll(true)}})},
+                        .Subscribe(0)},
       m_fmsAttached{fmt::format("NT_FMS:FMSAttached:{}", path)},
       m_dsAttached{fmt::format("NT_FMS:DSAttached:{}", path)},
       m_allianceStationId{fmt::format("NT_FMS:AllianceStationID:{}", path)},
@@ -73,5 +73,5 @@ void NTFMSModel::Update() {
 }
 
 bool NTFMSModel::Exists() {
-  return m_inst.IsConnected() && m_controlWord.Exists();
+  return m_controlWord.Exists();
 }

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

@@ -112,10 +112,7 @@ NTField2DModel::NTField2DModel(nt::NetworkTableInstance inst,
                                std::string_view path)
     : m_path{fmt::format("{}/", path)},
       m_inst{inst},
-      m_tableSub{inst,
-                 {{m_path}},
-                 {{nt::PubSubOption::SendAll(true),
-                   nt::PubSubOption::Periodic(0.05)}}},
+      m_tableSub{inst, {{m_path}}, {.periodic = 0.05, .sendAll = true}},
       m_nameTopic{inst.GetTopic(fmt::format("{}/.name", path))},
       m_poller{inst} {
   m_poller.AddListener(m_tableSub, nt::EventFlags::kTopic |
@@ -171,7 +168,7 @@ void NTField2DModel::Update() {
 }
 
 bool NTField2DModel::Exists() {
-  return m_inst.IsConnected() && m_nameTopic.Exists();
+  return m_nameTopic.Exists();
 }
 
 bool NTField2DModel::IsReadOnly() {

glass/src/libnt/native/cpp/NTGyro.cpp

@@ -14,8 +14,7 @@ NTGyroModel::NTGyroModel(std::string_view path)
 
 NTGyroModel::NTGyroModel(nt::NetworkTableInstance inst, std::string_view path)
     : m_inst{inst},
-      m_angle{inst.GetDoubleTopic(fmt::format("{}/Value", path))
-                  .Subscribe(0, {{nt::PubSubOption::SendAll(true)}})},
+      m_angle{inst.GetDoubleTopic(fmt::format("{}/Value", path)).Subscribe(0)},
       m_name{inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe({})},
       m_angleData{fmt::format("NT_Gyro:{}", path)},
       m_nameValue{wpi::rsplit(path, '/').second} {}
@@ -30,5 +29,5 @@ void NTGyroModel::Update() {
 }
 
 bool NTGyroModel::Exists() {
-  return m_inst.IsConnected() && m_angle.Exists();
+  return m_angle.Exists();
 }

glass/src/libnt/native/cpp/NTMecanumDrive.cpp

@@ -22,16 +22,16 @@ NTMecanumDriveModel::NTMecanumDriveModel(nt::NetworkTableInstance inst,
                          .Subscribe(0)},
       m_flPercent{
           inst.GetDoubleTopic(fmt::format("{}/Front Left Motor Speed", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
+              .GetEntry(0)},
       m_frPercent{
           inst.GetDoubleTopic(fmt::format("{}/Front Right Motor Speed", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
+              .GetEntry(0)},
       m_rlPercent{
           inst.GetDoubleTopic(fmt::format("{}/Rear Left Motor Speed", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
+              .GetEntry(0)},
       m_rrPercent{
           inst.GetDoubleTopic(fmt::format("{}/Rear Right Motor Speed", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
+              .GetEntry(0)},
       m_nameValue{wpi::rsplit(path, '/').second},
       m_flPercentData{fmt::format("NTMcnmDriveFL:{}", path)},
       m_frPercentData{fmt::format("NTMcnmDriveFR:{}", path)},
@@ -81,5 +81,5 @@ void NTMecanumDriveModel::Update() {
 }
 
 bool NTMecanumDriveModel::Exists() {
-  return m_inst.IsConnected() && m_flPercent.Exists();
+  return m_flPercent.Exists();
 }

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

@@ -240,10 +240,7 @@ NTMechanism2DModel::NTMechanism2DModel(nt::NetworkTableInstance inst,
                                        std::string_view path)
     : m_inst{inst},
       m_path{fmt::format("{}/", path)},
-      m_tableSub{inst,
-                 {{m_path}},
-                 {{nt::PubSubOption::SendAll(true),
-                   nt::PubSubOption::Periodic(0.05)}}},
+      m_tableSub{inst, {{m_path}}, {.periodic = 0.05, .sendAll = true}},
       m_nameTopic{m_inst.GetTopic(fmt::format("{}/.name", path))},
       m_dimensionsTopic{m_inst.GetTopic(fmt::format("{}/dims", path))},
       m_bgColorTopic{m_inst.GetTopic(fmt::format("{}/backgroundColor", path))},
@@ -289,16 +286,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,20 +300,39 @@ 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);
+          }
+        }
       }
     }
   }
 }
 
 bool NTMechanism2DModel::Exists() {
-  return m_inst.IsConnected() && m_nameTopic.Exists();
+  return m_nameTopic.Exists();
 }
 
 bool NTMechanism2DModel::IsReadOnly() {

glass/src/libnt/native/cpp/NTMotorController.cpp

@@ -2,32 +2,31 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "glass/networktables/NTSpeedController.h"
+#include "glass/networktables/NTMotorController.h"
 
 #include <fmt/format.h>
 #include <wpi/StringExtras.h>
 
 using namespace glass;
 
-NTSpeedControllerModel::NTSpeedControllerModel(std::string_view path)
-    : NTSpeedControllerModel(nt::NetworkTableInstance::GetDefault(), path) {}
+NTMotorControllerModel::NTMotorControllerModel(std::string_view path)
+    : NTMotorControllerModel(nt::NetworkTableInstance::GetDefault(), path) {}
 
-NTSpeedControllerModel::NTSpeedControllerModel(nt::NetworkTableInstance inst,
+NTMotorControllerModel::NTMotorControllerModel(nt::NetworkTableInstance inst,
                                                std::string_view path)
     : m_inst{inst},
-      m_value{inst.GetDoubleTopic(fmt::format("{}/Value", path))
-                  .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
+      m_value{inst.GetDoubleTopic(fmt::format("{}/Value", path)).GetEntry(0)},
       m_name{inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe("")},
       m_controllable{inst.GetBooleanTopic(fmt::format("{}/.controllable", path))
                          .Subscribe(false)},
       m_valueData{fmt::format("NT_SpdCtrl:{}", path)},
       m_nameValue{wpi::rsplit(path, '/').second} {}
 
-void NTSpeedControllerModel::SetPercent(double value) {
+void NTMotorControllerModel::SetPercent(double value) {
   m_value.Set(value);
 }
 
-void NTSpeedControllerModel::Update() {
+void NTMotorControllerModel::Update() {
   for (auto&& v : m_value.ReadQueue()) {
     m_valueData.SetValue(v.value, v.time);
   }
@@ -39,6 +38,6 @@ void NTSpeedControllerModel::Update() {
   }
 }
 
-bool NTSpeedControllerModel::Exists() {
-  return m_inst.IsConnected() && m_value.Exists();
+bool NTMotorControllerModel::Exists() {
+  return m_value.Exists();
 }

glass/src/libnt/native/cpp/NTPIDController.cpp

@@ -18,14 +18,11 @@ NTPIDControllerModel::NTPIDControllerModel(nt::NetworkTableInstance inst,
       m_name{inst.GetStringTopic(fmt::format("{}/.name", path)).Subscribe("")},
       m_controllable{inst.GetBooleanTopic(fmt::format("{}/.controllable", path))
                          .Subscribe(false)},
-      m_p{inst.GetDoubleTopic(fmt::format("{}/p", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
-      m_i{inst.GetDoubleTopic(fmt::format("{}/i", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
-      m_d{inst.GetDoubleTopic(fmt::format("{}/d", path))
-              .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
-      m_setpoint{inst.GetDoubleTopic(fmt::format("{}/setpoint", path))
-                     .GetEntry(0, {{nt::PubSubOption::SendAll(true)}})},
+      m_p{inst.GetDoubleTopic(fmt::format("{}/p", path)).GetEntry(0)},
+      m_i{inst.GetDoubleTopic(fmt::format("{}/i", path)).GetEntry(0)},
+      m_d{inst.GetDoubleTopic(fmt::format("{}/d", path)).GetEntry(0)},
+      m_setpoint{
+          inst.GetDoubleTopic(fmt::format("{}/setpoint", path)).GetEntry(0)},
       m_pData{fmt::format("NTPIDCtrlP:{}", path)},
       m_iData{fmt::format("NTPIDCtrlI:{}", path)},
       m_dData{fmt::format("NTPIDCtrlD:{}", path)},
@@ -70,5 +67,5 @@ void NTPIDControllerModel::Update() {
 }
 
 bool NTPIDControllerModel::Exists() {
-  return m_inst.IsConnected() && m_setpoint.Exists();
+  return m_setpoint.Exists();
 }

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

@@ -60,5 +60,5 @@ void NTStringChooserModel::Update() {
 }
 
 bool NTStringChooserModel::Exists() {
-  return m_inst.IsConnected() && m_options.Exists();
+  return m_options.Exists();
 }

glass/src/libnt/native/cpp/NTSubsystem.cpp

@@ -34,5 +34,5 @@ void NTSubsystemModel::Update() {
 }
 
 bool NTSubsystemModel::Exists() {
-  return m_inst.IsConnected() && m_defaultCommand.Exists();
+  return m_defaultCommand.Exists();
 }

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

@@ -432,6 +432,27 @@ void NetworkTablesModel::Update() {
         }
       }
       if (event.flags & nt::EventFlags::kUnpublish) {
+        // meta topic handling
+        if (wpi::starts_with(info->name, '$')) {
+          // meta topic handling
+          if (info->name == "$clients") {
+            m_clients.clear();
+          } else if (info->name == "$serverpub") {
+            m_server.publishers.clear();
+          } else if (info->name == "$serversub") {
+            m_server.subscribers.clear();
+          } else if (wpi::starts_with(info->name, "$clientpub$")) {
+            auto it = m_clients.find(wpi::drop_front(info->name, 11));
+            if (it != m_clients.end()) {
+              it->second.publishers.clear();
+            }
+          } else if (wpi::starts_with(info->name, "$clientsub$")) {
+            auto it = m_clients.find(wpi::drop_front(info->name, 11));
+            if (it != m_clients.end()) {
+              it->second.subscribers.clear();
+            }
+          }
+        }
         auto it = std::find(m_sortedEntries.begin(), m_sortedEntries.end(),
                             entry.get());
         // will be removed completely below
@@ -457,6 +478,10 @@ void NetworkTablesModel::Update() {
             entry->info.type_str == "msgpack") {
           // meta topic handling
           if (entry->info.name == "$clients") {
+            // need to remove deleted entries as UpdateClients() uses GetEntry()
+            if (updateTree) {
+              std::erase(m_sortedEntries, nullptr);
+            }
             UpdateClients(entry->value.GetRaw());
           } else if (entry->info.name == "$serverpub") {
             m_server.UpdatePublishers(entry->value.GetRaw());
@@ -484,9 +509,7 @@ void NetworkTablesModel::Update() {
   }
 
   // remove deleted entries
-  m_sortedEntries.erase(
-      std::remove(m_sortedEntries.begin(), m_sortedEntries.end(), nullptr),
-      m_sortedEntries.end());
+  std::erase(m_sortedEntries, nullptr);
 
   RebuildTree();
 }
@@ -552,7 +575,7 @@ void NetworkTablesModel::RebuildTreeImpl(std::vector<TreeNode>* tree,
 }
 
 bool NetworkTablesModel::Exists() {
-  return m_inst.IsConnected();
+  return true;
 }
 
 NetworkTablesModel::Entry* NetworkTablesModel::GetEntry(std::string_view name) {
@@ -616,9 +639,9 @@ static void DecodeSubscriberOptions(
   for (uint32_t j = 0; j < numMapElem; ++j) {
     std::string key;
     mpack_expect_str(&r, &key);
-    if (key == "immediate") {
-      options->immediate = mpack_expect_bool(&r);
-    } else if (key == "sendAll") {
+    if (key == "topicsonly") {
+      options->topicsOnly = mpack_expect_bool(&r);
+    } else if (key == "all") {
       options->sendAll = mpack_expect_bool(&r);
     } else if (key == "periodic") {
       options->periodic = mpack_expect_float(&r);
@@ -828,54 +851,6 @@ static bool StringToFloatArray(std::string_view in, std::vector<T>* out) {
   return true;
 }
 
-static int fromxdigit(char ch) {
-  if (ch >= 'a' && ch <= 'f') {
-    return (ch - 'a' + 10);
-  } else if (ch >= 'A' && ch <= 'F') {
-    return (ch - 'A' + 10);
-  } else {
-    return ch - '0';
-  }
-}
-
-static std::string_view UnescapeString(std::string_view source,
-                                       wpi::SmallVectorImpl<char>& buf) {
-  assert(source.size() >= 2 && source.front() == '"' && source.back() == '"');
-  buf.clear();
-  buf.reserve(source.size() - 2);
-  for (auto s = source.begin() + 1, end = source.end() - 1; s != end; ++s) {
-    if (*s != '\\') {
-      buf.push_back(*s);
-      continue;
-    }
-    switch (*++s) {
-      case 't':
-        buf.push_back('\t');
-        break;
-      case 'n':
-        buf.push_back('\n');
-        break;
-      case 'x': {
-        if (!isxdigit(*(s + 1))) {
-          buf.push_back('x');  // treat it like a unknown escape
-          break;
-        }
-        int ch = fromxdigit(*++s);
-        if (std::isxdigit(*(s + 1))) {
-          ch <<= 4;
-          ch |= fromxdigit(*++s);
-        }
-        buf.push_back(static_cast<char>(ch));
-        break;
-      }
-      default:
-        buf.push_back(*s);
-        break;
-    }
-  }
-  return {buf.data(), buf.size()};
-}
-
 static bool StringToStringArray(std::string_view in,
                                 std::vector<std::string>* out) {
   in = wpi::trim(in);
@@ -904,7 +879,9 @@ static bool StringToStringArray(std::string_view in,
                  "GUI: NetworkTables: Could not understand value '{}'\n", val);
       return false;
     }
-    out->emplace_back(UnescapeString(val, buf));
+    val.remove_prefix(1);
+    val.remove_suffix(1);
+    out->emplace_back(wpi::UnescapeCString(val, buf).first);
   }
 
   return true;
@@ -1454,7 +1431,7 @@ static void DisplayClient(const NetworkTablesModel::Client& client) {
     ImGui::TableSetupColumn("Topics", ImGuiTableColumnFlags_WidthStretch, 6.0f);
     ImGui::TableSetupColumn("Periodic", ImGuiTableColumnFlags_WidthStretch,
                             1.0f);
-    ImGui::TableSetupColumn("Immediate", ImGuiTableColumnFlags_WidthStretch,
+    ImGui::TableSetupColumn("Topics Only", ImGuiTableColumnFlags_WidthStretch,
                             1.0f);
     ImGui::TableSetupColumn("Send All", ImGuiTableColumnFlags_WidthStretch,
                             1.0f);
@@ -1470,7 +1447,7 @@ static void DisplayClient(const NetworkTablesModel::Client& client) {
       ImGui::TableNextColumn();
       ImGui::Text("%0.3f", sub.options.periodic);
       ImGui::TableNextColumn();
-      ImGui::Text(sub.options.immediate ? "Yes" : "No");
+      ImGui::Text(sub.options.topicsOnly ? "Yes" : "No");
       ImGui::TableNextColumn();
       ImGui::Text(sub.options.sendAll ? "Yes" : "No");
       ImGui::TableNextColumn();

glass/src/libnt/native/cpp/StandardNetworkTables.cpp

@@ -12,8 +12,8 @@
 #include "glass/networktables/NTGyro.h"
 #include "glass/networktables/NTMecanumDrive.h"
 #include "glass/networktables/NTMechanism2D.h"
+#include "glass/networktables/NTMotorController.h"
 #include "glass/networktables/NTPIDController.h"
-#include "glass/networktables/NTSpeedController.h"
 #include "glass/networktables/NTStringChooser.h"
 #include "glass/networktables/NTSubsystem.h"
 #include "glass/networktables/NetworkTablesProvider.h"
@@ -142,14 +142,14 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) {
         });
       });
   provider.Register(
-      NTSpeedControllerModel::kType,
+      NTMotorControllerModel::kType,
       [](nt::NetworkTableInstance inst, const char* path) {
-        return std::make_unique<NTSpeedControllerModel>(inst, path);
+        return std::make_unique<NTMotorControllerModel>(inst, path);
       },
       [](Window* win, Model* model, const char* path) {
         win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
         return MakeFunctionView([=] {
-          DisplaySpeedController(static_cast<NTSpeedControllerModel*>(model));
+          DisplayMotorController(static_cast<NTMotorControllerModel*>(model));
         });
       });
   provider.Register(

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

@@ -47,7 +47,7 @@ class NTFMSModel : public FMSModel {
   void SetEnabled(bool val) override {}
   void SetTest(bool val) override {}
   void SetAutonomous(bool val) override {}
-  void SetGameSpecificMessage(const char* val) override {}
+  void SetGameSpecificMessage(std::string_view val) override {}
 
   void Update() override;
   bool Exists() override;

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

@@ -13,15 +13,15 @@
 #include <networktables/StringTopic.h>
 
 #include "glass/DataSource.h"
-#include "glass/hardware/SpeedController.h"
+#include "glass/hardware/MotorController.h"
 
 namespace glass {
-class NTSpeedControllerModel : public SpeedControllerModel {
+class NTMotorControllerModel : public MotorControllerModel {
  public:
   static constexpr const char* kType = "Motor Controller";
 
-  explicit NTSpeedControllerModel(std::string_view path);
-  NTSpeedControllerModel(nt::NetworkTableInstance inst, std::string_view path);
+  explicit NTMotorControllerModel(std::string_view path);
+  NTMotorControllerModel(nt::NetworkTableInstance inst, std::string_view path);
 
   const char* GetName() const override { return m_nameValue.c_str(); }
   const char* GetSimDevice() const override { return nullptr; }

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

@@ -30,7 +30,7 @@ class NetworkTablesModel : public Model {
  public:
   struct SubscriberOptions {
     float periodic = 0.1f;
-    bool immediate = false;
+    bool topicsOnly = false;
     bool sendAll = false;
     bool prefixMatch = false;
     // std::string otherStr;

googletest/CMakeLists.txt

@@ -1,27 +1,21 @@
-# Download and unpack googletest at configure time
-configure_file(CMakeLists.txt.in googletest-download/CMakeLists.txt)
-execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" .
-  RESULT_VARIABLE result
-  WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download )
-if(result)
-  message(FATAL_ERROR "CMake step for googletest failed: ${result}")
-endif()
-execute_process(COMMAND ${CMAKE_COMMAND} --build .
-  RESULT_VARIABLE result
-  WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download )
-if(result)
-  message(FATAL_ERROR "Build step for googletest failed: ${result}")
-endif()
+include(FetchContent)
 
-# Prevent overriding the parent project's compiler/linker
-# settings on Windows
-set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+FetchContent_Declare(
+    googletest
+    GIT_REPOSITORY  https://github.com/google/googletest.git
+    GIT_TAG         58d77fa8070e8cec2dc1ed015d66b454c8d78850 # 1.12.1
+)
 
-# Add googletest directly to our build. This defines
-# the gtest and gtest_main targets.
-add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/googletest-src
-                 ${CMAKE_CURRENT_BINARY_DIR}/googletest-build
-                 EXCLUDE_FROM_ALL)
+FetchContent_GetProperties(googletest)
+if(NOT googletest_POPULATED)
+    FetchContent_Populate(googletest)
+
+    # Prevent overriding the parent project's compiler/linker
+    # settings on Windows
+    set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+
+    add_subdirectory(${googletest_SOURCE_DIR} ${googletest_BINARY_DIR} EXCLUDE_FROM_ALL)
+endif()
 
 target_compile_features(gtest PUBLIC cxx_std_20)
 target_compile_features(gtest_main PUBLIC cxx_std_20)

googletest/CMakeLists.txt.in

@@ -1,15 +0,0 @@
-cmake_minimum_required(VERSION 3.3.0)
-
-project(googletest-download NONE)
-
-include(ExternalProject)
-ExternalProject_Add(googletest
-  GIT_REPOSITORY    https://github.com/google/googletest.git
-  GIT_TAG           e2239ee6043f73722e7aa812a459f54a28552929 # 1.11.0
-  SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/googletest-src"
-  BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/googletest-build"
-  CONFIGURE_COMMAND ""
-  BUILD_COMMAND     ""
-  INSTALL_COMMAND   ""
-  TEST_COMMAND      ""
-)

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

@@ -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.
+
+package edu.wpi.first.hal;
+
+public class CANStreamMessage {
+  @SuppressWarnings("MemberName")
+  public final byte[] data = new byte[8];
+
+  @SuppressWarnings("MemberName")
+  public int length;
+
+  @SuppressWarnings("MemberName")
+  public long timestamp;
+
+  @SuppressWarnings("MemberName")
+  public int messageID;
+
+  /**
+   * API used from JNI to set the data.
+   *
+   * @param length Length of packet in bytes.
+   * @param messageID CAN message ID of the message.
+   * @param timestamp CAN frame timestamp in microseconds.
+   * @return Buffer containing CAN frame.
+   */
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
+  public byte[] setStreamData(int length, int messageID, long timestamp) {
+    this.messageID = messageID;
+    this.length = length;
+    this.timestamp = timestamp;
+    return data;
+  }
+}

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

@@ -39,5 +39,7 @@ public class DIOJNI extends JNIWrapper {
 
   public static native void setDigitalPWMDutyCycle(int pwmGenerator, double dutyCycle);
 
+  public static native void setDigitalPWMPPS(int pwmGenerator, double dutyCycle);
+
   public static native void setDigitalPWMOutputChannel(int pwmGenerator, int channel);
 }

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

@@ -22,6 +22,8 @@ public final class HALUtil extends JNIWrapper {
 
   public static native int getFPGARevision();
 
+  public static native String getSerialNumber();
+
   public static native long getFPGATime();
 
   public static native int getHALRuntimeType();

hal/src/main/java/edu/wpi/first/hal/can/CANJNI.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.hal.can;
 
+import edu.wpi.first.hal.CANStreamMessage;
 import edu.wpi.first.hal.JNIWrapper;
 import java.nio.ByteBuffer;
 import java.nio.IntBuffer;
@@ -24,4 +25,11 @@ public class CANJNI extends JNIWrapper {
       IntBuffer messageID, int messageIDMask, ByteBuffer timeStamp);
 
   public static native void getCANStatus(CANStatus status);
+
+  public static native int openCANStreamSession(int messageID, int messageIDMask, int maxMessages);
+
+  public static native void closeCANStreamSession(int sessionHandle);
+
+  public static native int readCANStreamSession(
+      int sessionHandle, CANStreamMessage[] messages, int messagesToRead);
 }

hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java

@@ -151,5 +151,9 @@ public class RoboRioDataJNI extends JNIWrapper {
 
   public static native void setBrownoutVoltage(double brownoutVoltage);
 
+  public static native String getSerialNumber();
+
+  public static native void setSerialNumber(String serialNumber);
+
   public static native void resetData();
 }

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

@@ -240,6 +240,30 @@ void HAL_SetDigitalPWMDutyCycle(HAL_DigitalPWMHandle pwmGenerator,
   }
 }
 
+void HAL_SetDigitalPWMPPS(HAL_DigitalPWMHandle pwmGenerator, double dutyCycle,
+                          int32_t* status) {
+  auto port = digitalPWMHandles->Get(pwmGenerator);
+  if (port == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  int32_t id = *port;
+  digitalSystem->writePWMPeriodPower(0xffff, status);
+  double rawDutyCycle = 31.0 * dutyCycle;
+  if (rawDutyCycle > 30.5) {
+    rawDutyCycle = 30.5;
+  }
+  {
+    std::scoped_lock lock(digitalPwmMutex);
+    if (id < 4)
+      digitalSystem->writePWMDutyCycleA(id, static_cast<uint8_t>(rawDutyCycle),
+                                        status);
+    else
+      digitalSystem->writePWMDutyCycleB(
+          id - 4, static_cast<uint8_t>(rawDutyCycle), status);
+  }
+}
+
 void HAL_SetDigitalPWMOutputChannel(HAL_DigitalPWMHandle pwmGenerator,
                                     int32_t channel, int32_t* status) {
   auto port = digitalPWMHandles->Get(pwmGenerator);

hal/src/main/native/athena/DigitalInternal.h

@@ -38,7 +38,7 @@ constexpr int32_t kExpectedLoopTiming = 40;
  *   reliably down to 10.0 ms; starting at about 8.5ms, the servo sometimes hums
  *   and get hot; by 5.0ms the hum is nearly continuous
  * - 10ms periods work well for Victor 884
- * - 5ms periods allows higher update rates for Luminary Micro Jaguar speed
+ * - 5ms periods allows higher update rates for Luminary Micro Jaguar motor
  *   controllers. Due to the shipping firmware on the Jaguar, we can't run the
  *   update period less than 5.05 ms.
  *

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

@@ -111,24 +111,12 @@ int32_t HAL_GetDutyCycleHighTime(HAL_DutyCycleHandle dutyCycleHandle,
 
   // TODO Handle Overflow
   unsigned char overflow = 0;
-  uint32_t freq0 = dutyCycle->dutyCycle->readFrequency(&overflow, status);
-  uint32_t output = dutyCycle->dutyCycle->readOutput(&overflow, status);
-  uint32_t freq1 = dutyCycle->dutyCycle->readFrequency(&overflow, status);
+  uint32_t highTime = dutyCycle->dutyCycle->readHighTicks(&overflow, status);
   if (*status != 0) {
     return 0;
   }
-  if (freq0 != freq1) {
-    // Frequency rolled over. Reread output
-    output = dutyCycle->dutyCycle->readOutput(&overflow, status);
-    if (*status != 0) {
-      return 0;
-    }
-  }
-  if (freq1 == 0) {
-    return 0;
-  }
   // Output will be at max 4e7, so x25 will still fit in a 32 bit signed int.
-  return (output / freq1) * 25;
+  return highTime * 25;
 }
 
 int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,

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"
 
@@ -42,7 +43,6 @@ struct JoystickDataCache {
   HAL_JoystickButtons buttons[HAL_kMaxJoysticks];
   HAL_AllianceStationID allianceStation;
   float matchTime;
-  bool updated;
 };
 static_assert(std::is_standard_layout_v<JoystickDataCache>);
 // static_assert(std::is_trivial_v<JoystickDataCache>);
@@ -113,7 +113,9 @@ void JoystickDataCache::Update() {
 static HAL_ControlWord newestControlWord;
 static JoystickDataCache caches[3];
 static JoystickDataCache* currentRead = &caches[0];
-static JoystickDataCache* currentCache = &caches[1];
+static JoystickDataCache* currentReadLocal = &caches[0];
+static std::atomic<JoystickDataCache*> currentCache{&caches[1]};
+static JoystickDataCache* lastGiven = &caches[1];
 static JoystickDataCache* cacheToUpdate = &caches[2];
 
 static wpi::mutex cacheMutex;
@@ -170,6 +172,38 @@ static int32_t HAL_GetMatchInfoInternal(HAL_MatchInfo* info) {
   return status;
 }
 
+namespace {
+struct TcpCache {
+  TcpCache() { std::memset(this, 0, sizeof(*this)); }
+  void Update(uint32_t mask);
+  void CloneTo(TcpCache* other) { std::memcpy(other, this, sizeof(*this)); }
+
+  HAL_MatchInfo matchInfo;
+  HAL_JoystickDescriptor descriptors[HAL_kMaxJoysticks];
+};
+static_assert(std::is_standard_layout_v<TcpCache>);
+}  // namespace
+
+static std::atomic_uint32_t tcpMask{0xFFFFFFFF};
+static TcpCache tcpCache;
+static TcpCache tcpCurrent;
+static wpi::mutex tcpCacheMutex;
+
+constexpr uint32_t combinedMatchInfoMask = kTcpRecvMask_MatchInfoOld |
+                                           kTcpRecvMask_MatchInfo |
+                                           kTcpRecvMask_GameSpecific;
+
+void TcpCache::Update(uint32_t mask) {
+  if ((mask & combinedMatchInfoMask) != 0) {
+    HAL_GetMatchInfoInternal(&matchInfo);
+  }
+  for (int i = 0; i < HAL_kMaxJoysticks; i++) {
+    if ((mask & (1 << i)) != 0) {
+      HAL_GetJoystickDescriptorInternal(i, &descriptors[i]);
+    }
+  }
+}
+
 namespace hal::init {
 void InitializeFRCDriverStation() {
   std::memset(&newestControlWord, 0, sizeof(newestControlWord));
@@ -178,6 +212,15 @@ void InitializeFRCDriverStation() {
 }
 }  // namespace hal::init
 
+namespace hal {
+static void DefaultPrintErrorImpl(const char* line, size_t size) {
+  std::fwrite(line, size, 1, stderr);
+}
+}  // namespace hal
+
+static std::atomic<void (*)(const char* line, size_t size)> gPrintErrorImpl{
+    hal::DefaultPrintErrorImpl};
+
 extern "C" {
 
 int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
@@ -255,7 +298,8 @@ int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
       if (callStack && callStack[0] != '\0') {
         fmt::format_to(fmt::appender{buf}, "{}\n", callStack);
       }
-      std::fwrite(buf.data(), buf.size(), 1, stderr);
+      auto printError = gPrintErrorImpl.load();
+      printError(buf.data(), buf.size());
     }
     if (i == KEEP_MSGS) {
       // replace the oldest one
@@ -274,6 +318,10 @@ int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode,
   return retval;
 }
 
+void HAL_SetPrintErrorImpl(void (*func)(const char* line, size_t size)) {
+  gPrintErrorImpl.store(func ? func : hal::DefaultPrintErrorImpl);
+}
+
 int32_t HAL_SendConsoleLine(const char* line) {
   std::string_view lineRef{line};
   if (lineRef.size() <= 65535) {
@@ -324,11 +372,16 @@ void HAL_GetAllJoystickData(HAL_JoystickAxes* axes, HAL_JoystickPOVs* povs,
 
 int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
                                   HAL_JoystickDescriptor* desc) {
-  return HAL_GetJoystickDescriptorInternal(joystickNum, desc);
+  CHECK_JOYSTICK_NUMBER(joystickNum);
+  std::scoped_lock lock{tcpCacheMutex};
+  *desc = tcpCurrent.descriptors[joystickNum];
+  return 0;
 }
 
 int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) {
-  return HAL_GetMatchInfoInternal(info);
+  std::scoped_lock lock{tcpCacheMutex};
+  *info = tcpCurrent.matchInfo;
+  return 0;
 }
 
 HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
@@ -373,7 +426,6 @@ void HAL_FreeJoystickName(char* name) {
 }
 
 int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) {
-  CHECK_JOYSTICK_NUMBER(joystickNum);
   HAL_JoystickDescriptor joystickDesc;
   if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
     return -1;
@@ -416,20 +468,44 @@ void HAL_ObserveUserProgramTest(void) {
 
 // Constant number to be used for our occur handle
 constexpr int32_t refNumber = 42;
+constexpr int32_t tcpRefNumber = 94;
+
+static void tcpOccur(void) {
+  uint32_t mask = FRC_NetworkCommunication_getNewTcpRecvMask();
+  tcpMask.fetch_or(mask);
+}
+
+static void udpOccur(void) {
+  cacheToUpdate->Update();
+
+  JoystickDataCache* given = cacheToUpdate;
+  JoystickDataCache* prev = currentCache.exchange(cacheToUpdate);
+  if (prev == nullptr) {
+    cacheToUpdate = currentReadLocal;
+    currentReadLocal = lastGiven;
+  } else {
+    // Current read local does not update
+    cacheToUpdate = prev;
+  }
+  lastGiven = given;
+
+  driverStation->newDataEvents.Wakeup();
+}
 
 static void newDataOccur(uint32_t refNum) {
-  // Since we could get other values, require our specific handle
-  // to signal our threads
-  if (refNum != refNumber) {
-    return;
+  switch (refNum) {
+    case refNumber:
+      udpOccur();
+      break;
+
+    case tcpRefNumber:
+      tcpOccur();
+      break;
+
+    default:
+      std::printf("Unknown occur %u\n", refNum);
+      break;
   }
-  cacheToUpdate->Update();
-  {
-    std::scoped_lock lock{cacheMutex};
-    std::swap(currentCache, cacheToUpdate);
-    currentCache->updated = true;
-  }
-  driverStation->newDataEvents.Wakeup();
 }
 
 void HAL_RefreshDSData(void) {
@@ -438,14 +514,22 @@ void HAL_RefreshDSData(void) {
   FRC_NetworkCommunication_getControlWord(
       reinterpret_cast<ControlWord_t*>(&controlWord));
   std::scoped_lock lock{cacheMutex};
-  if (currentCache->updated) {
-    std::swap(currentCache, currentRead);
-    currentCache->updated = false;
+  JoystickDataCache* prev = currentCache.exchange(nullptr);
+  if (prev != nullptr) {
+    currentRead = prev;
   }
   newestControlWord = controlWord;
+
+  uint32_t mask = tcpMask.exchange(0);
+  if (mask != 0) {
+    tcpCache.Update(mask);
+    std::scoped_lock tcpLock(tcpCacheMutex);
+    tcpCache.CloneTo(&tcpCurrent);
+  }
 }
 
 void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) {
+  hal::init::CheckInit();
   driverStation->newDataEvents.Add(handle);
 }
 
@@ -465,5 +549,6 @@ void InitializeDriverStation() {
   NetCommRPCProxy_SetOccurFuncPointer(newDataOccur);
   // Set up our occur reference number
   setNewDataOccurRef(refNumber);
+  FRC_NetworkCommunication_setNewTcpDataOccurRef(tcpRefNumber);
 }
 }  // namespace hal

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

@@ -18,6 +18,9 @@
 #include <FRC_NetworkCommunication/LoadOut.h>
 #include <FRC_NetworkCommunication/UsageReporting.h>
 #include <fmt/format.h>
+#include <wpi/MemoryBuffer.h>
+#include <wpi/StringExtras.h>
+#include <wpi/fs.h>
 #include <wpi/mutex.h>
 #include <wpi/timestamp.h>
 
@@ -82,6 +85,7 @@ void InitializeHAL() {
 }  // namespace init
 
 void ReleaseFPGAInterrupt(int32_t interruptNumber) {
+  hal::init::CheckInit();
   if (!global) {
     return;
   }
@@ -252,6 +256,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 +265,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;
@@ -267,7 +273,22 @@ int64_t HAL_GetFPGARevision(int32_t* status) {
   return global->readRevision(status);
 }
 
+size_t HAL_GetSerialNumber(char* buffer, size_t size) {
+  const char* serialNum = std::getenv("serialnum");
+  if (serialNum) {
+    std::strncpy(buffer, serialNum, size);
+    buffer[size - 1] = '\0';
+    return std::strlen(buffer);
+  } else {
+    if (size > 0) {
+      buffer[0] = '\0';
+    }
+    return 0;
+  }
+}
+
 uint64_t HAL_GetFPGATime(int32_t* status) {
+  hal::init::CheckInit();
   if (!global) {
     *status = NiFpga_Status_ResourceNotInitialized;
     return 0;
@@ -314,6 +335,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 +344,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 +353,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/athena/mockdata/DriverStationData.cpp

@@ -103,13 +103,13 @@ void HALSIM_SetJoystickIsXbox(int32_t stick, HAL_Bool isXbox) {}
 
 void HALSIM_SetJoystickType(int32_t stick, int32_t type) {}
 
-void HALSIM_SetJoystickName(int32_t stick, const char* name) {}
+void HALSIM_SetJoystickName(int32_t stick, const char* name, size_t size) {}
 
 void HALSIM_SetJoystickAxisType(int32_t stick, int32_t axis, int32_t type) {}
 
-void HALSIM_SetGameSpecificMessage(const char* message) {}
+void HALSIM_SetGameSpecificMessage(const char* message, size_t size) {}
 
-void HALSIM_SetEventName(const char* name) {}
+void HALSIM_SetEventName(const char* name, size_t size) {}
 
 void HALSIM_SetMatchType(HAL_MatchType type) {}
 

hal/src/main/native/athena/mockdata/RoboRioData.cpp

@@ -29,6 +29,19 @@ DEFINE_CAPI(int32_t, UserFaults5V, 0)
 DEFINE_CAPI(int32_t, UserFaults3V3, 0)
 DEFINE_CAPI(double, BrownoutVoltage, 6.75)
 
+int32_t HALSIM_RegisterRoboRioSerialNumberCallback(
+    HAL_RoboRioStringCallback callback, void* param, HAL_Bool initialNotify) {
+  return 0;
+}
+void HALSIM_CancelRoboRioSerialNumberCallback(int32_t uid) {}
+size_t HALSIM_GetRoboRioSerialNumber(char* buffer, size_t size) {
+  if (size > 0) {
+    buffer[0] = '\0';
+  }
+  return 0;
+}
+void HALSIM_SetRoboRioSerialNumber(const char* buffer, size_t size) {}
+
 void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
                                         void* param, HAL_Bool initialNotify) {}
 }  // extern "C"