[sysid] Fix wrong position Kd with unnormalized time (#6433)

Fix typo.

.clang-format

@@ -3,110 +3,171 @@ Language:        Cpp
 BasedOnStyle:  Google
 AccessModifierOffset: -1
 AlignAfterOpenBracket: Align
-AlignConsecutiveMacros: false
-AlignConsecutiveAssignments: false
-AlignConsecutiveDeclarations: false
+AlignArrayOfStructures: None
+AlignConsecutiveAssignments:
+  Enabled:         false
+  AcrossEmptyLines: false
+  AcrossComments:  false
+  AlignCompound:   false
+  PadOperators:    true
+AlignConsecutiveBitFields:
+  Enabled:         false
+  AcrossEmptyLines: false
+  AcrossComments:  false
+  AlignCompound:   false
+  PadOperators:    false
+AlignConsecutiveDeclarations:
+  Enabled:         false
+  AcrossEmptyLines: false
+  AcrossComments:  false
+  AlignCompound:   false
+  PadOperators:    false
+AlignConsecutiveMacros:
+  Enabled:         false
+  AcrossEmptyLines: false
+  AcrossComments:  false
+  AlignCompound:   false
+  PadOperators:    false
 AlignEscapedNewlines: Left
-AlignOperands:   true
-AlignTrailingComments: true
+AlignOperands:   Align
+AlignTrailingComments:
+  Kind:            Always
+  OverEmptyLines:  0
 AllowAllArgumentsOnNextLine: true
-AllowAllConstructorInitializersOnNextLine: true
 AllowAllParametersOfDeclarationOnNextLine: true
 AllowShortBlocksOnASingleLine: Never
 AllowShortCaseLabelsOnASingleLine: false
+AllowShortEnumsOnASingleLine: true
 AllowShortFunctionsOnASingleLine: Inline
-AllowShortLambdasOnASingleLine: All
 AllowShortIfStatementsOnASingleLine: Never
+AllowShortLambdasOnASingleLine: All
 AllowShortLoopsOnASingleLine: false
 AlwaysBreakAfterDefinitionReturnType: None
 AlwaysBreakAfterReturnType: None
 AlwaysBreakBeforeMultilineStrings: true
 AlwaysBreakTemplateDeclarations: Yes
+AttributeMacros:
+  - __capability
 BinPackArguments: true
 BinPackParameters: true
+BitFieldColonSpacing: Both
 BraceWrapping:
   AfterCaseLabel:  false
   AfterClass:      false
-  AfterControlStatement: false
+  AfterControlStatement: Never
   AfterEnum:       false
+  AfterExternBlock: false
   AfterFunction:   false
   AfterNamespace:  false
   AfterObjCDeclaration: false
   AfterStruct:     false
   AfterUnion:      false
-  AfterExternBlock: false
   BeforeCatch:     false
   BeforeElse:      false
+  BeforeLambdaBody: false
+  BeforeWhile:     false
   IndentBraces:    false
   SplitEmptyFunction: true
   SplitEmptyRecord: true
   SplitEmptyNamespace: true
-BreakBeforeBinaryOperators: None
-BreakBeforeBraces: Attach
-BreakBeforeInheritanceComma: false
-BreakInheritanceList: BeforeColon
-BreakBeforeTernaryOperators: true
-BreakConstructorInitializersBeforeComma: false
-BreakConstructorInitializers: BeforeColon
+BreakAfterAttributes: Always
 BreakAfterJavaFieldAnnotations: false
+BreakArrays:     true
+BreakBeforeBinaryOperators: None
+BreakBeforeConceptDeclarations: Always
+BreakBeforeBraces: Attach
+BreakBeforeInlineASMColon: OnlyMultiline
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializers: BeforeColon
+BreakInheritanceList: BeforeColon
 BreakStringLiterals: true
 ColumnLimit:     80
 CommentPragmas:  '^ IWYU pragma:'
 CompactNamespaces: false
-ConstructorInitializerAllOnOneLineOrOnePerLine: true
 ConstructorInitializerIndentWidth: 4
 ContinuationIndentWidth: 4
 Cpp11BracedListStyle: true
-DeriveLineEnding: true
 DerivePointerAlignment: false
 DisableFormat:   false
+EmptyLineAfterAccessModifier: Never
+EmptyLineBeforeAccessModifier: LogicalBlock
 ExperimentalAutoDetectBinPacking: false
 FixNamespaceComments: true
 ForEachMacros:
   - foreach
   - Q_FOREACH
   - BOOST_FOREACH
+IfMacros:
+  - KJ_IF_MAYBE
 IncludeBlocks:   Regroup
 IncludeCategories:
   - Regex:           '^<ext/.*\.h>'
     Priority:        2
     SortPriority:    0
+    CaseSensitive:   false
   - Regex:           '^<.*\.h>'
     Priority:        1
     SortPriority:    0
+    CaseSensitive:   false
   - Regex:           '^<.*'
     Priority:        2
     SortPriority:    0
+    CaseSensitive:   false
   - Regex:           '.*'
     Priority:        3
     SortPriority:    0
+    CaseSensitive:   false
 IncludeIsMainRegex: '([-_](test|unittest))?$'
 IncludeIsMainSourceRegex: ''
+IndentAccessModifiers: false
+IndentCaseBlocks: false
 IndentCaseLabels: true
+IndentExternBlock: AfterExternBlock
 IndentGotoLabels: true
 IndentPPDirectives: None
+IndentRequiresClause: true
 IndentWidth:     2
 IndentWrappedFunctionNames: false
+InsertBraces:    false
+InsertNewlineAtEOF: false
+InsertTrailingCommas: None
+IntegerLiteralSeparator:
+  Binary:          0
+  BinaryMinDigits: 0
+  Decimal:         0
+  DecimalMinDigits: 0
+  Hex:             0
+  HexMinDigits:    0
 JavaScriptQuotes: Leave
 JavaScriptWrapImports: true
 KeepEmptyLinesAtTheStartOfBlocks: false
+LambdaBodyIndentation: Signature
+LineEnding:      DeriveLF
 MacroBlockBegin: ''
 MacroBlockEnd:   ''
+Macros:
+  - 'HAL_ENUM(name)=enum name'
 MaxEmptyLinesToKeep: 1
 NamespaceIndentation: None
 ObjCBinPackProtocolList: Never
 ObjCBlockIndentWidth: 2
+ObjCBreakBeforeNestedBlockParam: true
 ObjCSpaceAfterProperty: false
 ObjCSpaceBeforeProtocolList: true
+PackConstructorInitializers: NextLine
 PenaltyBreakAssignment: 2
 PenaltyBreakBeforeFirstCallParameter: 1
 PenaltyBreakComment: 300
 PenaltyBreakFirstLessLess: 120
+PenaltyBreakOpenParenthesis: 0
 PenaltyBreakString: 1000
 PenaltyBreakTemplateDeclaration: 10
 PenaltyExcessCharacter: 1000000
+PenaltyIndentedWhitespace: 0
 PenaltyReturnTypeOnItsOwnLine: 200
 PointerAlignment: Left
+PPIndentWidth:   -1
+QualifierAlignment: Leave
 RawStringFormats:
   - Language:        Cpp
     Delimiters:
@@ -133,34 +194,65 @@ RawStringFormats:
       - PARSE_TEXT_PROTO
       - ParseTextOrDie
       - ParseTextProtoOrDie
-    CanonicalDelimiter: ''
+      - ParseTestProto
+      - ParsePartialTestProto
+    CanonicalDelimiter: pb
     BasedOnStyle:    google
+ReferenceAlignment: Pointer
 ReflowComments:  true
+RemoveBracesLLVM: false
+RemoveSemicolon: false
+RequiresClausePosition: OwnLine
+RequiresExpressionIndentation: OuterScope
+SeparateDefinitionBlocks: Leave
+ShortNamespaceLines: 1
 SortIncludes:    false
-SortUsingDeclarations: true
+SortJavaStaticImport: Before
+SortUsingDeclarations: LexicographicNumeric
 SpaceAfterCStyleCast: false
 SpaceAfterLogicalNot: false
 SpaceAfterTemplateKeyword: true
+SpaceAroundPointerQualifiers: Default
 SpaceBeforeAssignmentOperators: true
+SpaceBeforeCaseColon: false
 SpaceBeforeCpp11BracedList: false
 SpaceBeforeCtorInitializerColon: true
 SpaceBeforeInheritanceColon: true
 SpaceBeforeParens: ControlStatements
+SpaceBeforeParensOptions:
+  AfterControlStatements: true
+  AfterForeachMacros: true
+  AfterFunctionDefinitionName: false
+  AfterFunctionDeclarationName: false
+  AfterIfMacros:   true
+  AfterOverloadedOperator: false
+  AfterRequiresInClause: false
+  AfterRequiresInExpression: false
+  BeforeNonEmptyParentheses: false
 SpaceBeforeRangeBasedForLoopColon: true
+SpaceBeforeSquareBrackets: false
 SpaceInEmptyBlock: false
 SpaceInEmptyParentheses: false
 SpacesBeforeTrailingComments: 2
-SpacesInAngles:  false
+SpacesInAngles:  Never
 SpacesInConditionalStatement: false
 SpacesInContainerLiterals: true
 SpacesInCStyleCastParentheses: false
+SpacesInLineCommentPrefix:
+  Minimum:         1
+  Maximum:         -1
 SpacesInParentheses: false
 SpacesInSquareBrackets: false
-SpaceBeforeSquareBrackets: false
-Standard:        c++17
+Standard:        c++20
 StatementMacros:
   - Q_UNUSED
   - QT_REQUIRE_VERSION
 TabWidth:        8
 UseTab:          Never
+WhitespaceSensitiveMacros:
+  - BOOST_PP_STRINGIZE
+  - CF_SWIFT_NAME
+  - NS_SWIFT_NAME
+  - PP_STRINGIZE
+  - STRINGIZE
 ...

.clang-tidy

@@ -35,7 +35,6 @@ Checks:
   bugprone-unhandled-self-assignment,
   bugprone-unused-raii,
   bugprone-virtual-near-miss,
-  cert-dcl58-cpp,
   cert-err52-cpp,
   cert-err60-cpp,
   cert-mem57-cpp,
@@ -50,10 +49,8 @@ Checks:
   google-build-namespaces,
   google-explicit-constructor,
   google-global-names-in-headers,
-  google-readability-avoid-underscore-in-googletest-name,
   google-readability-casting,
   google-runtime-operator,
-  llvm-twine-local,
   misc-definitions-in-headers,
   misc-misplaced-const,
   misc-new-delete-overloads,
@@ -70,6 +67,3 @@ Checks:
   modernize-use-using,
   readability-braces-around-statements'
 FormatStyle: file
-CheckOptions:
-  - key: bugprone-dangling-handle
-    value: 'wpi::StringRef;wpi::Twine'

.gersemirc

@@ -0,0 +1,6 @@
+color: false
+definitions: []
+line_length: 100
+list_expansion: favour-inlining
+quiet: false
+unsafe: false

.gitattributes

@@ -1,4 +1,13 @@
+*.cpp text eol=lf
 *.gradle text eol=lf
+*.h text eol=lf
+*.inc text eol=lf
 *.java text eol=lf
+*.json text eol=lf
 *.md text eol=lf
 *.xml text eol=lf
+
+# Generated files
+hal/src/generated/** linguist-generated
+ntcore/src/generated/** linguist-generated
+wpimath/src/generated/** linguist-generated

.github/CODEOWNERS

@@ -29,8 +29,6 @@
 
 /wpilibNewCommands/  @wpilibsuite/commandbased
 
-/wpilibOldCommands/  @wpilibsuite/commandbased
-
 /wpilibcExamples/  @wpilibsuite/wpilib @wpilibsuite/documentation
 
 /wpilibjExamples/  @wpilibsuite/wpilib @wpilibsuite/documentation

.github/ISSUE_TEMPLATE/bug_report.md

@@ -15,6 +15,8 @@ Steps to reproduce the behavior:
 1. ...
 2. ...
 
+ - Link to code:
+
 **Expected behavior**
 A clear and concise description of what you expected to happen.
 
@@ -22,10 +24,8 @@ A clear and concise description of what you expected to happen.
 If applicable, add screenshots to help explain your problem.
 
 **Desktop (please complete the following information):**
- - WPILib Version: [e.g. 2021.3.1]
  - OS: [e.g. Windows 11]
- - Java version [e.g. 1.10.2]
- - C++ version [e.g. 17]
+ - Project Information: [In Visual Studio Code, press the WPILib button and choose WPILib: Open Project Information. Press the copy button and paste the data here. If not using VS Code, please include WPILib version, Gradle version, Java version, C++ version (if applicable), and any third party libraries and versions]
 
 **Additional context**
 Add any other context about the problem here.

.github/workflows/cmake.yml

@@ -2,64 +2,106 @@ name: CMake
 
 on: [pull_request, push]
 
+concurrency:
+  group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
+  cancel-in-progress: true
+
 jobs:
   build:
     strategy:
       fail-fast: false
       matrix:
         include:
-          - os: ubuntu-latest
+          - os: ubuntu-22.04
             name: Linux
-            container: wpilib/roborio-cross-ubuntu:2022-20.04
-            flags: ""
-          - os: macOS-11
+            container: wpilib/roborio-cross-ubuntu:2024-22.04
+            flags: "-DCMAKE_BUILD_TYPE=Release -DWITH_EXAMPLES=ON"
+          - os: macOS-12
             name: macOS
             container: ""
-            flags: "-DWITH_JAVA=OFF"
+            env: "PATH=\"/usr/local/opt/protobuf@3/bin:$PATH\""
+            flags: "-DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON -DCMAKE_LIBRARY_PATH=/usr/local/opt/protobuf@3/lib -DProtobuf_INCLUDE_DIR=/usr/local/opt/protobuf@3/include -DProtobuf_PROTOC_EXECUTABLE=/usr/local/opt/protobuf@3/bin/protoc"
+
     name: "Build - ${{ matrix.name }}"
     runs-on: ${{ matrix.os }}
     container: ${{ matrix.container }}
     steps:
-      - uses: actions/checkout@v2
-      - name: Install Dependencies
-        run: |
-          if [ "$RUNNER_OS" == "macOS" ]; then
-            brew install opencv
-          fi
-      - name: Set up Python 3.8
-        uses: actions/setup-python@v2
+      - name: Install dependencies (Linux)
+        if: runner.os == 'Linux'
+        run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv4.5-java python-is-python3 libprotobuf-dev protobuf-compiler ninja-build
+
+      - name: Install QuickBuffers (Linux)
+        if: runner.os == 'Linux'
+        run: wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.3/protoc-gen-quickbuf_1.3.3_amd64.deb && sudo apt install ./protoc-gen-quickbuf_1.3.3_amd64.deb
+
+      - name: Install opencv (macOS)
+        run: brew install opencv protobuf@3 ninja
+        if: runner.os == 'macOS'
+
+      - name: Set up Python 3.8 (macOS)
+        if: runner.os == 'macOS'
+        uses: actions/setup-python@v5
         with:
           python-version: 3.8
+
+      - name: Run sccache-cache
+        uses: mozilla-actions/sccache-action@v0.0.3
+
       - name: Install jinja
         run: python -m pip install jinja2
+
+      - uses: actions/checkout@v4
+
       - name: configure
-        run: mkdir build && cd build && cmake ${{ matrix.flags }} ..
+        run: cmake -S . -B build -G "Ninja" -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache ${{ matrix.flags }}
+        env:
+          SCCACHE_GHA_ENABLED: "true"
+
       - name: build
         working-directory: build
-        run: cmake --build . -j$(nproc)
+        run: cmake --build . --parallel $(nproc)
+        env:
+          SCCACHE_GHA_ENABLED: "true"
+
       - name: test
         working-directory: build
         run: ctest --output-on-failure
 
-  build-vcpkg:
+  build-windows:
     name: "Build - Windows"
-    runs-on: windows-2019
+    runs-on: windows-2022
     steps:
-      - uses: actions/checkout@v2
-      - name: Prepare vcpkg
-        uses: lukka/run-vcpkg@v7
+      - uses: ilammy/msvc-dev-cmd@v1
+
+      - name: Install CMake
+        uses: lukka/get-cmake@v3.27.6
+
+      - name: Run sccache-cache
+        uses: mozilla-actions/sccache-action@v0.0.3
+
+      - name: Install jinja
+        run: python -m pip install jinja2
+
+      - uses: actions/checkout@v4
+
+      - name: Run vcpkg
+        uses: lukka/run-vcpkg@v11.1
         with:
-          vcpkgArguments: opencv
           vcpkgDirectory: ${{ runner.workspace }}/vcpkg
-          vcpkgTriplet: x64-windows
-          vcpkgGitCommitId: d781bd9ca77ac3dc2f13d88169021d48459c665f # HEAD on 2021-07-25
-      - name: Configure & Build
-        uses: lukka/run-cmake@v3
-        with:
-          buildDirectory: ${{ runner.workspace }}/build
-          cmakeAppendedArgs: -DWITH_JAVA=OFF
-          cmakeListsOrSettingsJson: CMakeListsTxtAdvanced
-          useVcpkgToolchainFile: true
-      - name: Run Tests
-        run: ctest -C "Debug" --output-on-failure
-        working-directory: ${{ runner.workspace }}/build
+          vcpkgGitCommitId: 37c3e63a1306562f7f59c4c3c8892ddd50fdf992 # HEAD on 2024-02-24
+
+      - name: configure
+        run: cmake -S . -B build -G "Ninja" -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_BUILD_TYPE=Release -DWITH_JAVA=OFF -DWITH_EXAMPLES=ON -DUSE_SYSTEM_FMTLIB=ON -DUSE_SYSTEM_LIBUV=ON -DUSE_SYSTEM_EIGEN=OFF -DCMAKE_TOOLCHAIN_FILE=${{ runner.workspace }}/vcpkg/scripts/buildsystems/vcpkg.cmake -DVCPKG_INSTALL_OPTIONS=--clean-after-build -DVCPKG_TARGET_TRIPLET=x64-windows-release -DVCPKG_HOST_TRIPLET=x64-windows-release
+        env:
+          SCCACHE_GHA_ENABLED: "true"
+
+      - name: build
+        working-directory: build
+        run: cmake --build . --parallel $(nproc)
+        env:
+          SCCACHE_GHA_ENABLED: "true"
+
+      - name: test
+        working-directory: build
+        # UnitTest_test segfaults on exit occasionally
+        run: ctest --output-on-failure -E 'UnitTest'

.github/workflows/comment-command.yml

@@ -0,0 +1,58 @@
+name: Comment Commands
+on:
+  issue_comment:
+    types: [ created ]
+
+jobs:
+  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@v7
+        with:
+          script: |
+            const {owner, repo} = context.issue
+            github.rest.reactions.createForIssueComment({
+              owner,
+              repo,
+              comment_id: context.payload.comment.id,
+              content: "rocket",
+            });
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+          token: ${{ secrets.COMMENT_COMMAND_PAT_TOKEN }}
+      - name: Fetch all history and metadata
+        run: |
+          git checkout -b pr
+          git branch -f main origin/main
+      - name: Checkout PR
+        run: |
+          gh pr checkout $NUMBER
+        env:
+          GITHUB_TOKEN: "${{ secrets.COMMENT_COMMAND_PAT_TOKEN }}"
+          NUMBER: ${{ github.event.issue.number }}
+      - name: Set up Python 3.8
+        uses: actions/setup-python@v5
+        with:
+          python-version: 3.8
+      - name: Setup Java
+        uses: actions/setup-java@v4
+        with:
+          distribution: 'zulu'
+          java-version: 17
+      - name: Install wpiformat
+        run: pip3 install wpiformat
+      - name: Run wpiformat
+        run: wpiformat
+      - 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 "Formatting fixes"
+          git push

.github/workflows/documentation.yml

@@ -2,59 +2,65 @@ name: Documentation
 
 on: [push, workflow_dispatch]
 
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true
+
 env:
   BASE_PATH: allwpilib/docs
 
 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:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v4
         with:
           fetch-depth: 0
           persist-credentials: false
-      - uses: actions/setup-java@v1
+      - uses: actions/setup-java@v4
         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
+          echo "BRANCH=development" >> $GITHUB_ENV
         if: github.ref == 'refs/heads/main'
       - name: Set environment variables (Tag)
         run: |
           echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
-          echo "TARGET_FOLDER=$BASE_PATH/beta" >> $GITHUB_ENV
+          echo "BRANCH=beta" >> $GITHUB_ENV
         if: startsWith(github.ref, 'refs/tags/v')
       - name: Set environment variables (Release)
         run: |
           echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
-          echo "TARGET_FOLDER=$BASE_PATH/release" >> $GITHUB_ENV
+          echo "BRANCH=release" >> $GITHUB_ENV
         if: startsWith(github.ref, 'refs/tags/v') && !contains(github.ref, 'alpha') && !contains(github.ref, 'beta')
       - name: Build with Gradle
         run: ./gradlew docs:generateJavaDocs docs:doxygen -PbuildServer ${{ env.EXTRA_GRADLE_ARGS }}
       - name: Install SSH Client 🔑
-        uses: webfactory/ssh-agent@v0.4.1
+        uses: webfactory/ssh-agent@v0.8.0
         with:
           ssh-private-key: ${{ secrets.GH_DEPLOY_KEY }}
-      - name: Deploy Java 🚀
-        uses: JamesIves/github-pages-deploy-action@3.7.1
+      - name: Deploy 🚀
+        uses: JamesIves/github-pages-deploy-action@v4.4.1
         with:
-          SSH: true
-          REPOSITORY_NAME: wpilibsuite/wpilibsuite.github.io
-          BRANCH: main
-          CLEAN: true
-          FOLDER: docs/build/docs/javadoc
-          TARGET_FOLDER: ${{ env.TARGET_FOLDER }}/java
-      - name: Deploy C++ 🚀
-        uses: JamesIves/github-pages-deploy-action@3.7.1
+          ssh-key: true
+          repository-name: wpilibsuite/wpilibsuite.github.io
+          branch: allwpilib-${{ env.BRANCH }}
+          clean: true
+          single-commit: true
+          folder: docs/build/docs
+      - name: Trigger Workflow
+        uses: actions/github-script@v6
         with:
-          SSH: true
-          REPOSITORY_NAME: wpilibsuite/wpilibsuite.github.io
-          BRANCH: main
-          CLEAN: true
-          FOLDER: docs/build/docs/doxygen/html
-          TARGET_FOLDER: ${{ env.TARGET_FOLDER }}/cpp
+          github-token: ${{ secrets.DISPATCH_PAT_TOKEN }}
+          script: |
+            github.rest.actions.createWorkflowDispatch({
+              owner: context.repo.owner,
+              repo: 'wpilibsuite.github.io',
+              workflow_id: 'static.yml',
+              ref: 'main',
+            })

.github/workflows/fix_compile_commands.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+
+import argparse
+import json
+
+
+def main():
+    parser = argparse.ArgumentParser(
+        description="Fix compile_commands.json generated by Gradle"
+    )
+    parser.add_argument("filename", help="compile_commands.json location")
+    cmd_args = parser.parse_args()
+
+    # Read JSON
+    with open(cmd_args.filename) as f:
+        data = json.load(f)
+
+    for obj in data:
+        out_args = []
+
+        # Filter out -isystem flags that cause false positives
+        iter_args = iter(obj["arguments"])
+        for arg in iter_args:
+            if arg == "-isystem":
+                next_arg = next(iter_args)
+
+                # /usr/lib/gcc/x86_64-pc-linux-gnu/13.2.1/include/xmmintrin.h:54:1:
+                # error: conflicting types for '_mm_prefetch' [clang-diagnostic-error]
+                if not next_arg.startswith("/usr/lib/gcc/"):
+                    out_args += ["-isystem", next_arg]
+            else:
+                out_args.append(arg)
+
+        obj["arguments"] = out_args
+
+    # Write JSON
+    with open(cmd_args.filename, "w") as f:
+        json.dump(data, f, indent=2)
+
+
+if __name__ == "__main__":
+    main()

.github/workflows/gazebo.yml

@@ -1,15 +0,0 @@
-name: Gazebo
-
-on: [pull_request, push]
-
-jobs:
-  build:
-    name: "Build"
-    runs-on: ubuntu-latest
-    container: wpilib/gazebo-ubuntu:18.04
-    steps:
-      - uses: actions/checkout@v2
-        with:
-          fetch-depth: 0
-      - name: Build with Gradle
-        run: ./gradlew simulation:frc_gazebo_plugins:build simulation:halsim_gazebo:build -PbuildServer -PforceGazebo

.github/workflows/gradle-wrapper-validation.yml

@@ -1,10 +1,14 @@
 name: "Validate Gradle Wrapper"
 on: [pull_request, push]
 
+concurrency:
+  group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
+  cancel-in-progress: true
+
 jobs:
   validation:
     name: "Validation"
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v4
       - uses: gradle/wrapper-validation-action@v1

.github/workflows/gradle.yml

@@ -2,66 +2,120 @@ name: Gradle
 
 on: [pull_request, push]
 
+concurrency:
+  group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
+  cancel-in-progress: true
+
 jobs:
   build-docker:
     strategy:
       fail-fast: false
       matrix:
         include:
-          - container: wpilib/roborio-cross-ubuntu:2022-18.04
+          - container: wpilib/roborio-cross-ubuntu:2024-22.04
             artifact-name: Athena
             build-options: "-Ponlylinuxathena"
-          - container: wpilib/raspbian-cross-ubuntu:10-18.04
-            artifact-name: Raspbian
-            build-options: "-Ponlylinuxraspbian"
-          - container: wpilib/aarch64-cross-ubuntu:bionic-18.04
-            artifact-name: Aarch64
-            build-options: "-Ponlylinuxaarch64bionic"
-          - container: wpilib/ubuntu-base:18.04
+          - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04
+            artifact-name: Arm32
+            build-options: "-Ponlylinuxarm32"
+          - container: wpilib/aarch64-cross-ubuntu:bullseye-22.04
+            artifact-name: Arm64
+            build-options: "-Ponlylinuxarm64"
+          - container: wpilib/ubuntu-base:22.04
             artifact-name: Linux
-            build-options: "-Dorg.gradle.jvmargs=-Xmx2g"
+            build-options: "-Ponlylinuxx86-64"
     name: "Build - ${{ matrix.artifact-name }}"
-    runs-on: ubuntu-latest
-    container: ${{ matrix.container }}
+    runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v2
+      - name: Free Disk Space
+        uses: jlumbroso/free-disk-space@main
+        with:
+          tool-cache: false
+          android: true
+          dotnet: true
+          haskell: true
+          large-packages: false
+          docker-images: false
+          swap-storage: false
+      - uses: actions/checkout@v4
         with:
           fetch-depth: 0
       - name: Set release environment variable
         run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
         if: startsWith(github.ref, 'refs/tags/v')
       - name: Build with Gradle
-        run: ./gradlew build -PbuildServer -PskipJavaFormat ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
-      - uses: actions/upload-artifact@v2
+        uses: addnab/docker-run-action@v3
+        with:
+          image: ${{ matrix.container }}
+          options: -v ${{ github.workspace }}:/work -w /work -e ARTIFACTORY_PUBLISH_USERNAME -e ARTIFACTORY_PUBLISH_PASSWORD -e GITHUB_REF -e CI
+          run: df . && rm -f semicolon_delimited_script && echo $GITHUB_REF && ./gradlew build --build-cache -PbuildServer -PskipJavaFormat ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
+        env:
+          ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
+          ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
+      - name: Check free disk space
+        run: df .
+      - uses: actions/upload-artifact@v4
         with:
           name: ${{ matrix.artifact-name }}
           path: build/allOutputs
 
   build-host:
     env:
-      MACOSX_DEPLOYMENT_TARGET: 10.14
+      MACOSX_DEPLOYMENT_TARGET: 12
     strategy:
       fail-fast: false
       matrix:
         include:
-          - os: windows-2019
-            artifact-name: Win64
+          - os: windows-2022
+            artifact-name: Win64Debug
             architecture: x64
-          - os: windows-2019
-            artifact-name: Win32
-            architecture: x86
-          - os: macOS-11
+            task: "build"
+            build-options: "-PciDebugOnly"
+            outputs: "build/allOutputs"
+            build-dir: "c:\\work"
+          - os: windows-2022
+            artifact-name: Win64Release
+            architecture: x64
+            build-options: "-PciReleaseOnly"
+            task: "copyAllOutputs"
+            outputs: "build/allOutputs"
+            build-dir: "c:\\work"
+          - os: windows-2022
+            artifact-name: WinArm64Debug
+            architecture: x64
+            task: "build"
+            build-options: "-PciDebugOnly -Pbuildwinarm64 -Ponlywindowsarm64"
+            outputs: "build/allOutputs"
+            build-dir: "c:\\work"
+          - os: windows-2022
+            artifact-name: WinArm64Release
+            architecture: x64
+            build-options: "-PciReleaseOnly -Pbuildwinarm64 -Ponlywindowsarm64"
+            task: "copyAllOutputs"
+            outputs: "build/allOutputs"
+            build-dir: "c:\\work"
+          - os: macOS-12
             artifact-name: macOS
             architecture: x64
+            task: "build"
+            outputs: "build/allOutputs"
+            build-dir: "."
+          - os: windows-2022
+            artifact-name: Win32
+            architecture: x86
+            task: ":ntcoreffi:build"
+            outputs: "ntcoreffi/build/outputs"
+            build-dir: "c:\\work"
     name: "Build - ${{ matrix.artifact-name }}"
     runs-on: ${{ matrix.os }}
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v4
         with:
           fetch-depth: 0
-      - uses: actions/setup-java@v1
+      - uses: actions/setup-java@v4
         with:
-          java-version: 11
+          distribution: 'zulu'
+          java-version: 17
           architecture: ${{ matrix.architecture }}
       - name: Import Developer ID Certificate
         uses: wpilibsuite/import-signing-certificate@v1
@@ -81,36 +135,55 @@ jobs:
         run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
         shell: bash
         if: startsWith(github.ref, 'refs/tags/v')
+      - name: Set Java Heap Size
+        run: sed -i 's/-Xmx2g/-Xmx1g/g' gradle.properties
+        if: matrix.artifact-name == 'Win32'
+      - name: Configure build directory (Windows)
+        run: xcopy . ${{ matrix.build-dir }} /i /s /e /h /q
+        if: matrix.os == 'windows-2022'
+      - name: Check disk free space (Windows)
+        run: wmic logicaldisk get caption, freespace
+        if: matrix.os == 'windows-2022'
       - name: Build with Gradle
-        run: ./gradlew build -PbuildServer -PskipJavaFormat ${{ env.EXTRA_GRADLE_ARGS }}
+        run: ./gradlew ${{ matrix.task }} --build-cache -PbuildServer -PskipJavaFormat ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
+        working-directory: ${{ matrix.build-dir }}
+        env:
+          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 }} ${{ env.EXTRA_GRADLE_ARGS }}
+        run: ./gradlew copyAllOutputs --build-cache -PbuildServer -PskipJavaFormat -PdeveloperID=${{ secrets.APPLE_DEVELOPER_ID }} ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
+        working-directory: ${{ matrix.build-dir }}
         if: |
           matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
           (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v')))
-      - uses: actions/upload-artifact@v2
+      - name: Check disk free space (Windows)
+        run: wmic logicaldisk get caption, freespace
+        if: matrix.os == 'windows-2022'
+      - uses: actions/upload-artifact@v4
         with:
           name: ${{ matrix.artifact-name }}
-          path: build/allOutputs
+          path: ${{ matrix.build-dir }}/${{ matrix.outputs }}
 
   build-documentation:
     name: "Build - Documentation"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v4
         with:
           fetch-depth: 0
-      - uses: actions/setup-java@v1
+      - uses: actions/setup-java@v4
         with:
-          java-version: 13
-      - name: Install libclang-9
-        run: sudo apt update && sudo apt install -y libclang-cpp9 libclang1-9
+          distribution: 'zulu'
+          java-version: 17
       - name: Set release environment variable
         run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
         if: startsWith(github.ref, 'refs/tags/v')
       - name: Build with Gradle
-        run: ./gradlew docs:zipDocs -PbuildServer ${{ env.EXTRA_GRADLE_ARGS }}
-      - uses: actions/upload-artifact@v2
+        run: ./gradlew docs:zipDocs --build-cache -PbuildServer ${{ env.EXTRA_GRADLE_ARGS }}
+        env:
+          ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
+          ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
+      - uses: actions/upload-artifact@v4
         with:
           name: Documentation
           path: docs/build/outputs
@@ -118,29 +191,53 @@ jobs:
   combine:
     name: Combine
     needs: [build-docker, build-host, build-documentation]
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v2
+      - name: Free Disk Space
+        if: |
+          github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
+        uses: jlumbroso/free-disk-space@main
+        with:
+          tool-cache: false
+          android: true
+          dotnet: true
+          haskell: true
+          large-packages: false
+          docker-images: false
+          swap-storage: false
+      - uses: actions/checkout@v4
+        if: |
+          github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
         with:
           repository: wpilibsuite/build-tools
-      - uses: actions/download-artifact@v2
+      - uses: actions/download-artifact@v4
+        if: |
+          github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
         with:
           path: combiner/products/build/allOutputs
       - name: Flatten Artifacts
+        if: |
+          github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
         run: rsync -a --delete combiner/products/build/allOutputs/*/* combiner/products/build/allOutputs/
       - name: Check version number exists
+        if: |
+          github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
         run: |
           cat combiner/products/build/allOutputs/version.txt
           test -s combiner/products/build/allOutputs/version.txt
-      - uses: actions/setup-java@v1
-        with:
-          java-version: 11
-      - name: Combine
+      - uses: actions/setup-java@v4
         if: |
-          !startsWith(github.ref, 'refs/tags/v') &&
-          github.ref != 'refs/heads/main'
-        run: cd combiner && ./gradlew publish -Pallwpilib
-      - name: Combine (Master)
+          github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
+        with:
+          distribution: 'zulu'
+          java-version: 17
+      - name: Combine (Main)
         if: |
           github.repository_owner == 'wpilibsuite' &&
           github.ref == 'refs/heads/main'
@@ -158,7 +255,10 @@ jobs:
           RUN_AZURE_ARTIFACTORY_RELEASE: "TRUE"
           ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
           ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
-      - uses: actions/upload-artifact@v2
+      - uses: actions/upload-artifact@v4
+        if: |
+          github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
         with:
           name: Maven
           path: ~/releases

.github/workflows/lint-format.yml

@@ -6,77 +6,91 @@ on:
     branches-ignore:
       - main
 
+concurrency:
+  group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
+  cancel-in-progress: true
+
 jobs:
   wpiformat:
     name: "wpiformat"
-    runs-on: ubuntu-latest
-    container: wpilib/roborio-cross-ubuntu:2022-20.04
+    runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
       - name: Fetch all history and metadata
         run: |
-          git fetch --prune --unshallow
           git checkout -b pr
           git branch -f main origin/main
       - name: Set up Python 3.8
-        uses: actions/setup-python@v2
+        uses: actions/setup-python@v5
         with:
           python-version: 3.8
-      - name: Install clang-format
-        run: |
-          sudo sh -c "echo 'deb http://archive.ubuntu.com/ubuntu/ $(lsb_release -cs)-proposed restricted main multiverse universe' >> /etc/apt/sources.list.d/proposed-repositories.list"
-          sudo apt-get update -q
-          sudo apt-get install -y clang-format-12
       - name: Install wpiformat
-        run: pip3 install wpiformat
+        run: pip3 install wpiformat==2024.32
       - name: Run
-        run: wpiformat -clang 12
+        run: wpiformat
       - name: Check output
         run: git --no-pager diff --exit-code HEAD
       - name: Generate diff
         run: git diff HEAD > wpiformat-fixes.patch
         if: ${{ failure() }}
-      - uses: actions/upload-artifact@v2
+      - uses: actions/upload-artifact@v4
         with:
           name: wpiformat fixes
           path: wpiformat-fixes.patch
         if: ${{ failure() }}
+      - name: Write to job summary
+        run: |
+          echo '```diff' >> $GITHUB_STEP_SUMMARY
+          cat wpiformat-fixes.patch >> $GITHUB_STEP_SUMMARY
+          echo '' >> $GITHUB_STEP_SUMMARY
+          echo '```' >> $GITHUB_STEP_SUMMARY
+        if: ${{ failure() }}
+
   tidy:
     name: "clang-tidy"
-    runs-on: ubuntu-latest
-    container: wpilib/roborio-cross-ubuntu:2022-20.04
+    runs-on: ubuntu-22.04
+    container: wpilib/roborio-cross-ubuntu:2023-22.04
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
       - name: Fetch all history and metadata
         run: |
-          git fetch --prune --unshallow
+          git config --global --add safe.directory /__w/allwpilib/allwpilib
           git checkout -b pr
           git branch -f main origin/main
       - name: Set up Python 3.8
-        uses: actions/setup-python@v2
+        uses: actions/setup-python@v5
         with:
           python-version: 3.8
-      - name: Install clang-tidy
-        run: |
-          sudo sh -c "echo 'deb http://archive.ubuntu.com/ubuntu/ $(lsb_release -cs)-proposed restricted main multiverse universe' >> /etc/apt/sources.list.d/proposed-repositories.list"
-          sudo apt-get update -q
-          sudo apt-get install -y clang-tidy-12 clang-format-12
       - name: Install wpiformat
         run: pip3 install wpiformat
       - name: Create compile_commands.json
-        run: ./gradlew generateCompileCommands -Ptoolchain-optional-roboRio
+        run: |
+          ./gradlew generateCompileCommands -Ptoolchain-optional-roboRio
+          ./.github/workflows/fix_compile_commands.py build/TargetedCompileCommands/linuxx86-64release/compile_commands.json
+          ./.github/workflows/fix_compile_commands.py build/TargetedCompileCommands/linuxx86-64debug/compile_commands.json
       - name: List changed files
         run: wpiformat -list-changed-files
-      - name: Run clang-tidy
-        run: wpiformat -clang 12 -no-format -tidy-changed -compile-commands=build/compile_commands/linuxx86-64 -vv
+      - name: Run clang-tidy release
+        run: wpiformat -no-format -tidy-changed -compile-commands=build/TargetedCompileCommands/linuxx86-64release -vv
+      - name: Run clang-tidy debug
+        run: wpiformat -no-format -tidy-changed -compile-commands=build/TargetedCompileCommands/linuxx86-64debug -vv
   javaformat:
     name: "Java format"
-    runs-on: ubuntu-latest
-    container: wpilib/ubuntu-base:18.04
+    runs-on: ubuntu-22.04
+    container: wpilib/ubuntu-base:22.04
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v4
         with:
           fetch-depth: 0
+      - name: Fetch all history and metadata
+        run: |
+          git config --global --add safe.directory /__w/allwpilib/allwpilib
+          git checkout -b pr
+          git branch -f main origin/main
       - name: Run Java format
         run: ./gradlew javaFormat spotbugsMain spotbugsTest spotbugsDev
       - name: Check output
@@ -86,15 +100,14 @@ jobs:
         if: ${{ failure() }}
   documentation:
     name: "Documentation"
-    runs-on: ubuntu-latest
+    runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v4
         with:
           fetch-depth: 0
-      - uses: actions/setup-java@v1
+      - uses: actions/setup-java@v4
         with:
-          java-version: 13
-      - name: Install libclang-9
-        run: sudo apt update && sudo apt install -y libclang-cpp9 libclang1-9
+          distribution: 'zulu'
+          java-version: 17
       - name: Build with Gradle
         run: ./gradlew docs:zipDocs -PbuildServer -PdocWarningsAsErrors ${{ env.EXTRA_GRADLE_ARGS }}

.github/workflows/pregenerate.yml

@@ -0,0 +1,46 @@
+name: Check Pregenerated Files
+
+on:
+  pull_request:
+  push:
+    branches-ignore:
+      - main
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
+  cancel-in-progress: true
+
+jobs:
+  update:
+    name: "Update"
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+      - name: Set up Python 3.9
+        uses: actions/setup-python@v5
+        with:
+          python-version: 3.9
+      - name: Install jinja
+        run: python -m pip install jinja2
+      - name: Install protobuf dependencies
+        run: sudo apt-get update && sudo apt-get install -y protobuf-compiler && wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.3/protoc-gen-quickbuf-1.3.3-linux-x86_64.exe && chmod +x protoc-gen-quickbuf-1.3.3-linux-x86_64.exe
+      - name: Run hal
+        run: ./hal/generate_usage_reporting.py
+      - name: Run ntcore
+        run: ./ntcore/generate_topics.py
+      - name: Run wpimath
+        run: ./wpimath/generate_numbers.py && ./wpimath/generate_quickbuf.py protoc protoc-gen-quickbuf-1.3.3-linux-x86_64.exe
+      - name: Add untracked files to index so they count as changes
+        run: git add -A
+      - name: Check output
+        run: git --no-pager diff --exit-code HEAD
+      - name: Generate diff
+        run: git diff HEAD > pregenerated-files-fixes.patch
+        if: ${{ failure() }}
+      - uses: actions/upload-artifact@v4
+        with:
+          name: pregenerated-files-fixes
+          path: pregenerated-files-fixes.patch
+        if: ${{ failure() }}

.github/workflows/sanitizers.yml

@@ -2,6 +2,10 @@ name: Sanitizers
 
 on: [pull_request, push]
 
+concurrency:
+  group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
+  cancel-in-progress: true
+
 jobs:
   build:
     strategy:
@@ -11,39 +15,45 @@ jobs:
           - name: asan
             cmake-flags: "-DCMAKE_BUILD_TYPE=Asan"
             ctest-env: ""
-            ctest-flags: "-E 'wpiutil|ntcore|wpilibc'"
+            ctest-flags: "-E 'wpilibc'"
           - name: tsan
             cmake-flags: "-DCMAKE_BUILD_TYPE=Tsan"
             ctest-env: "TSAN_OPTIONS=second_deadlock_stack=1"
-            ctest-flags: "-E 'ntcore|cscore|cameraserver|wpilibc|wpilibNewCommands'"
+            ctest-flags: "-E 'cscore|cameraserver|wpilibc|wpilibNewCommands'"
           - name: ubsan
             cmake-flags: "-DCMAKE_BUILD_TYPE=Ubsan"
             ctest-env: ""
             ctest-flags: ""
     name: "${{ matrix.name }}"
-    runs-on: ubuntu-latest
-    container: wpilib/roborio-cross-ubuntu:2022-20.04
+    runs-on: ubuntu-22.04
+    container: wpilib/roborio-cross-ubuntu:2024-22.04
     steps:
-      - uses: actions/checkout@v2
       - name: Install Dependencies
-        run: |
-          sudo add-apt-repository ppa:ubuntu-toolchain-r/test
-          sudo apt install -y gcc-11 g++-11
-          sudo update-alternatives \
-            --install /usr/bin/gcc gcc /usr/bin/gcc-11 11 \
-            --slave /usr/bin/g++ g++ /usr/bin/g++-11
-          sudo update-alternatives --set gcc /usr/bin/gcc-11
-      - name: Set up Python 3.8
-        uses: actions/setup-python@v2
-        with:
-          python-version: 3.8
+        run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv4.5-java python-is-python3 clang-14 libprotobuf-dev protobuf-compiler ninja-build
+
+      - name: Install QuickBuffers
+        if: runner.os == 'Linux'
+        run: wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.3/protoc-gen-quickbuf_1.3.3_amd64.deb && sudo apt install ./protoc-gen-quickbuf_1.3.3_amd64.deb
+
+      - name: Run sccache-cache
+        uses: mozilla-actions/sccache-action@v0.0.3
+
       - name: Install jinja
         run: python -m pip install jinja2
+
+      - uses: actions/checkout@v4
+
       - name: configure
-        run: mkdir build && cd build && cmake ${{ matrix.cmake-flags }} ..
+        run: mkdir build && cd build && cmake -G Ninja -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_C_COMPILER:FILEPATH=/usr/bin/clang-14 -DCMAKE_CXX_COMPILER:FILEPATH=/usr/bin/clang++-14 -DWITH_JAVA=OFF ${{ matrix.cmake-flags }} ..
+        env:
+          SCCACHE_GHA_ENABLED: "true"
+
       - name: build
         working-directory: build
-        run: cmake --build . -j$(nproc)
+        run: cmake --build . --parallel $(nproc)
+        env:
+          SCCACHE_GHA_ENABLED: "true"
+
       - name: test
         working-directory: build
         run: ${{ matrix.ctest-env }} ctest --output-on-failure ${{ matrix.ctest-flags }}

.github/workflows/upstream-utils.yml

@@ -0,0 +1,76 @@
+name: Upstream utils
+
+on:
+  pull_request:
+  push:
+    branches-ignore:
+      - main
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.head_ref || github.ref }}
+  cancel-in-progress: true
+
+jobs:
+  update:
+    name: "Update"
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+      - name: Fetch all history and metadata
+        run: |
+          git checkout -b pr
+          git branch -f main origin/main
+      - name: Set up Python 3.9
+        uses: actions/setup-python@v5
+        with:
+          python-version: 3.9
+      - name: Configure committer identity
+        run: |
+          git config --global user.email "you@example.com"
+          git config --global user.name "Your Name"
+      - name: Run update_eigen.py
+        run: |
+          cd upstream_utils
+          ./update_eigen.py
+      - name: Run update_fmt.py
+        run: |
+          cd upstream_utils
+          ./update_fmt.py
+      - name: Run update_gcem.py
+        run: |
+          cd upstream_utils
+          ./update_gcem.py
+      - name: Run update_json.py
+        run: |
+          cd upstream_utils
+          ./update_json.py
+      - name: Run update_libuv.py
+        run: |
+          cd upstream_utils
+          ./update_libuv.py
+      - name: Run update_llvm.py
+        run: |
+          cd upstream_utils
+          ./update_llvm.py
+      - name: Run update_mpack.py
+        run: |
+          cd upstream_utils
+          ./update_mpack.py
+      - name: Run update_stack_walker.py
+        run: |
+          cd upstream_utils
+          ./update_stack_walker.py
+      - name: Run update_memory.py
+        run: |
+          cd upstream_utils
+          ./update_memory.py
+      - name: Run update_protobuf.py
+        run: |
+          cd upstream_utils
+          ./update_protobuf.py
+      - name: Add untracked files to index so they count as changes
+        run: git add -A
+      - name: Check output
+        run: git --no-pager diff --exit-code HEAD

.gitignore

@@ -5,6 +5,12 @@ doxygen.log
 build*/
 !buildSrc/
 
+simgui-ds.json
+simgui-window.json
+simgui.json
+
+networktables.json
+
 # Created by the jenkins test script
 test-reports
 
@@ -15,6 +21,9 @@ test-reports
 .idea/
 out/
 
+# Fleet
+.fleet
+
 # Created by http://www.gitignore.io
 
 ### Linux ###
@@ -196,6 +205,7 @@ NO
 *.user
 *.userosscache
 *.sln.docstates
+CMakeSettings.json
 
 # Visual C++ cache files
 ipch/

.styleguide

@@ -10,6 +10,7 @@ cppSrcFileInclude {
 }
 
 modifiableFileExclude {
+  cmake/toolchains/
   \.patch$
   gradlew
 }
@@ -18,6 +19,7 @@ generatedFileExclude {
   FRCNetComm\.java$
   simulation/gz_msgs/src/include/simulation/gz_msgs/msgs\.h$
   fieldImages/src/main/native/resources/
+  apriltag/src/test/resources/
 }
 
 repoRootNameOverride {
@@ -28,8 +30,9 @@ includeOtherLibs {
   ^Eigen/
   ^cameraserver/
   ^cscore
-  ^drake/
   ^fmt/
+  ^gtest/
+  ^google/
   ^hal/
   ^imgui
   ^implot
@@ -44,4 +47,5 @@ includeOtherLibs {
   ^wpi/
   ^wpigui
   ^wpimath/
+  ^wpinet/
 }

CMakeLists.txt

@@ -1,25 +1,33 @@
 # Disable in-source builds to prevent source tree corruption.
 if(" ${CMAKE_CURRENT_SOURCE_DIR}" STREQUAL " ${CMAKE_CURRENT_BINARY_DIR}")
-  message(FATAL_ERROR "
+    message(
+        FATAL_ERROR
+        "
 FATAL: In-source builds are not allowed.
        You should create a separate directory for build files.
-")
+"
+    )
 endif()
 
-if ("${CMAKE_HOST_SYSTEM_NAME}" STREQUAL "Windows")
+if("${CMAKE_HOST_SYSTEM_NAME}" STREQUAL "Windows")
     set(CMAKE_SYSTEM_VERSION 10.0.18362.0 CACHE STRING INTERNAL FORCE)
     set(CMAKE_VS_WINDOWS_TARGET_PLATFORM_VERSION 10.0.18362.0 CACHE STRING INTERNAL FORCE)
 endif()
 
+cmake_minimum_required(VERSION 3.11)
 project(allwpilib)
-cmake_minimum_required(VERSION 3.3.0)
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules")
 
+# Make timestamps of extracted files from FetchContent the time of extraction
+if(POLICY CMP0135)
+    cmake_policy(SET CMP0135 NEW)
+endif()
+
 message(STATUS "Platform version: ${CMAKE_VS_WINDOWS_TARGET_PLATFORM_VERSION}")
 
 set(WPILIB_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR})
 
-INCLUDE(CPack)
+include(CPack)
 
 set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS TRUE)
 set_property(GLOBAL PROPERTY USE_FOLDERS ON)
@@ -30,31 +38,33 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${WPILIB_BINARY_DIR}/bin)
 set(CMAKE_JAVA_TARGET_OUTPUT_DIR ${WPILIB_BINARY_DIR}/jar)
 
 # use, i.e. don't skip the full RPATH for the build tree
-SET(CMAKE_SKIP_BUILD_RPATH  FALSE)
+set(CMAKE_SKIP_BUILD_RPATH FALSE)
 
 # when building, don't use the install RPATH already
 # (but later on when installing)
-SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
 
-SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/wpilib/lib")
+set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
 
 # add the automatically determined parts of the RPATH
 # which point to directories outside the build tree to the install RPATH
-SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
 
 # the RPATH to be used when installing, but only if it's not a system directory
-LIST(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/wpilib/lib" isSystemDir)
-IF("${isSystemDir}" STREQUAL "-1")
-   SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/wpilib/lib")
-ENDIF("${isSystemDir}" STREQUAL "-1")
+list(FIND CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES "${CMAKE_INSTALL_PREFIX}/lib" isSystemDir)
+if("${isSystemDir}" STREQUAL "-1")
+    set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
+endif()
 
 # Options for building certain parts of the repo. Everything is built by default.
 option(BUILD_SHARED_LIBS "Build with shared libs (needed for JNI)" ON)
-option(WITH_JAVA "Include java and JNI in the build" ON)
+option(WITH_JAVA "Include Java and JNI in the build" ON)
+option(WITH_JAVA_SOURCE "Build Java source jars" ${WITH_JAVA})
 option(WITH_CSCORE "Build cscore (needs OpenCV)" ON)
+option(WITH_NTCORE "Build ntcore" ON)
 option(WITH_WPIMATH "Build wpimath" ON)
+option(WITH_WPIUNITS "Build wpiunits" ON)
 option(WITH_WPILIB "Build hal, wpilibc/j, and myRobot (needs OpenCV)" ON)
-option(WITH_OLD_COMMANDS "Build old commands" OFF)
 option(WITH_EXAMPLES "Build examples" OFF)
 option(WITH_TESTS "Build unit tests (requires internet connection)" ON)
 option(WITH_GUI "Build GUI items" ON)
@@ -64,110 +74,159 @@ option(WITH_SIMULATION_MODULES "Build simulation modules" ON)
 option(WITH_EXTERNAL_HAL "Use a separately built HAL" OFF)
 set(EXTERNAL_HAL_FILE "" CACHE FILEPATH "Location to look for an external HAL CMake File")
 
-# Options for using a package manager (vcpkg) for certain dependencies.
-option(USE_VCPKG_FMTLIB "Use vcpkg fmtlib" OFF)
-option(USE_VCPKG_LIBUV "Use vcpkg libuv" OFF)
-option(USE_VCPKG_EIGEN "Use vcpkg eigen" OFF)
-
-# Options for installation.
-option(WITH_FLAT_INSTALL "Use a flat install directory" OFF)
+# Options for using a package manager (e.g., vcpkg) for certain dependencies.
+option(USE_SYSTEM_FMTLIB "Use system fmtlib" OFF)
+option(USE_SYSTEM_LIBUV "Use system libuv" OFF)
+option(USE_SYSTEM_EIGEN "Use system eigen" OFF)
 
 # Options for location of OpenCV Java.
 set(OPENCV_JAVA_INSTALL_DIR "" CACHE PATH "Location to search for the OpenCV jar file")
 
+# Options for compilation flags.
+option(NO_WERROR "Disable -Werror flag during compilation" OFF)
+
 # Set default build type to release with debug info (i.e. release mode optimizations
 # are performed, but debug info still exists).
-if (NOT CMAKE_BUILD_TYPE)
-    set (CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "" FORCE)
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "" FORCE)
 endif()
 
-# We always want flat install with MSVC.
-if (MSVC)
-  set(WITH_FLAT_INSTALL ON)
-endif()
-
-if (WITH_JAVA AND NOT BUILD_SHARED_LIBS)
-    message(FATAL_ERROR "
+if(WITH_JAVA AND NOT BUILD_SHARED_LIBS)
+    message(
+        FATAL_ERROR
+        "
 FATAL: Cannot build static libs with Java enabled.
        Static libs requires both BUILD_SHARED_LIBS=OFF and
        WITH_JAVA=OFF
-")
+"
+    )
 endif()
 
-if (WITH_SIMULATION_MODULES AND NOT BUILD_SHARED_LIBS)
-    message(FATAL_ERROR "
+if(WITH_SIMULATION_MODULES AND NOT BUILD_SHARED_LIBS)
+    message(
+        FATAL_ERROR
+        "
 FATAL: Cannot build static libs with simulation modules enabled.
        Static libs requires both BUILD_SHARED_LIBS=OFF and
        WITH_SIMULATION_MODULES=OFF
-")
+"
+    )
 endif()
 
-if (NOT WITH_JAVA OR NOT WITH_CSCORE)
+if(NOT WITH_JAVA OR NOT WITH_CSCORE)
     if(NOT "${OPENCV_JAVA_INSTALL_DIR}" STREQUAL "")
-        message(WARNING "
+        message(
+            WARNING
+            "
 WARNING: OpenCV Java dir set but java is not enabled!
 It will be ignored.
-")
+"
+        )
     endif()
 endif()
 
-if (NOT WITH_WPILIB AND WITH_SIMULATION_MODULES)
-    message(FATAL_ERROR "
+if(NOT WITH_WPILIB AND WITH_SIMULATION_MODULES)
+    message(
+        FATAL_ERROR
+        "
 FATAL: Cannot build simulation modules with wpilib disabled.
        Enable wpilib by setting WITH_WPILIB=ON
-")
+"
+    )
 endif()
 
-if (NOT WITH_WPIMATH AND WITH_WPILIB)
-    message(FATAL_ERROR "
+if(NOT WITH_NTCORE AND WITH_CSCORE)
+    message(
+        FATAL_ERROR
+        "
+FATAL: Cannot build cameraserver without ntcore.
+       Enable ntcore by setting WITH_NTCORE=ON
+"
+    )
+endif()
+
+if(NOT WITH_NTCORE AND WITH_GUI)
+    message(
+        FATAL_ERROR
+        "
+FATAL: Cannot build GUI modules without ntcore.
+       Enable ntcore by setting WITH_NTCORE=ON
+"
+    )
+endif()
+
+if(NOT WITH_NTCORE AND WITH_SIMULATION_MODULES)
+    message(
+        FATAL_ERROR
+        "
+FATAL: Cannot build simulation modules without ntcore.
+       Enable ntcore by setting WITH_NTCORE=ON
+"
+    )
+endif()
+
+if(NOT WITH_NTCORE AND WITH_WPILIB)
+    message(
+        FATAL_ERROR
+        "
+FATAL: Cannot build wpilib without ntcore.
+       Enable ntcore by setting WITH_NTCORE=ON
+"
+    )
+endif()
+
+if(NOT WITH_WPIMATH AND WITH_WPILIB)
+    message(
+        FATAL_ERROR
+        "
 FATAL: Cannot build wpilib without wpimath.
        Enable wpimath by setting WITH_WPIMATH=ON
-")
+"
+    )
 endif()
 
-set( wpilib_dest wpilib)
-set( include_dest wpilib/include )
-set( main_lib_dest wpilib/lib )
-set( java_lib_dest wpilib/java )
-set( jni_lib_dest wpilib/jni )
-
-if (WITH_FLAT_INSTALL)
-    set (wpilib_config_dir ${wpilib_dest})
-else()
-    set (wpilib_config_dir share/wpilib)
+if(NOT WITH_WPIUNITS AND WITH_WPIMATH AND WITH_JAVA)
+    message(
+        FATAL_ERROR
+        "
+FATAL: Cannot build Java wpimath without wpiunits.
+       Enable wpiunits by setting WITH_WPIUNITS=ON or disable the Java build by setting WITH_JAVA=OFF
+"
+    )
 endif()
 
-if (USE_VCPKG_LIBUV)
-set (LIBUV_VCPKG_REPLACE "find_package(unofficial-libuv CONFIG)")
+set(include_dest include)
+set(java_lib_dest java)
+set(jni_lib_dest jni)
+
+if(USE_SYSTEM_LIBUV)
+    set(LIBUV_SYSTEM_REPLACE
+        "
+find_dependency(libuv CONFIG)
+"
+    )
 endif()
 
-if (USE_VCPKG_EIGEN)
-set (EIGEN_VCPKG_REPLACE "find_package(Eigen3 CONFIG)")
+if(USE_SYSTEM_EIGEN)
+    set(EIGEN_SYSTEM_REPLACE "find_package(Eigen3 CONFIG)")
 endif()
 
 find_package(LIBSSH 0.7.1)
 
-if (WITH_FLAT_INSTALL)
-set(WPIUTIL_DEP_REPLACE "include($\{SELF_DIR\}/wpiutil-config.cmake)")
-set(NTCORE_DEP_REPLACE "include($\{SELF_DIR\}/ntcore-config.cmake)")
-set(CSCORE_DEP_REPLACE_IMPL "include(\${SELF_DIR}/cscore-config.cmake)")
-set(CAMERASERVER_DEP_REPLACE_IMPL "include(\${SELF_DIR}/cameraserver-config.cmake)")
-set(HAL_DEP_REPLACE_IMPL "include(\${SELF_DIR}/hal-config.cmake)")
-set(WPIMATH_DEP_REPLACE "include($\{SELF_DIR\}/wpimath-config.cmake)")
-set(WPILIBC_DEP_REPLACE_IMPL "include(\${SELF_DIR}/wpilibc-config.cmake)")
-set(WPILIBNEWCOMMANDS_DEP_REPLACE "include(\${SELF_DIR}/wpilibNewcommands-config.cmake)")
-set(WPILIBOLDCOMMANDS_DEP_REPLACE "include(\${SELF_DIR}/wpilibOldcommands-config.cmake)")
-else()
-set(WPIUTIL_DEP_REPLACE "find_dependency(wpiutil)")
-set(NTCORE_DEP_REPLACE "find_dependency(ntcore)")
-set(CSCORE_DEP_REPLACE_IMPL "find_dependency(cscore)")
+find_package(Protobuf REQUIRED)
+
+set(APRILTAG_DEP_REPLACE "find_dependency(apriltag)")
 set(CAMERASERVER_DEP_REPLACE_IMPL "find_dependency(cameraserver)")
+set(CSCORE_DEP_REPLACE_IMPL "find_dependency(cscore)")
 set(HAL_DEP_REPLACE_IMPL "find_dependency(hal)")
-set(WPIMATH_DEP_REPLACE "find_dependency(wpimath)")
+set(NTCORE_DEP_REPLACE "find_dependency(ntcore)")
 set(WPILIBC_DEP_REPLACE_IMPL "find_dependency(wpilibc)")
+set(WPILIBJ_DEP_REPLACE "find_dependency(wpilibj)")
 set(WPILIBNEWCOMMANDS_DEP_REPLACE "find_dependency(wpilibNewCommands)")
-set(WPILIBOLDCOMMANDS_DEP_REPLACE "find_dependency(wpilibOldCommands)")
-endif()
+set(WPIMATH_DEP_REPLACE "find_dependency(wpimath)")
+set(WPINET_DEP_REPLACE "find_dependency(wpinet)")
+set(WPIUNITS_DEP_REPLACE "find_dependency(wpiunits)")
+set(WPIUTIL_DEP_REPLACE "find_dependency(wpiutil)")
 
 set(FILENAME_DEP_REPLACE "get_filename_component(SELF_DIR \"$\{CMAKE_CURRENT_LIST_FILE\}\" PATH)")
 set(SELF_DIR "$\{SELF_DIR\}")
@@ -185,7 +244,15 @@ if(isMultiConfig)
         list(APPEND CMAKE_CONFIGURATION_TYPES Ubsan)
     endif()
 else()
-    set(allowedBuildTypes Asan Tsan Ubsan Debug Release RelWithDebInfo MinSizeRel)
+    set(allowedBuildTypes
+        Asan
+        Tsan
+        Ubsan
+        Debug
+        Release
+        RelWithDebInfo
+        MinSizeRel
+    )
     set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "${allowedBuildTypes}")
 
     if(CMAKE_BUILD_TYPE AND NOT CMAKE_BUILD_TYPE IN_LIST allowedBuildTypes)
@@ -194,106 +261,156 @@ else()
 endif()
 
 set(CMAKE_C_FLAGS_ASAN
-    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=address -fno-omit-frame-pointer" CACHE STRING
-    "Flags used by the C compiler for Asan build type or configuration." FORCE)
+    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=address -fno-omit-frame-pointer"
+    CACHE STRING
+    "Flags used by the C compiler for Asan build type or configuration."
+    FORCE
+)
 
 set(CMAKE_CXX_FLAGS_ASAN
-    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=address -fno-omit-frame-pointer" CACHE STRING
-    "Flags used by the C++ compiler for Asan build type or configuration." FORCE)
+    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=address -fno-omit-frame-pointer"
+    CACHE STRING
+    "Flags used by the C++ compiler for Asan build type or configuration."
+    FORCE
+)
 
 set(CMAKE_EXE_LINKER_FLAGS_ASAN
-    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=address" CACHE STRING
-    "Linker flags to be used to create executables for Asan build type." FORCE)
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=address"
+    CACHE STRING
+    "Linker flags to be used to create executables for Asan build type."
+    FORCE
+)
 
 set(CMAKE_SHARED_LINKER_FLAGS_ASAN
-    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=address" CACHE STRING
-    "Linker lags to be used to create shared libraries for Asan build type." FORCE)
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=address"
+    CACHE STRING
+    "Linker lags to be used to create shared libraries for Asan build type."
+    FORCE
+)
 
 set(CMAKE_C_FLAGS_TSAN
-    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=thread -fno-omit-frame-pointer" CACHE STRING
-    "Flags used by the C compiler for Tsan build type or configuration." FORCE)
+    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=thread -fno-omit-frame-pointer"
+    CACHE STRING
+    "Flags used by the C compiler for Tsan build type or configuration."
+    FORCE
+)
 
 set(CMAKE_CXX_FLAGS_TSAN
-    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=thread -fno-omit-frame-pointer" CACHE STRING
-    "Flags used by the C++ compiler for Tsan build type or configuration." FORCE)
+    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=thread -fno-omit-frame-pointer"
+    CACHE STRING
+    "Flags used by the C++ compiler for Tsan build type or configuration."
+    FORCE
+)
 
 set(CMAKE_EXE_LINKER_FLAGS_TSAN
-    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=thread" CACHE STRING
-    "Linker flags to be used to create executables for Tsan build type." FORCE)
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=thread"
+    CACHE STRING
+    "Linker flags to be used to create executables for Tsan build type."
+    FORCE
+)
 
 set(CMAKE_SHARED_LINKER_FLAGS_TSAN
-    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=thread" CACHE STRING
-    "Linker lags to be used to create shared libraries for Tsan build type." FORCE)
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=thread"
+    CACHE STRING
+    "Linker lags to be used to create shared libraries for Tsan build type."
+    FORCE
+)
 
 set(CMAKE_C_FLAGS_UBSAN
-    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all -fno-omit-frame-pointer" CACHE STRING
-    "Flags used by the C compiler for Ubsan build type or configuration." FORCE)
+    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all -fno-omit-frame-pointer"
+    CACHE STRING
+    "Flags used by the C compiler for Ubsan build type or configuration."
+    FORCE
+)
 
 set(CMAKE_CXX_FLAGS_UBSAN
-    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all -fno-omit-frame-pointer" CACHE STRING
-    "Flags used by the C++ compiler for Ubsan build type or configuration." FORCE)
+    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all -fno-omit-frame-pointer"
+    CACHE STRING
+    "Flags used by the C++ compiler for Ubsan build type or configuration."
+    FORCE
+)
 
 set(CMAKE_EXE_LINKER_FLAGS_UBSAN
-    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all" CACHE STRING
-    "Linker flags to be used to create executables for Ubsan build type." FORCE)
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all"
+    CACHE STRING
+    "Linker flags to be used to create executables for Ubsan build type."
+    FORCE
+)
 
 set(CMAKE_SHARED_LINKER_FLAGS_UBSAN
-    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=undefined" CACHE STRING
-    "Linker lags to be used to create shared libraries for Ubsan build type." FORCE)
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=undefined"
+    CACHE STRING
+    "Linker lags to be used to create shared libraries for Ubsan build type."
+    FORCE
+)
 
-if (WITH_TESTS)
+if(WITH_TESTS)
     enable_testing()
     add_subdirectory(googletest)
     include(GoogleTest)
 endif()
 
 add_subdirectory(wpiutil)
-add_subdirectory(ntcore)
 
-if (WITH_WPIMATH)
+if(WITH_NTCORE)
+    add_subdirectory(wpinet)
+    add_subdirectory(ntcore)
+endif()
+
+if(WITH_WPIMATH)
+    if(WITH_JAVA)
+        add_subdirectory(wpiunits)
+    endif()
     add_subdirectory(wpimath)
 endif()
 
-if (WITH_GUI)
+if(WITH_WPIUNITS AND NOT WITH_WPIMATH)
+    # In case of building wpiunits standalone
+    add_subdirectory(wpiunits)
+endif()
+
+if(WITH_GUI)
     add_subdirectory(fieldImages)
     add_subdirectory(imgui)
     add_subdirectory(wpigui)
     add_subdirectory(glass)
     add_subdirectory(outlineviewer)
-    if (LIBSSH_FOUND)
+    add_subdirectory(sysid)
+    if(LIBSSH_FOUND)
         add_subdirectory(roborioteamnumbersetter)
+        add_subdirectory(datalogtool)
     endif()
 endif()
 
-if (WITH_WPILIB OR WITH_SIMULATION_MODULES)
+if(WITH_WPILIB OR WITH_SIMULATION_MODULES)
     set(HAL_DEP_REPLACE ${HAL_DEP_REPLACE_IMPL})
     add_subdirectory(hal)
 endif()
 
-if (WITH_CSCORE)
+if(WITH_CSCORE)
     set(CSCORE_DEP_REPLACE ${CSCORE_DEP_REPLACE_IMPL})
     set(CAMERASERVER_DEP_REPLACE ${CAMERASERVER_DEP_REPLACE_IMPL})
     add_subdirectory(cscore)
     add_subdirectory(cameraserver)
 endif()
 
-if (WITH_WPILIB)
+if(WITH_WPILIB)
     set(WPILIBC_DEP_REPLACE ${WPILIBC_DEP_REPLACE_IMPL})
+    add_subdirectory(apriltag)
     add_subdirectory(wpilibj)
     add_subdirectory(wpilibc)
     add_subdirectory(wpilibNewCommands)
-    if (WITH_OLD_COMMANDS)
-        add_subdirectory(wpilibOldCommands)
-    endif()
-    if (WITH_EXAMPLES)
+    add_subdirectory(romiVendordep)
+    add_subdirectory(xrpVendordep)
+    if(WITH_EXAMPLES)
         add_subdirectory(wpilibcExamples)
     endif()
     add_subdirectory(myRobot)
 endif()
 
-if (WITH_SIMULATION_MODULES AND NOT WITH_EXTERNAL_HAL)
+if(WITH_SIMULATION_MODULES AND NOT WITH_EXTERNAL_HAL)
     add_subdirectory(simulation)
 endif()
 
-configure_file(wpilib-config.cmake.in ${WPILIB_BINARY_DIR}/wpilib-config.cmake )
-install(FILES ${WPILIB_BINARY_DIR}/wpilib-config.cmake DESTINATION ${wpilib_config_dir})
+configure_file(wpilib-config.cmake.in ${WPILIB_BINARY_DIR}/wpilib-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/wpilib-config.cmake DESTINATION share/wpilib)

CONTRIBUTING.md

@@ -12,11 +12,11 @@ So you want to contribute your changes back to WPILib. Great! We have a few cont
 
 ## General Contribution Rules
 
-- Everything in the library must work for the 3000+ teams that will be using it.
+- Everything in the library must work for the 4000+ teams that will be using it.
 - We need to be able to maintain submitted changes, even if you are no longer working on the project.
 - Tool suite changes must be generally useful to a broad range of teams
 - Excluding bug fixes, changes in one language generally need to have corresponding changes in other languages.
-    - Some features, such the addition of C++11 for WPILibC or Functional Interfaces for WPILibJ, are specific to that version of WPILib only.
+    - Some features, such the addition of C++23 for WPILibC or Functional Interfaces for WPILibJ, are specific to that version of WPILib only. New language features added to C++ must be wrappable in Python for [RobotPy](https://github.com/robotpy).
     - Substantial changes often need to have corresponding LabVIEW changes. To do this, we will work with NI on these large changes.
 - Changes should have tests.
 - Code should be well documented.
@@ -27,7 +27,8 @@ So you want to contribute your changes back to WPILib. Great! We have a few cont
 - Bug reports and fixes
     - We will generally accept bug fixes without too much question. If they are only implemented for one language, we will implement them for any other necessary languages. Bug reports are also welcome, please submit them to our GitHub issue tracker.
 - While we do welcome improvements to the API, there are a few important rules to consider:
-    - Features must be added to both WPILibC and WPILibJ, with rare exceptions.
+    - Features must be added to Java (WPILibJ), C++ (WPILibC), with rare exceptions.
+        - Most of Python (RobotPy) is created by wrapping WPILibC with pybind11 via robotpy-build. However, new features to the command framework should also be submitted to [robotpy-commands-v2](https://github.com/robotpy/robotpy-commands-v2) as the command framework is reimplemented in Python.
     - During competition season, we will not merge any new feature additions. We want to ensure that the API is stable during the season to help minimize issues for teams.
     - Ask about large changes before spending a bunch of time on them! You can create a new issue on our GitHub tracker for feature request/discussion and talk about it with us there.
     - Features that make it easier for teams with less experience to be more successful are more likely to be accepted.
@@ -37,12 +38,42 @@ So you want to contribute your changes back to WPILib. Great! We have a few cont
 
 ## Coding Guidelines
 
-WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system. We currently use clang-format 10.0 with wpiformat.
-
+WPILib uses modified Google style guides for both C++ and Java, which can be found in the [styleguide repository](https://github.com/wpilibsuite/styleguide). Autoformatters are available for many popular editors at https://github.com/google/styleguide. Running wpiformat is required for all contributions and is enforced by our continuous integration system.
 While the library should be fully formatted according to the styles, additional elements of the style guide were not followed when the library was initially created. All new code should follow the guidelines. If you are looking for some easy ramp-up tasks, finding areas that don't follow the style guide and fixing them is very welcome.
 
+### Math documentation
+
 When writing math expressions in documentation, use https://www.unicodeit.net/ to convert LaTeX to a Unicode equivalent that's easier to read. Not all expressions will translate (e.g., superscripts of superscripts) so focus on making it readable by someone who isn't familiar with LaTeX. If content on multiple lines needs to be aligned in Doxygen/Javadoc comments (e.g., integration/summation limits, matrices packed with square brackets and superscripts for them), put them in @verbatim/@endverbatim blocks in Doxygen or `<pre>` tags in Javadoc so they render with monospace font.
 
+The LaTeX to Unicode conversions can also be done locally via the unicodeit Python package. To install it, execute:
+```bash
+pip install --user unicodeit
+```
+
+Here's example usage:
+```bash
+$ python -m unicodeit.cli 'x_{k+1} = Ax_k + Bu_k'
+xₖ₊₁ = Axₖ + Buₖ
+```
+
+On Linux, this process can be streamlined further by adding the following Bash function to your .bashrc (requires `wl-clipboard` on Wayland or `xclip` on X11):
+```bash
+# Converts LaTeX to Unicode, prints the result, and copies it to the clipboard
+uc() {
+  if [ $WAYLAND_DISPLAY ]; then
+    python -m unicodeit.cli $@ | tee >(wl-copy -n)
+  else
+    python -m unicodeit.cli $@ | tee >(xclip -sel)
+  fi
+}
+```
+
+Here's example usage:
+```bash
+$ uc 'x_{k+1} = Ax_k + Bu_k'
+xₖ₊₁ = Axₖ + Buₖ
+```
+
 ## Submitting Changes
 
 ### Pull Request Format

DevelopmentBuilds.md

@@ -0,0 +1,103 @@
+# Installing Development Builds
+
+This article contains instructions on building projects using a development build and a local WPILib build.
+
+**Note:** This only applies to Java/C++ teams.
+
+> [!WARNING]
+> **There are no stability or compatibility guarantees for builds outside of [tagged releases](https://github.com/wpilibsuite/allwpilib/releases). Changes may not be fully documented. Use them at your own risk!**
+>
+> Development builds may be non-functional between the end of the season and the start of beta testing. Development builds are also likely to be incompatible with vendor libraries during this time.
+
+## Development Build
+
+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).
+
+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 2024 GradleRIO version, ie `2024.0.0-alpha-1`
+
+```groovy
+wpi.maven.useLocal = false
+wpi.maven.useDevelopment = true
+wpi.versions.wpilibVersion = 'YEAR.+'
+wpi.versions.wpimathVersion = 'YEAR.+
+```
+
+The top of your ``build.gradle`` file should now look similar to the code below. Ignore any differences in versions.
+
+Java
+```groovy
+plugins {
+  id "java"
+  id "edu.wpi.first.GradleRIO" version "2024.0.0-alpha-1"
+}
+
+wpi.maven.useLocal = false
+wpi.maven.useDevelopment = true
+wpi.versions.wpilibVersion = '2024.+'
+wpi.versions.wpimathVersion = '2024.+'
+```
+
+C++
+```groovy
+plugins {
+  id "cpp"
+  id "google-test-test-suite"
+  id "edu.wpi.first.GradleRIO" version "2024.0.0-alpha-1"
+}
+
+wpi.maven.useLocal = false
+wpi.maven.useDevelopment = true
+wpi.versions.wpilibVersion = '2024.+'
+wpi.versions.wpimathVersion = '2024.+'
+```
+
+### 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.
+
+Java
+```groovy
+plugins {
+  id "java"
+  id "edu.wpi.first.GradleRIO" version "2024.0.0-alpha-1"
+}
+
+wpi.maven.useLocal = false
+wpi.maven.useFrcMavenLocalDevelopment = true
+wpi.versions.wpilibVersion = 'YEAR.424242.+'
+wpi.versions.wpimathVersion = 'YEAR.424242.+'
+```
+
+C++
+```groovy
+plugins {
+  id "cpp"
+  id "google-test-test-suite"
+  id "edu.wpi.first.GradleRIO" version "2024.0.0-alpha-1"
+}
+
+wpi.maven.useLocal = false
+wpi.maven.useFrcMavenLocalDevelopment = true
+wpi.versions.wpilibVersion = 'YEAR.424242.+'
+wpi.versions.wpimathVersion = 'YEAR.424242.+'
+```
+
+# roboRIO Development
+
+This repo contains a myRobot project built in way to do full project development without needing to do a full publish into GradleRIO. These also only require building the minimum amount of binaries for the roboRIO, so the builds are much faster as well.
+
+The setup only works if the roboRIO is USB connected. If an alternate IP address is preferred, the `address` block in myRobot\build.gradle can be changed to point to another address.
+
+The following 3 tasks can be used for deployment:
+* `:myRobot:deployShared` deploys the C++ project using shared dependencies. Prefer this one for most C++ development.
+* `:myRobot:deployStatic` deploys the C++ project with all dependencies statically linked.
+* `:myRobot:deployJava` deploys the Java project and all required dependencies. Also installs the JRE if not currently installed.
+
+Deploying any of these to the roboRIO will disable the current startup project until it is redeployed.
+
+From here, ssh into the roboRIO using the `lvuser` account and run `frcRunRobot.sh` (It's in path).

LICENSE.md

@@ -1,4 +1,4 @@
-Copyright (c) 2009-2022 FIRST and other WPILib contributors
+Copyright (c) 2009-2024 FIRST and other WPILib contributors
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

MAINTAINERS.md

@@ -1,5 +1,5 @@
 ## Publishing Third Party Dependencies
-Currently the 3rd party deps are imgui, opencv, and google test
+Currently the 3rd party deps are imgui, opencv, google test, libssh, and apriltaglib
 
 For publishing these dependencies, the version needs to be manually updated in the publish.gradle file of their respective repository.
 Then, in the azure build for the dependency you want to build for, manually start a pipeline build (As of current, this is the `Run Pipeline` button).
@@ -24,4 +24,4 @@ Upon pushing a tag, a release will be built, and the files will be uploaded to t
 Before publishing, make sure to update the version in build.gradle. Publishing must happen locally, using the command `./gradlew publishPlugin`. This does require your API key for publishing to be set.
 
 ## Building the installer
-Update the GradleRIO version in gradle.properties, and in the scripts folder in vscode, update the vscode extension. Then push, it will build the installer on azure.
+Update the GradleRIO version in gradle.properties, and in the scripts folder in vscode, update the vscode extension. To publish a release build, upload a new tag, and a release will automatically be built and published to artifactory and cloudflare.

MavenArtifacts.md

@@ -16,7 +16,7 @@ We provide two base types of artifacts.
 
 The first types are Java artifacts. These are usually published as `jar` files. Usually, the actual jar file is published with no classifier. The sources are published with the `-sources` classifier, and the javadocs are published with the `-javadoc` classifier.
 
-The second types are native artifacts. These are usually published as `zip` files (except for the `JNI` artifact types, which are `jar` files. See below for information on this). The `-sources` and `-headers` classifiers contain the sources and headers respecively for the library. Each artifact also contains a classifier for each platform we publish. This platform is in the format `{os}{arch}`. The platform artifact only contains the binaries for a specific platform. In addition, we provide a `-all` classifier. This classifer combines all of the platform artifacts into a single artifact. This is useful for tools that cannot determine what version to use during builds. However, we recommend using the platform specific classifier when possible. Note that the binary artifacts never contain the headers, you always need the `-headers` classifier to get those.
+The second types are native artifacts. These are usually published as `zip` files (except for the `JNI` artifact types, which are `jar` files. See below for information on this). The `-sources` and `-headers` classifiers contain the sources and headers respectively for the library. Each artifact also contains a classifier for each platform we publish. This platform is in the format `{os}{arch}`. The platform artifact only contains the binaries for a specific platform. In addition, we provide a `-all` classifier. This classifier combines all of the platform artifacts into a single artifact. This is useful for tools that cannot determine what version to use during builds. However, we recommend using the platform specific classifier when possible. Note that the binary artifacts never contain the headers, you always need the `-headers` classifier to get those.
 
 ## Artifact Names
 
@@ -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,16 +142,14 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository.
   * ntcore
   * cscore
   * wpimath
+  * wpinet
   * wpiutil
 
-* wpilibOldCommands
-  * wpilibc
-  * hal
-  * cameraserver
-  * ntcore
-  * cscore
-  * wpiutil
+* wpiunits
 
+* apriltag
+  * wpiutil
+  * wpimath
 
 ### Third Party Artifacts
 
@@ -149,6 +157,7 @@ This repository provides the builds of the following third party software.
 
 All artifacts are based at `edu.wpi.first.thirdparty.frcYEAR` in the repository.
 
+* apriltaglib
 * googletest
 * imgui
 * opencv

OtherVersions.md

@@ -1,93 +0,0 @@
-# Installing Development Builds
-
-This article contains instructions on building projects using a development build and a local WPILib build.
-
-**Note:** This only applies to Java/C++ teams.
-
-## Development Build
-
-Development builds are the per-commit build hosted everytime 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.
-
-```groovy
-wpi.maven.useLocal = false
-wpi.maven.useDevelopment = true
-wpi.versions.wpilibVersion = 'YEAR.+'
-wpi.versions.wpimathVersion = 'YEAR.+
-```
-
-The top of your ``build.gradle`` file should now look similar to the code below. Ignore any differences in versions.
-
-Java
-```groovy
-plugins {
-  id "java"
-  id "edu.wpi.first.GradleRIO" version "2022.1.1"
-}
-
-wpi.maven.useLocal = false
-wpi.maven.useDevelopment = true
-wpi.versions.wpilibVersion = '2022.+'
-wpi.versions.wpimathVersion = '2022.+'
-```
-
-C++
-```groovy
-plugins {
-  id "cpp"
-  id "google-test-test-suite"
-  id "edu.wpi.first.GradleRIO" version "2022.1.1"
-}
-
-wpi.maven.useLocal = false
-wpi.maven.useDevelopment = true
-wpi.versions.wpilibVersion = '2022.+'
-wpi.versions.wpimathVersion = '2022.+'
-```
-
-## 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.
-
-Java
-```groovy
-plugins {
-  id "java"
-  id "edu.wpi.first.GradleRIO" version "2022.1.1"
-}
-
-wpi.maven.useLocal = false
-wpi.maven.useFrcMavenLocalDevelopment = true
-wpi.versions.wpilibVersion = 'YEAR.424242.+'
-wpi.versions.wpimathVersion = 'YEAR.424242.+'
-```
-
-C++
-```groovy
-plugins {
-  id "cpp"
-  id "google-test-test-suite"
-  id "edu.wpi.first.GradleRIO" version "2022.1.1"
-}
-
-wpi.maven.useLocal = false
-wpi.maven.useFrcMavenLocalDevelopment = true
-wpi.versions.wpilibVersion = 'YEAR.424242.+'
-wpi.versions.wpimathVersion = 'YEAR.424242.+'
-```
-
-# roboRIO Development
-
-This repo contains a myRobot project built in way to do full project development without needing to do a full publish into GradleRIO. These also only require building the minimum amount of binaries for the roboRIO, so the builds are much faster as well.
-
-The setup only works if the roboRIO is USB connected. If an alternate IP address is preferred, the `address` block in myRobot\build.gradle can be changed to point to another address.
-
-The following 3 tasks can be used for deployment:
-* `:myRobot:deployShared` deploys the C++ project using shared dependencies. Prefer this one for most C++ development.
-* `:myRobot:deployStatic` deploys the C++ project with all dependencies statically linked.
-* `:myRobot:deployJava` deploys the Java project and all required dependencies. Also installs the JRE if not currently installed.
-
-Deploying any of these to the roboRIO will disable the current startup project until it is redeployed.
-
-From here, ssh into the roboRIO using the `admin` account (`lvuser` will fail to run in many cases). In the admin home directory, a file for each deploy type will exist (`myRobotCpp`, `myRobotCppStatic` and `myRobotJavaRun`). These can be run to start up the corresponding project.

README-CMAKE.md

@@ -12,14 +12,20 @@ WPILib is normally built with Gradle, however for some systems, such as Linux ba
 * halsim
 * wpigui
 * wpimath
+* wpiunits
+* wpilibNewCommands
 
 By default, all libraries except for the HAL and WPILib get built with a default CMake setup. The libraries are built as shared libraries, and include the JNI libraries as well as building the Java JARs.
 
 ## Prerequisites
 
-The most common prerequisite is going to be OpenCV. OpenCV needs to be findable by CMake. On systems like the Jetson, this is installed by default. Otherwise, you will need to build OpenCV from source and install it.
+The jinja2 pip package is needed to generate classes for NT4's pubsub.
 
-In addition, if you want JNI and Java, you will need a JDK of at least version 11 installed. In addition, you need a `JAVA_HOME` environment variable set properly and set to the JDK directory.
+The protobuf library and compiler are needed for protobuf generation. The QuickBuffers protoc-gen package is also required when Java is being built; this can be obtained from https://github.com/HebiRobotics/QuickBuffers/releases/.
+
+OpenCV needs to be findable by CMake. On systems like the Jetson, this is installed by default. Otherwise, you will need to build OpenCV from source and install it.
+
+If you want JNI and Java, you will need a JDK of at least version 11 installed. In addition, you need a `JAVA_HOME` environment variable set properly and set to the JDK directory.
 
 If you are building with unit tests or simulation modules, you will also need an Internet connection for the initial setup process, as CMake will clone google-test and imgui from GitHub.
 
@@ -29,16 +35,26 @@ The following build options are available:
 
 * `WITH_JAVA` (ON Default)
   * This option will enable Java and JNI builds. If this is on, `WITH_SHARED_LIBS` must be on. Otherwise CMake will error.
+* `WITH_JAVA_SOURCE` (ON Default)
+  * This option will build Java source JARs for each enabled Java library. This does not require `WITH_JAVA` to be on, allowing source JARs to be built without the compiled JARs if desired.
 * `WITH_SHARED_LIBS` (ON Default)
   * This option will cause cmake to build static libraries instead of shared libraries. If this is off, `WITH_JAVA` must be off. Otherwise CMake will error.
-* `WITH_TESTS` (ON Default)
-  * This option will build C++ unit tests. These can be run via `make test`.
 * `WITH_CSCORE` (ON Default)
   * This option will cause cscore to be built. Turning this off will implicitly disable cameraserver, the hal and wpilib as well, irrespective of their specific options. If this is off, the OpenCV build requirement is removed.
+* `WITH_NTCORE` (ON Default)
+  * This option will cause ntcore to be built. Turning this off will implicitly disable wpinet and wpilib as well, irrespective of their specific options.
 * `WITH_WPIMATH` (ON Default)
   * This option will build the wpimath library. This option must be on to build wpilib.
+* `WITH_WPIUNITS` (ON Default)
+  * This option will build the wpiunits library. This option must be on to build the Java wpimath library and requires `WITH_JAVA` to also be on.
 * `WITH_WPILIB` (ON Default)
   * This option will build the hal and wpilibc/j during the build. The HAL is the simulator hal, unless the external hal options are used. The cmake build has no capability to build for the RoboRIO.
+* `WITH_EXAMPLES` (ON Default)
+  * This option will build C++ examples.
+* `WITH_TESTS` (ON Default)
+  * This option will build C++ unit tests. These can be run via `make test`.
+* `WITH_GUI` (ON Default)
+  * This option will build GUI items.
 * `WITH_SIMULATION_MODULES` (ON Default)
   * This option will build simulation modules, including wpigui and the HALSim plugins.
 * `WITH_EXTERNAL_HAL` (OFF Default)
@@ -47,6 +63,8 @@ The following build options are available:
   * TODO
 * `OPENCV_JAVA_INSTALL_DIR`
   * Set this option to the location of the archive of the OpenCV Java bindings (it should be called opencv-xxx.jar, with the x'es being version numbers). NOTE: set it to the LOCATION of the file, not the file itself!
+* `NO_WERROR` (OFF Default)
+  * This option will disable the `-Werror` compilation flag for non-MSVC builds.
 
 ## Build Setup
 
@@ -87,7 +105,7 @@ Using the libraries from C++ is the easiest way to use the built libraries.
 To do so, create a new folder to contain your project. Add the following code below to a `CMakeLists.txt` file in that directory.
 
 ```
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.11)
 project(vision_app) # Project Name Here
 
 find_package(wpilib REQUIRED)

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,9 +15,9 @@ 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 with wpiformat](#formattinglinting-with-wpiformat)
+    - [Formatting/Linting](#formattinglinting)
     - [CMake](#cmake)
+  - [Running examples in simulation](#running-examples-in-simulation)
   - [Publishing](#publishing)
   - [Structure and Organization](#structure-and-organization)
 - [Contributing to WPILib](#contributing-to-wpilib)
@@ -26,26 +26,37 @@ Welcome to the WPILib project. This repository contains the HAL, WPILibJ, and WP
 
 The WPILib Mission is to enable FIRST Robotics teams to focus on writing game-specific software rather than focusing on hardware details - "raise the floor, don't lower the ceiling". We work to enable teams with limited programming knowledge and/or mentor experience to be as successful as possible, while not hampering the abilities of teams with more advanced programming capabilities. We support Kit of Parts control system components directly in the library. We also strive to keep parity between major features of each language (Java, C++, and NI's LabVIEW), so that teams aren't at a disadvantage for choosing a specific programming language. WPILib is an open source project, licensed under the BSD 3-clause license. You can find a copy of the license [here](LICENSE.md).
 
+# Quick Start
+
+Below is a list of instructions that guide you through cloning, building, publishing and using local allwpilib binaries in a robot project. This quick start is not intended as a replacement for the information further listed in this document.
+
+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](DevelopmentBuilds.md) `build.gradle` [to use the artifacts](DevelopmentBuilds.md)
+
 # Building WPILib
 
 Using Gradle makes building WPILib very straightforward. It only has a few dependencies on outside tools, such as the ARM cross compiler for creating roboRIO binaries.
 
 ## Requirements
 
-- [JDK 11](https://adoptopenjdk.net/)
+- [JDK 11](https://adoptium.net/temurin/releases/?version=11)
     - Note that the JRE is insufficient; the full JDK is required
     - On Ubuntu, run `sudo apt install openjdk-11-jdk`
     - On Windows, install the JDK 11 .msi from the link above
     - On macOS, install the JDK 11 .pkg from the link above
 - C++ compiler
-    - On Linux, install GCC 8 or greater
-    - On Windows, install [Visual Studio Community 2022 or 2019](https://visualstudio.microsoft.com/vs/community/) and select the C++ programming language during installation (Gradle can't use the build tools for Visual Studio)
-    - On macOS, install the Xcode command-line build tools via `xcode-select --install`
+    - On Linux, install GCC 11 or greater
+    - On Windows, install [Visual Studio Community 2022](https://visualstudio.microsoft.com/vs/community/) and select the C++ programming language during installation (Gradle can't use the build tools for Visual Studio)
+    - On macOS, install the Xcode command-line build tools via `xcode-select --install`. Xcode 13 or later is required.
 - ARM compiler toolchain
     - Run `./gradlew installRoboRioToolchain` after cloning this repository
     - If the WPILib installer was used, this toolchain is already installed
 - Raspberry Pi toolchain (optional)
-    - Run `./gradlew installRaspbianToolchain` after cloning this repository
+    - Run `./gradlew installArm32Toolchain` after cloning this repository
+
+On macOS ARM, run `softwareupdate --install-rosetta`. This is necessary to be able to use the macOS x86 roboRIO toolchain on ARM.
 
 ## Setup
 
@@ -77,15 +88,38 @@ If opening from a fresh clone, generated java dependencies will not exist. Most
 
 `./gradlew build` builds _everything_, which includes debug and release builds for desktop and all installed cross compilers. Many developers don't need or want to build all of this. Therefore, common tasks have shortcuts to only build necessary components for common development and testing tasks.
 
-`./gradlew testDesktopCpp` and `./gradlew testDesktopJava` will build and run the tests for `wpilibc` and `wpilibj` respectively. They will only build the minimum components required to run the tests.
+`./gradlew testDesktopCpp` and `./gradlew testDesktopJava` will build and run the tests for `wpilibc` and `wpilibj` respectively. They will only build the minimum components required to run the tests. `./gradlew testDesktop` will run both `testDesktopJava` and `testDesktopCpp`.
 
-`testDesktopCpp` and `testDesktopJava` tasks also exist for the projects `wpiutil`, `ntcore`, `cscore`, `hal` `wpilibOldCommands`, `wpilibNewCommands` and `cameraserver`. These can be ran with `./gradlew :projectName:task`.
+`testDesktopCpp`, `testDesktopJava`, and `testDesktop` tasks also exist for the following projects:
+
+- `apriltag`
+- `cameraserver`
+- `cscore`
+- `hal`
+- `ntcore`
+- `wpilibNewCommands`
+- `wpimath`
+- `wpinet`
+- `wpiunits`
+- `wpiutil`
+- `romiVendordep`
+- `xrpVendordep`
+
+These can be ran with `./gradlew :projectName:task`.
 
 `./gradlew buildDesktopCpp` and `./gradlew buildDesktopJava` will compile `wpilibcExamples` and `wpilibjExamples` respectively. The results can't be ran, but they can compile.
 
+### Build Cache
+
+Run with `--build-cache` on the command-line to use the shared [build cache](https://docs.gradle.org/current/userguide/build_cache.html) artifacts generated by the continuous integration server. Example:
+
+```bash
+./gradlew build --build-cache
+```
+
 ### Using Development Builds
 
-Please read the documentation available [here](OtherVersions.md)
+Please read the documentation available [here](DevelopmentBuilds.md)
 
 ### Custom toolchain location
 
@@ -95,27 +129,10 @@ 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` on Windows or `python3 -m wpiformat` on other platforms.
@@ -130,6 +147,16 @@ If you only want to run the Java autoformatter, run `./gradlew spotlessApply`.
 
 CMake is also supported for building. See [README-CMAKE.md](README-CMAKE.md).
 
+## Running examples in simulation
+
+Examples can be run in simulation with the following command:
+
+```bash
+./gradlew wpilibcExamples:runExample
+./gradlew wpilibjExamples:runExample
+```
+where `Example` is the example's folder name.
+
 ## Publishing
 
 If you are building to test with other dependencies or just want to export the build as a Maven-style dependency, simply run the `publish` task. This task will publish all available packages to ~/releases/maven/development. If you need to publish the project to a different repo, you can specify it with `-Prepo=repo_name`. Valid options are:
@@ -143,13 +170,11 @@ 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.
 
-The hal directory contains more C++ code meant to run on the roboRIO. HAL is an acronym for "Hardware Abstraction Layer", and it interfaces with the NI Libraries. The NI Libraries contain the low-level code for controlling devices on your robot. The NI Libraries are found in the ni-libraries folder.
+The hal directory contains more C++ code meant to run on the roboRIO. HAL is an acronym for "Hardware Abstraction Layer", and it interfaces with the NI Libraries. The NI Libraries contain the low-level code for controlling devices on your robot. The NI Libraries are found in the [ni-libraries](https://github.com/wpilibsuite/ni-libraries) project.
 
 The upstream_utils directory contains scripts for updating copies of thirdparty code in the repository.
 

ThirdPartyNotices.txt

@@ -17,44 +17,32 @@ Program               Locations
 -------               ---------
 RoboRIO Libraries     ni-libraries
 Google Test           gtest
-LLVM                  wpiutil/src/main/native/include/wpi/{various files}
-                      wpiutil/src/main/native/cpp/llvm/
-                      wpiutil/src/main/native/cpp/leb128.cpp
-                      wpiutil/src/test/native/cpp/leb128Test.cpp
-JSON for Modern C++   wpiutil/src/main/native/include/wpi/json.h
-                      wpiutil/src/main/native/cpp/json_*.cpp
+LLVM                  wpiutil/src/main/native/thirdparty/llvm
+                      wpiutil/src/test/native/cpp/llvm/
+JSON for Modern C++   wpiutil/src/main/native/thirdparty/json
                       wpiutil/src/test/native/cpp/json/
-libuv                 wpiutil/src/main/native/include/uv.h
-                      wpiutil/src/main/native/include/uv/
-                      wpiutil/src/main/native/libuv/
-fmtlib                wpiutil/src/main/native/fmtlib/
-sigslot               wpiutil/src/main/native/include/wpi/Signal.h
-                      wpiutil/src/test/native/cpp/sigslot/
-tcpsockets            wpiutil/src/main/native/cpp/TCP{Stream,Connector,Acceptor}.cpp
-                      wpiutil/src/main/native/include/wpi/TCP*.h
-MPack                 wpiutil/src/main/native/include/mpack.h
-                      wpiutil/src/main/native/cpp/mpack.cpp
-Bootstrap             wpiutil/src/main/native/resources/bootstrap-*
-CoreUI                wpiutil/src/main/native/resources/coreui-*
-Feather Icons         wpiutil/src/main/native/resources/feather-*
-jQuery                wpiutil/src/main/native/resources/jquery-*
-popper.js             wpiutil/src/main/native/resources/popper-*
+libuv                 wpinet/src/main/native/thirdparty/libuv/
+fmtlib                wpiutil/src/main/native/thirdparty/fmtlib/
+sigslot               wpiutil/src/main/native/thirdparty/sigslot
+tcpsockets            wpinet/src/main/native/thirdparty/tcpsockets
+MPack                 wpiutil/src/main/native/thirdparty/mpack
+Bootstrap             wpinet/src/main/native/resources/bootstrap-*
+CoreUI                wpinet/src/main/native/resources/coreui-*
+Feather Icons         wpinet/src/main/native/resources/feather-*
+jQuery                wpinet/src/main/native/resources/jquery-*
+popper.js             wpinet/src/main/native/resources/popper-*
 units                 wpimath/src/main/native/include/units/
-Eigen                 wpimath/src/main/native/eigeninclude/
-                      wpimath/src/main/native/include/unsupported/
+Eigen                 wpimath/src/main/native/thirdparty/eigen/include/
 StackWalker           wpiutil/src/main/native/windows/StackWalker.*
-TCB span              wpiutil/src/main/native/include/wpi/span.h
-                      wpiutil/src/test/native/cpp/span/
-GHC filesystem        wpiutil/src/main/native/include/wpi/ghc/
+GHC filesystem        wpiutil/src/main/native/thirdparty/include/wpi/ghc/
 Team 254 Library      wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
                       wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
                       wpilibc/src/main/native/include/spline/SplineParameterizer.h
                       wpilibc/src/main/native/include/trajectory/TrajectoryParameterizer.h
                       wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
 Portable File Dialogs wpigui/src/main/native/include/portable-file-dialogs.h
-Drake                 wpimath/src/main/native/cpp/drake/common/drake_assert_and_throw.cpp
-                      wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
-
+V8 export-template    wpiutil/src/main/native/include/wpi/SymbolExports.h
+GCEM                  wpimath/src/main/native/thirdparty/gcem/include/
 
 ==============================================================================
 Google Test License
@@ -90,12 +78,247 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 
 ==============================================================================
-LLVM Release License
+The LLVM Project is under the Apache License v2.0 with LLVM Exceptions:
+==============================================================================
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+    TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+    1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+    2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+    3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+    4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+    5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+    6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+    7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+    8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+    9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+    END OF TERMS AND CONDITIONS
+
+    APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+    Copyright [yyyy] [name of copyright owner]
+
+    Licensed under the Apache License, Version 2.0 (the "License");
+    you may not use this file except in compliance with the License.
+    You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+    Unless required by applicable law or agreed to in writing, software
+    distributed under the License is distributed on an "AS IS" BASIS,
+    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+    See the License for the specific language governing permissions and
+    limitations under the License.
+
+
+---- LLVM Exceptions to the Apache 2.0 License ----
+
+As an exception, if, as a result of your compiling your source code, portions
+of this Software are embedded into an Object form of such source code, you
+may redistribute such embedded portions in such Object form without complying
+with the conditions of Sections 4(a), 4(b) and 4(d) of the License.
+
+In addition, if you combine or link compiled forms of this Software with
+software that is licensed under the GPLv2 ("Combined Software") and if a
+court of competent jurisdiction determines that the patent provision (Section
+3), the indemnity provision (Section 9) or other Section of the License
+conflicts with the conditions of the GPLv2, you may retroactively and
+prospectively choose to deem waived or otherwise exclude such Section(s) of
+the License, but only in their entirety and only with respect to the Combined
+Software.
+
+==============================================================================
+Software from third parties included in the LLVM Project:
+==============================================================================
+The LLVM Project contains third party software which is under different license
+terms. All such code will be identified clearly using at least one of two
+mechanisms:
+1) It will be in a separate directory tree with its own `LICENSE.txt` or
+   `LICENSE` file at the top containing the specific license and restrictions
+   which apply to that software, or
+2) It will contain specific license and restriction terms at the top of every
+   file.
+
+==============================================================================
+Legacy LLVM License (https://llvm.org/docs/DeveloperPolicy.html#legacy):
 ==============================================================================
 University of Illinois/NCSA
 Open Source License
 
-Copyright (c) 2003-2017 University of Illinois at Urbana-Champaign.
+Copyright (c) 2003-2019 University of Illinois at Urbana-Champaign.
 All rights reserved.
 
 Developed by:
@@ -843,41 +1066,6 @@ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.
 
-=============
-Drake Library
-=============
-All components of Drake are licensed under the BSD 3-Clause License
-shown below. Where noted in the source code, some portions may
-be subject to other permissive, non-viral licenses.
-
-Copyright 2012-2016 Robot Locomotion Group @ CSAIL
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are
-met:
-
-Redistributions of source code must retain the above copyright notice,
-this list of conditions and the following disclaimer.  Redistributions
-in binary form must reproduce the above copyright notice, this list of
-conditions and the following disclaimer in the documentation and/or
-other materials provided with the distribution.  Neither the name of
-the Massachusetts Institute of Technology nor the names of its
-contributors may be used to endorse or promote products derived from
-this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
-A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
-HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
-SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
-LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
-DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
-THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
-OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
 =====================
 Portable File Dialogs
 =====================
@@ -969,3 +1157,55 @@ AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.
+
+==================
+V8 export-template
+==================
+Copyright 2014, the V8 project authors. All rights reserved.
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are
+met:
+
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above
+      copyright notice, this list of conditions and the following
+      disclaimer in the documentation and/or other materials provided
+      with the distribution.
+    * Neither the name of Google Inc. nor the names of its
+      contributors may be used to endorse or promote products derived
+      from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+============
+GCEM License
+============
+Copyright 2022 - ktholer (https://github.com/kthohr/gcem)
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+================
+2024 Field Image
+================
+2024 Field Image from MikLast: https://www.chiefdelphi.com/t/2024-crescendo-top-down-field-renders/447764

apriltag/CMakeLists.txt

@@ -0,0 +1,150 @@
+project(apriltag)
+
+include(CompileWarnings)
+include(GenResources)
+include(FetchContent)
+
+fetchcontent_declare(
+    apriltaglib
+    GIT_REPOSITORY https://github.com/wpilibsuite/apriltag.git
+    GIT_TAG 64be6ab26abf5e995321997fd0752c609a7e30f4
+)
+
+# 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
+        RELATIVE ${CMAKE_CURRENT_SOURCE_DIR}
+        src/main/native/resources/*.json
+    )
+    add_jar(
+        apriltag_jar
+        SOURCES ${JAVA_SOURCES}
+        RESOURCES
+        NAMESPACE "edu/wpi/first/apriltag" ${JAVA_RESOURCES}
+        INCLUDE_JARS wpimath_jar wpiunits_jar ${EJML_JARS} wpiutil_jar ${OPENCV_JAR_FILE}
+        OUTPUT_NAME apriltag
+        GENERATE_NATIVE_HEADERS apriltag_jni_headers
+    )
+
+    install_jar(apriltag_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(TARGETS apriltag_jar FILE apriltag_jar.cmake DESTINATION share/apriltag)
+
+    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)
+
+    install(TARGETS apriltagjni EXPORT apriltagjni)
+endif()
+
+if(WITH_JAVA_SOURCE)
+    find_package(Java REQUIRED)
+    include(UseJava)
+    file(GLOB APRILTAG_SOURCES src/main/java/edu/wpi/first/apriltag/*.java)
+    add_jar(
+        apriltag_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/apriltag" ${APRILTAG_SOURCES}
+        NAMESPACE
+            "edu/wpi/first/apriltag/jni"
+            src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java
+        OUTPUT_NAME apriltag-sources
+    )
+
+    get_property(APRILTAG_SRC_JAR_FILE TARGET apriltag_src_jar PROPERTY JAR_FILE)
+    install(FILES ${APRILTAG_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
+
+    set_property(TARGET apriltag_src_jar PROPERTY FOLDER "java")
+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 /wd4005 /wd4996)
+else()
+    target_compile_options(
+        apriltag
+        PRIVATE -Wno-sign-compare -Wno-gnu-zero-variadic-macro-arguments -Wno-type-limits
+    )
+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)
+install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/apriltag")
+
+configure_file(apriltag-config.cmake.in ${WPILIB_BINARY_DIR}/apriltag-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/apriltag-config.cmake DESTINATION share/apriltag)
+install(EXPORT apriltag DESTINATION share/apriltag)
+
+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/README.md

@@ -0,0 +1,28 @@
+# AprilTag
+
+## Adding new field to AprilTagFields
+
+### Adding field JSON
+
+1. Add a field layout CSV file to `src/main/native/resources/edu/wpi/first/apriltag`
+    1. See docstring in `convert_apriltag_layouts.py` for more
+2. Run `convert_apriltag_layouts.py` in the same directory as this readme to generate the JSON
+3. That script overwrites all generated JSONs, so undo undesired changes if necessary
+4. Update the field dimensions at the bottom of the JSON
+    1. Length should be in meters from alliance wall to alliance wall
+    2. Width should be in meters from inside guardrail plastic to plastic
+
+### Java updates
+
+1. Update `src/main/java/edu/wpi/first/apriltag/AprilTagFields.java`
+    1. Add enum value for new field to `AprilTagFields`
+    2. Update `AprilTagFields.kDefaultField` if necessary
+
+### C++ updates
+
+1. Update `src/main/native/include/frc/apriltag/AprilTagFields.h`
+    1. Add enum value for new field to `AprilTagFields`
+    2. Update `AprilTagFields::kDefaultField` if necessary
+2. Update `src/main/native/cpp/AprilTagFields.cpp`
+    1. Add resource getter prototype like `std::string_view GetResource_2024_crescendo_json()`
+    2. Add case for new field to switch in `LoadAprilTagLayoutField()`

apriltag/apriltag-config.cmake.in

@@ -0,0 +1,10 @@
+include(CMakeFindDependencyMacro)
+@FILENAME_DEP_REPLACE@
+@WPIMATH_DEP_REPLACE@
+@WPIUTIL_DEP_REPLACE@
+
+@FILENAME_DEP_REPLACE@
+include(${SELF_DIR}/apriltag.cmake)
+if(@WITH_JAVA@)
+    include(${SELF_DIR}/apriltag_jar.cmake)
+endif()

apriltag/build.gradle

@@ -0,0 +1,87 @@
+apply from: "${rootDir}/shared/resources.gradle"
+
+ext {
+    nativeName = 'apriltag'
+    devMain = 'edu.wpi.first.apriltag.DevMain'
+    useJava = true
+    useCpp = true
+    sharedCvConfigs = [
+        apriltagDev : [],
+        apriltagTest: []]
+    staticCvConfigs = []
+
+    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')
+}
+
+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/convert_apriltag_layouts.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+
+"""
+This script converts all AprilTag field layout CSV files in
+src/main/native/resources/edu/wpi/first/apriltag to the JSON format
+AprilTagFields expects.
+
+The input CSV has the following format:
+
+* Columns: ID, X, Y, Z, Rotation
+* ID is a positive integer
+* X, Y, and Z are decimal inches
+* Rotation is yaw in degrees
+
+The values come from a table in the layout marking diagram (e.g.,
+https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf).
+"""
+
+import csv
+import json
+import os
+
+from wpimath import geometry, units
+import numpy as np
+
+
+def main():
+    # Find AprilTag field layout CSVs
+    filenames = [
+        os.path.join(dp, f)
+        for dp, dn, fn in os.walk("src/main/native/resources/edu/wpi/first/apriltag")
+        for f in fn
+        if f.endswith(".csv")
+    ]
+
+    for filename in filenames:
+        json_data = {"tags": [], "field": {"length": 0.0, "width": 0.0}}
+
+        # Read CSV and fill in JSON data
+        with open(filename, newline="") as csvfile:
+            reader = csv.reader(csvfile, delimiter=",")
+
+            # Skip header
+            next(reader)
+
+            for row in reader:
+                # Unpack row elements
+                id = int(row[0])
+                x = float(row[1])
+                y = float(row[2])
+                z = float(row[3])
+                rotation = float(row[4])
+
+                # Turn yaw into quaternion
+                q = geometry.Rotation3d(
+                    units.radians(0.0),
+                    units.radians(0.0),
+                    units.degreesToRadians(rotation),
+                ).getQuaternion()
+
+                json_data["tags"].append(
+                    {
+                        "ID": id,
+                        "pose": {
+                            "translation": {
+                                "x": units.inchesToMeters(x),
+                                "y": units.inchesToMeters(y),
+                                "z": units.inchesToMeters(z),
+                            },
+                            "rotation": {
+                                "quaternion": {
+                                    "W": q.W(),
+                                    "X": q.X(),
+                                    "Y": q.Y(),
+                                    "Z": q.Z(),
+                                }
+                            },
+                        },
+                    }
+                )
+
+        # Write JSON
+        with open(filename.replace(".csv", ".json"), "w") as f:
+            json.dump(json_data, f, indent=2)
+            f.write("\n")
+
+
+if __name__ == "__main__":
+    main()

apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.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 final class DevMain {
+  /** Main entry point. */
+  public static void main(String[] args) {
+    System.out.println("Hello World!");
+    AprilTagDetector detector = new AprilTagDetector();
+    detector.addFamily("tag16h5");
+    AprilTagDetector.Config config = new AprilTagDetector.Config();
+    config.refineEdges = false;
+    detector.setConfig(config);
+    detector.close();
+  }
+
+  private DevMain() {}
+}

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

@@ -0,0 +1,11 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/apriltag/AprilTagDetector.h"
+
+int main() {
+  frc::AprilTagDetector detector;
+  detector.AddFamily("tag16h5");
+  detector.SetConfig({.refineEdges = false});
+}

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

@@ -0,0 +1,82 @@
+// 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 com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.apriltag.jni.AprilTagJNI;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.util.RawFrame;
+import java.util.Objects;
+
+/** Represents an AprilTag's metadata. */
+@SuppressWarnings("MemberName")
+public class AprilTag {
+  /** The tag's ID. */
+  @JsonProperty(value = "ID")
+  public int ID;
+
+  /** The tag's pose. */
+  @JsonProperty(value = "pose")
+  public Pose3d pose;
+
+  /**
+   * Constructs an AprilTag.
+   *
+   * @param ID The tag's ID.
+   * @param pose The tag's pose.
+   */
+  @SuppressWarnings("ParameterName")
+  @JsonCreator
+  public AprilTag(
+      @JsonProperty(required = true, value = "ID") int ID,
+      @JsonProperty(required = true, value = "pose") Pose3d pose) {
+    this.ID = ID;
+    this.pose = pose;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof AprilTag) {
+      var other = (AprilTag) obj;
+      return ID == other.ID && pose.equals(other.pose);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(ID, pose);
+  }
+
+  @Override
+  public String toString() {
+    return "AprilTag(ID: " + ID + ", pose: " + pose + ")";
+  }
+
+  /**
+   * Generates a RawFrame containing the apriltag with the id with family 16h5 passed in.
+   *
+   * @param id id
+   * @return A RawFrame containing the AprilTag image
+   */
+  public static RawFrame generate16h5AprilTagImage(int id) {
+    RawFrame frame = new RawFrame();
+    AprilTagJNI.generate16h5AprilTagImage(frame, frame.getNativeObj(), id);
+    return frame;
+  }
+
+  /**
+   * Generates a RawFrame containing the apriltag with the id with family 36h11 passed in.
+   *
+   * @param id id
+   * @return A RawFrame containing the AprilTag image
+   */
+  public static RawFrame generate36h11AprilTagImage(int id) {
+    RawFrame frame = new RawFrame();
+    AprilTagJNI.generate36h11AprilTagImage(frame, frame.getNativeObj(), id);
+    return frame;
+  }
+}

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

@@ -0,0 +1,191 @@
+// 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.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Arrays;
+
+/** A detection of an AprilTag tag. */
+public class AprilTagDetection {
+  /**
+   * Gets the decoded tag's family name.
+   *
+   * @return Decoded family name
+   */
+  public String getFamily() {
+    return m_family;
+  }
+
+  /**
+   * Gets the decoded ID of the tag.
+   *
+   * @return Decoded ID
+   */
+  public int getId() {
+    return m_id;
+  }
+
+  /**
+   * Gets how many error bits were corrected. Note: accepting large numbers of corrected errors
+   * leads to greatly increased false positive rates. NOTE: As of this implementation, the detector
+   * cannot detect tags with a hamming distance greater than 2.
+   *
+   * @return Hamming distance (number of corrected error bits)
+   */
+  public int getHamming() {
+    return m_hamming;
+  }
+
+  /**
+   * Gets a measure of the quality of the binary decoding process: the average difference between
+   * the intensity of a data bit versus the decision threshold. Higher numbers roughly indicate
+   * better decodes. This is a reasonable measure of detection accuracy only for very small tags--
+   * not effective for larger tags (where we could have sampled anywhere within a bit cell and still
+   * gotten a good detection.)
+   *
+   * @return Decision margin
+   */
+  public float getDecisionMargin() {
+    return m_decisionMargin;
+  }
+
+  /**
+   * Gets the 3x3 homography matrix describing the projection from an "ideal" tag (with corners at
+   * (-1,1), (1,1), (1,-1), and (-1, -1)) to pixels in the image.
+   *
+   * @return Homography matrix data
+   */
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
+  public double[] getHomography() {
+    return m_homography;
+  }
+
+  /**
+   * Gets the 3x3 homography matrix describing the projection from an "ideal" tag (with corners at
+   * (-1,1), (1,1), (1,-1), and (-1, -1)) to pixels in the image.
+   *
+   * @return Homography matrix
+   */
+  public Matrix<N3, N3> getHomographyMatrix() {
+    return MatBuilder.fill(Nat.N3(), Nat.N3(), m_homography);
+  }
+
+  /**
+   * Gets the center of the detection in image pixel coordinates.
+   *
+   * @return Center point X coordinate
+   */
+  public double getCenterX() {
+    return m_centerX;
+  }
+
+  /**
+   * Gets the center of the detection in image pixel coordinates.
+   *
+   * @return Center point Y coordinate
+   */
+  public double getCenterY() {
+    return m_centerY;
+  }
+
+  /**
+   * Gets a corner of the tag in image pixel coordinates. These always wrap counter-clock wise
+   * around the tag. Index 0 is the bottom left corner.
+   *
+   * @param ndx Corner index (range is 0-3, inclusive)
+   * @return Corner point X coordinate
+   */
+  public double getCornerX(int ndx) {
+    return m_corners[ndx * 2];
+  }
+
+  /**
+   * Gets a corner of the tag in image pixel coordinates. These always wrap counter-clock wise
+   * around the tag. Index 0 is the bottom left corner.
+   *
+   * @param ndx Corner index (range is 0-3, inclusive)
+   * @return Corner point Y coordinate
+   */
+  public double getCornerY(int ndx) {
+    return m_corners[ndx * 2 + 1];
+  }
+
+  /**
+   * Gets the corners of the tag in image pixel coordinates. These always wrap counter-clock wise
+   * around the tag. The first set of corner coordinates are the coordinates for the bottom left
+   * corner.
+   *
+   * @return Corner point array (X and Y for each corner in order)
+   */
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
+  public double[] getCorners() {
+    return m_corners;
+  }
+
+  private final String m_family;
+  private final int m_id;
+  private final int m_hamming;
+  private final float m_decisionMargin;
+  private final double[] m_homography;
+  private final double m_centerX;
+  private final double m_centerY;
+  private final double[] m_corners;
+
+  /**
+   * Constructs a new detection result. Used from JNI.
+   *
+   * @param family family
+   * @param id id
+   * @param hamming hamming
+   * @param decisionMargin dm
+   * @param homography homography
+   * @param centerX centerX
+   * @param centerY centerY
+   * @param corners corners
+   */
+  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
+  public AprilTagDetection(
+      String family,
+      int id,
+      int hamming,
+      float decisionMargin,
+      double[] homography,
+      double centerX,
+      double centerY,
+      double[] corners) {
+    m_family = family;
+    m_id = id;
+    m_hamming = hamming;
+    m_decisionMargin = decisionMargin;
+    m_homography = homography;
+    m_centerX = centerX;
+    m_centerY = centerY;
+    m_corners = corners;
+  }
+
+  @Override
+  public String toString() {
+    return "DetectionResult [centerX="
+        + m_centerX
+        + ", centerY="
+        + m_centerY
+        + ", corners="
+        + Arrays.toString(m_corners)
+        + ", decisionMargin="
+        + m_decisionMargin
+        + ", hamming="
+        + m_hamming
+        + ", homography="
+        + Arrays.toString(m_homography)
+        + ", family="
+        + m_family
+        + ", id="
+        + m_id
+        + "]";
+  }
+}

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

@@ -0,0 +1,310 @@
+// 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;
+import org.opencv.core.Mat;
+
+/**
+ * An AprilTag detector engine. This is expensive to set up and tear down, so most use cases should
+ * only create one of these, add a family to it, set up any other configuration, and repeatedly call
+ * Detect().
+ */
+public class AprilTagDetector implements AutoCloseable {
+  /** Detector configuration. */
+  @SuppressWarnings("MemberName")
+  public static class Config {
+    /**
+     * How many threads should be used for computation. Default is single-threaded operation (1
+     * thread).
+     */
+    public int numThreads = 1;
+
+    /**
+     * Quad decimation. Detection of quads can be done on a lower-resolution image, improving speed
+     * at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary
+     * payload is still done at full resolution. Default is 2.0.
+     */
+    public float quadDecimate = 2.0f;
+
+    /**
+     * What Gaussian blur should be applied to the segmented image (used for quad detection). Very
+     * noisy images benefit from non-zero values (e.g. 0.8). Default is 0.0.
+     */
+    public float quadSigma;
+
+    /**
+     * When true, the edges of the each quad are adjusted to "snap to" strong gradients nearby. This
+     * is useful when decimation is employed, as it can increase the quality of the initial quad
+     * estimate substantially. Generally recommended to be on (true). Default is true.
+     *
+     * <p>Very computationally inexpensive. Option is ignored if quad_decimate = 1.
+     */
+    public boolean refineEdges = true;
+
+    /**
+     * How much sharpening should be done to decoded images. This can help decode small tags but may
+     * or may not help in odd lighting conditions or low light conditions. Default is 0.25.
+     */
+    public double decodeSharpening = 0.25;
+
+    /**
+     * Debug mode. When true, the decoder writes a variety of debugging images to the current
+     * working directory at various stages through the detection process. This is slow and should
+     * *not* be used on space-limited systems such as the RoboRIO. Default is disabled (false).
+     */
+    public boolean debug;
+
+    /** Default constructor. */
+    public Config() {}
+
+    /**
+     * Constructs a detector configuration.
+     *
+     * @param numThreads How many threads should be used for computation.
+     * @param quadDecimate Quad decimation.
+     * @param quadSigma What Gaussian blur should be applied to the segmented image (used for quad
+     *     detection).
+     * @param refineEdges When true, the edges of the each quad are adjusted to "snap to" strong
+     *     gradients nearby.
+     * @param decodeSharpening How much sharpening should be done to decoded images.
+     * @param debug Debug mode.
+     */
+    Config(
+        int numThreads,
+        float quadDecimate,
+        float quadSigma,
+        boolean refineEdges,
+        double decodeSharpening,
+        boolean debug) {
+      this.numThreads = numThreads;
+      this.quadDecimate = quadDecimate;
+      this.quadSigma = quadSigma;
+      this.refineEdges = refineEdges;
+      this.decodeSharpening = decodeSharpening;
+      this.debug = debug;
+    }
+
+    @Override
+    public int hashCode() {
+      return numThreads
+          + Float.hashCode(quadDecimate)
+          + Float.hashCode(quadSigma)
+          + Boolean.hashCode(refineEdges)
+          + Double.hashCode(decodeSharpening)
+          + Boolean.hashCode(debug);
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+      if (!(obj instanceof Config)) {
+        return false;
+      }
+
+      Config other = (Config) obj;
+      return numThreads == other.numThreads
+          && quadDecimate == other.quadDecimate
+          && quadSigma == other.quadSigma
+          && refineEdges == other.refineEdges
+          && decodeSharpening == other.decodeSharpening
+          && debug == other.debug;
+    }
+  }
+
+  /** Quad threshold parameters. */
+  @SuppressWarnings("MemberName")
+  public static class QuadThresholdParameters {
+    /** Threshold used to reject quads containing too few pixels. Default is 5 pixels. */
+    public int minClusterPixels = 5;
+
+    /**
+     * How many corner candidates to consider when segmenting a group of pixels into a quad. Default
+     * is 10.
+     */
+    public int maxNumMaxima = 10;
+
+    /**
+     * Critical angle, in radians. The detector will reject quads where pairs of edges have angles
+     * that are close to straight or close to 180 degrees. Zero means that no quads are rejected.
+     * Default is 10 degrees.
+     */
+    public double criticalAngle = 10 * Math.PI / 180.0;
+
+    /**
+     * When fitting lines to the contours, the maximum mean squared error allowed. This is useful in
+     * rejecting contours that are far from being quad shaped; rejecting these quads "early" saves
+     * expensive decoding processing. Default is 10.0.
+     */
+    public float maxLineFitMSE = 10.0f;
+
+    /**
+     * Minimum brightness offset. When we build our model of black &amp; white pixels, we add an
+     * extra check that the white model must be (overall) brighter than the black model. How much
+     * brighter? (in pixel values, [0,255]). Default is 5.
+     */
+    public int minWhiteBlackDiff = 5;
+
+    /**
+     * Whether the thresholded image be should be deglitched. Only useful for very noisy images.
+     * Default is disabled (false).
+     */
+    public boolean deglitch;
+
+    /** Default constructor. */
+    public QuadThresholdParameters() {}
+
+    /**
+     * Constructs quad threshold parameters.
+     *
+     * @param minClusterPixels Threshold used to reject quads containing too few pixels.
+     * @param maxNumMaxima How many corner candidates to consider when segmenting a group of pixels
+     *     into a quad.
+     * @param criticalAngle Critical angle, in radians.
+     * @param maxLineFitMSE When fitting lines to the contours, the maximum mean squared error
+     *     allowed.
+     * @param minWhiteBlackDiff Minimum brightness offset.
+     * @param deglitch Whether the thresholded image be should be deglitched.
+     */
+    QuadThresholdParameters(
+        int minClusterPixels,
+        int maxNumMaxima,
+        double criticalAngle,
+        float maxLineFitMSE,
+        int minWhiteBlackDiff,
+        boolean deglitch) {
+      this.minClusterPixels = minClusterPixels;
+      this.maxNumMaxima = maxNumMaxima;
+      this.criticalAngle = criticalAngle;
+      this.maxLineFitMSE = maxLineFitMSE;
+      this.minWhiteBlackDiff = minWhiteBlackDiff;
+      this.deglitch = deglitch;
+    }
+
+    @Override
+    public int hashCode() {
+      return minClusterPixels
+          + maxNumMaxima
+          + Double.hashCode(criticalAngle)
+          + Float.hashCode(maxLineFitMSE)
+          + minWhiteBlackDiff
+          + Boolean.hashCode(deglitch);
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+      if (!(obj instanceof QuadThresholdParameters)) {
+        return false;
+      }
+
+      QuadThresholdParameters other = (QuadThresholdParameters) obj;
+      return minClusterPixels == other.minClusterPixels
+          && maxNumMaxima == other.maxNumMaxima
+          && criticalAngle == other.criticalAngle
+          && maxLineFitMSE == other.maxLineFitMSE
+          && minWhiteBlackDiff == other.minWhiteBlackDiff
+          && deglitch == other.deglitch;
+    }
+  }
+
+  /** Constructs an AprilTagDetector. */
+  public AprilTagDetector() {
+    m_native = AprilTagJNI.createDetector();
+  }
+
+  @Override
+  public void close() {
+    if (m_native != 0) {
+      AprilTagJNI.destroyDetector(m_native);
+    }
+    m_native = 0;
+  }
+
+  /**
+   * Sets detector configuration.
+   *
+   * @param config Configuration
+   */
+  public void setConfig(Config config) {
+    AprilTagJNI.setDetectorConfig(m_native, config);
+  }
+
+  /**
+   * Gets detector configuration.
+   *
+   * @return Configuration
+   */
+  public Config getConfig() {
+    return AprilTagJNI.getDetectorConfig(m_native);
+  }
+
+  /**
+   * Sets quad threshold parameters.
+   *
+   * @param params Parameters
+   */
+  public void setQuadThresholdParameters(QuadThresholdParameters params) {
+    AprilTagJNI.setDetectorQTP(m_native, params);
+  }
+
+  /**
+   * Gets quad threshold parameters.
+   *
+   * @return Parameters
+   */
+  public QuadThresholdParameters getQuadThresholdParameters() {
+    return AprilTagJNI.getDetectorQTP(m_native);
+  }
+
+  /**
+   * Adds a family of tags to be detected.
+   *
+   * @param fam Family name, e.g. "tag16h5"
+   * @throws IllegalArgumentException if family name not recognized
+   */
+  public void addFamily(String fam) {
+    addFamily(fam, 2);
+  }
+
+  /**
+   * Adds a family of tags to be detected.
+   *
+   * @param fam Family name, e.g. "tag16h5"
+   * @param bitsCorrected Maximum number of bits to correct
+   * @throws IllegalArgumentException if family name not recognized
+   */
+  public void addFamily(String fam, int bitsCorrected) {
+    if (!AprilTagJNI.addFamily(m_native, fam, bitsCorrected)) {
+      throw new IllegalArgumentException("unknown family name '" + fam + "'");
+    }
+  }
+
+  /**
+   * Removes a family of tags from the detector.
+   *
+   * @param fam Family name, e.g. "tag16h5"
+   */
+  public void removeFamily(String fam) {
+    AprilTagJNI.removeFamily(m_native, fam);
+  }
+
+  /** Unregister all families. */
+  public void clearFamilies() {
+    AprilTagJNI.clearFamilies(m_native);
+  }
+
+  /**
+   * Detect tags from an 8-bit image.
+   *
+   * <p>The image must be grayscale.
+   *
+   * @param img 8-bit OpenCV Mat image
+   * @return Results (array of AprilTagDetection)
+   */
+  public AprilTagDetection[] detect(Mat img) {
+    return AprilTagJNI.detect(m_native, img.cols(), img.rows(), img.cols(), img.dataAddr());
+  }
+
+  private long m_native;
+}

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

@@ -0,0 +1,298 @@
+// 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 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 java.io.IOException;
+import java.io.InputStream;
+import java.io.InputStreamReader;
+import java.io.UncheckedIOException;
+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;
+
+/**
+ * Class for representing a layout of AprilTags on a field and reading them from a JSON format.
+ *
+ * <p>The JSON format contains two top-level objects, "tags" and "field". The "tags" object is a
+ * list of all AprilTags contained within a layout. Each AprilTag serializes to a JSON object
+ * containing an ID and a Pose3d. The "field" object is a descriptor of the size of the field in
+ * meters with "width" and "length" values. This is to account for arbitrary field sizes when
+ * transforming the poses.
+ *
+ * <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.
+ *
+ * <p>Tag poses represent the center of the tag, with a zero rotation representing a tag that is
+ * upright and facing away from the (blue) alliance wall (that is, towards the opposing alliance).
+ */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class AprilTagFieldLayout {
+  /** Common origin positions for the AprilTag coordinate system. */
+  public enum OriginPosition {
+    /** Blue alliance wall, right side. */
+    kBlueAllianceWallRightSide,
+    /** Red alliance wall, right side. */
+    kRedAllianceWallRightSide,
+  }
+
+  private final Map<Integer, AprilTag> m_apriltags = new HashMap<>();
+
+  @JsonProperty(value = "field")
+  private FieldDimensions m_fieldDimensions;
+
+  private Pose3d m_origin;
+
+  /**
+   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
+   *
+   * @param path Path of the JSON file to import from.
+   * @throws IOException If reading from the file fails.
+   */
+  public AprilTagFieldLayout(String path) throws IOException {
+    this(Path.of(path));
+  }
+
+  /**
+   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
+   *
+   * @param path Path of the JSON file to import from.
+   * @throws IOException If reading from the file fails.
+   */
+  public AprilTagFieldLayout(Path path) throws IOException {
+    AprilTagFieldLayout layout =
+        new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class);
+    m_apriltags.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 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));
+  }
+
+  @JsonCreator
+  private AprilTagFieldLayout(
+      @JsonProperty(required = true, value = "tags") List<AprilTag> apriltags,
+      @JsonProperty(required = true, value = "field") FieldDimensions fieldDimensions) {
+    // To ensure the underlying semantics don't change with what kind of list is passed in
+    for (AprilTag tag : apriltags) {
+      m_apriltags.put(tag.ID, tag);
+    }
+    m_fieldDimensions = fieldDimensions;
+    setOrigin(OriginPosition.kBlueAllianceWallRightSide);
+  }
+
+  /**
+   * Returns a List of the {@link AprilTag AprilTags} used in this layout.
+   *
+   * @return The {@link AprilTag AprilTags} used in this layout.
+   */
+  @JsonProperty("tags")
+  public List<AprilTag> getTags() {
+    return new ArrayList<>(m_apriltags.values());
+  }
+
+  /**
+   * Returns the length of the field the layout is representing in meters.
+   *
+   * @return length, in meters
+   */
+  @JsonIgnore
+  public double getFieldLength() {
+    return m_fieldDimensions.fieldLength;
+  }
+
+  /**
+   * Returns the length of the field the layout is representing in meters.
+   *
+   * @return width, in meters
+   */
+  @JsonIgnore
+  public double getFieldWidth() {
+    return m_fieldDimensions.fieldWidth;
+  }
+
+  /**
+   * Sets the origin based on a predefined enumeration of coordinate frame origins. The origins are
+   * calculated from the field dimensions.
+   *
+   * <p>This transforms the Pose3d objects returned by {@link #getTagPose(int)} to return the
+   * correct pose relative to a predefined coordinate frame.
+   *
+   * @param origin The predefined origin
+   */
+  @JsonIgnore
+  public final 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 Pose3d objects 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 final void setOrigin(Pose3d origin) {
+    m_origin = origin;
+  }
+
+  /**
+   * Returns the origin used for tag pose transformation.
+   *
+   * @return the origin
+   */
+  @JsonIgnore
+  public Pose3d getOrigin() {
+    return m_origin;
+  }
+
+  /**
+   * Gets an AprilTag pose by its ID.
+   *
+   * @param ID The ID of the tag.
+   * @return The pose corresponding to the ID passed in or an empty optional if a tag with that ID
+   *     was not found.
+   */
+  @SuppressWarnings("ParameterName")
+  public Optional<Pose3d> getTagPose(int ID) {
+    AprilTag tag = m_apriltags.get(ID);
+    if (tag == null) {
+      return Optional.empty();
+    }
+    return Optional.of(tag.pose.relativeTo(m_origin));
+  }
+
+  /**
+   * Serializes a AprilTagFieldLayout to a JSON file.
+   *
+   * @param path The path to write to.
+   * @throws IOException If writing to the file fails.
+   */
+  public void serialize(String path) throws IOException {
+    serialize(Path.of(path));
+  }
+
+  /**
+   * Serializes a AprilTagFieldLayout to a JSON file.
+   *
+   * @param path The path to write to.
+   * @throws IOException If writing to the file fails.
+   */
+  public void serialize(Path path) throws IOException {
+    new ObjectMapper().writeValue(path.toFile(), this);
+  }
+
+  /**
+   * Get an official {@link AprilTagFieldLayout}.
+   *
+   * @param field The loadable AprilTag field layout.
+   * @return AprilTagFieldLayout of the field.
+   * @throws UncheckedIOException If the layout does not exist.
+   */
+  public static AprilTagFieldLayout loadField(AprilTagFields field) {
+    try {
+      return loadFromResource(field.m_resourceFile);
+    } catch (IOException e) {
+      throw new UncheckedIOException(
+          "Could not load AprilTagFieldLayout from " + field.m_resourceFile, e);
+    }
+  }
+
+  /**
+   * Deserializes a field layout from a resource within a internal jar file.
+   *
+   * <p>Users should use {@link AprilTagFields#loadAprilTagLayoutField()} to load official layouts
+   * and {@link #AprilTagFieldLayout(String)} for custom layouts.
+   *
+   * @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 {
+    InputStream stream = AprilTagFieldLayout.class.getResourceAsStream(resourcePath);
+    if (stream == null) {
+      // Class.getResourceAsStream() returns null if the resource does not exist.
+      throw new IOException("Could not locate resource: " + resourcePath);
+    }
+    InputStreamReader reader = new InputStreamReader(stream);
+    try {
+      return new ObjectMapper().readerFor(AprilTagFieldLayout.class).readValue(reader);
+    } catch (IOException e) {
+      throw new IOException("Failed to load AprilTagFieldLayout: " + resourcePath);
+    }
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof AprilTagFieldLayout) {
+      var other = (AprilTagFieldLayout) obj;
+      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_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;
+
+    @JsonCreator()
+    FieldDimensions(
+        @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,40 @@
+// 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 java.io.UncheckedIOException;
+
+/** Loadable AprilTag field layouts. */
+public enum AprilTagFields {
+  /** 2022 Rapid React. */
+  k2022RapidReact("2022-rapidreact.json"),
+  /** 2023 Charged Up. */
+  k2023ChargedUp("2023-chargedup.json"),
+  /** 2024 Crescendo. */
+  k2024Crescendo("2024-crescendo.json");
+
+  /** Base resource directory. */
+  public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/";
+
+  /** Alias to the current game. */
+  public static final AprilTagFields kDefaultField = k2024Crescendo;
+
+  /** Resource filename. */
+  public final String m_resourceFile;
+
+  AprilTagFields(String resourceFile) {
+    m_resourceFile = kBaseResourceDir + resourceFile;
+  }
+
+  /**
+   * Get a {@link AprilTagFieldLayout} from the resource JSON.
+   *
+   * @return AprilTagFieldLayout of the field
+   * @throws UncheckedIOException If the layout does not exist
+   */
+  public AprilTagFieldLayout loadAprilTagLayoutField() {
+    return AprilTagFieldLayout.loadField(this);
+  }
+}

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

@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.apriltag;
+
+import edu.wpi.first.math.geometry.Transform3d;
+
+/** A pair of AprilTag pose estimates. */
+@SuppressWarnings("MemberName")
+public class AprilTagPoseEstimate {
+  /**
+   * Constructs a pose estimate.
+   *
+   * @param pose1 first pose
+   * @param pose2 second pose
+   * @param error1 error of first pose
+   * @param error2 error of second pose
+   */
+  public AprilTagPoseEstimate(Transform3d pose1, Transform3d pose2, double error1, double error2) {
+    this.pose1 = pose1;
+    this.pose2 = pose2;
+    this.error1 = error1;
+    this.error2 = error2;
+  }
+
+  /**
+   * 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 getAmbiguity() {
+    double min = Math.min(error1, error2);
+    double max = Math.max(error1, error2);
+
+    if (max > 0) {
+      return min / max;
+    } else {
+      return -1;
+    }
+  }
+
+  /** Pose 1. */
+  public final Transform3d pose1;
+
+  /** Pose 2. */
+  public final Transform3d pose2;
+
+  /** Object-space error of pose 1. */
+  public final double error1;
+
+  /** Object-space error of pose 2. */
+  public final double error2;
+}

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

@@ -0,0 +1,199 @@
+// 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;
+import edu.wpi.first.math.geometry.Transform3d;
+
+/** Pose estimators for AprilTag tags. */
+public class AprilTagPoseEstimator {
+  /** Configuration for the pose estimator. */
+  @SuppressWarnings("MemberName")
+  public static class Config {
+    /**
+     * Creates a pose estimator configuration.
+     *
+     * @param tagSize tag size, in meters
+     * @param fx camera horizontal focal length, in pixels
+     * @param fy camera vertical focal length, in pixels
+     * @param cx camera horizontal focal center, in pixels
+     * @param cy camera vertical focal center, in pixels
+     */
+    public Config(double tagSize, double fx, double fy, double cx, double cy) {
+      this.tagSize = tagSize;
+      this.fx = fx;
+      this.fy = fy;
+      this.cx = cx;
+      this.cy = cy;
+    }
+
+    /** Tag size, in meters. */
+    public double tagSize;
+
+    /** Camera horizontal focal length, in pixels. */
+    public double fx;
+
+    /** Camera vertical focal length, in pixels. */
+    public double fy;
+
+    /** Camera horizontal focal center, in pixels. */
+    public double cx;
+
+    /** Camera vertical focal center, in pixels. */
+    public double cy;
+
+    @Override
+    public int hashCode() {
+      return Double.hashCode(tagSize)
+          + Double.hashCode(fx)
+          + Double.hashCode(fy)
+          + Double.hashCode(cx)
+          + Double.hashCode(cy);
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+      if (!(obj instanceof Config)) {
+        return false;
+      }
+
+      Config other = (Config) obj;
+      return tagSize == other.tagSize
+          && fx == other.fx
+          && fy == other.fy
+          && cx == other.cx
+          && cy == other.cy;
+    }
+  }
+
+  /**
+   * Creates estimator.
+   *
+   * @param config Configuration
+   */
+  public AprilTagPoseEstimator(Config config) {
+    m_config = new Config(config.tagSize, config.fx, config.fy, config.cx, config.cy);
+  }
+
+  /**
+   * Sets estimator configuration.
+   *
+   * @param config Configuration
+   */
+  public void setConfig(Config config) {
+    m_config.tagSize = config.tagSize;
+    m_config.fx = config.fx;
+    m_config.fy = config.fy;
+    m_config.cx = config.cx;
+    m_config.cy = config.cy;
+  }
+
+  /**
+   * Gets estimator configuration.
+   *
+   * @return Configuration
+   */
+  public Config getConfig() {
+    return new Config(m_config.tagSize, m_config.fx, m_config.fy, m_config.cx, m_config.cy);
+  }
+
+  /**
+   * Estimates the pose of the tag using the homography method described in [1].
+   *
+   * @param detection Tag detection
+   * @return Pose estimate
+   */
+  public Transform3d estimateHomography(AprilTagDetection detection) {
+    return estimateHomography(detection.getHomography());
+  }
+
+  /**
+   * Estimates the pose of the tag using the homography method described in [1].
+   *
+   * @param homography Homography 3x3 matrix data
+   * @return Pose estimate
+   */
+  public Transform3d estimateHomography(double[] homography) {
+    return AprilTagJNI.estimatePoseHomography(
+        homography, m_config.tagSize, m_config.fx, m_config.fy, m_config.cx, m_config.cy);
+  }
+
+  /**
+   * Estimates the pose of the tag. This returns one or two possible poses for the tag, along with
+   * the object-space error of each.
+   *
+   * <p>This uses the homography method described in [1] for the initial estimate. Then Orthogonal
+   * Iteration [2] is used to refine this estimate. Then [3] is used to find a potential second
+   * local minima and Orthogonal Iteration is used to refine this second estimate.
+   *
+   * <p>[1]: E. Olson, “Apriltag: A robust and flexible visual fiducial system,” in 2011 IEEE
+   * International Conference on Robotics and Automation, May 2011, pp. 3400–3407.
+   *
+   * <p>[2]: Lu, G. D. Hager and E. Mjolsness, "Fast and globally convergent pose estimation from
+   * video images," in IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 22, no.
+   * 6, pp. 610-622, June 2000. doi: 10.1109/34.862199
+   *
+   * <p>[3]: Schweighofer and A. Pinz, "Robust Pose Estimation from a Planar Target," in IEEE
+   * Transactions on Pattern Analysis and Machine Intelligence, vol. 28, no. 12, pp. 2024-2030, Dec.
+   * 2006. doi: 10.1109/TPAMI.2006.252
+   *
+   * @param detection Tag detection
+   * @param nIters Number of iterations
+   * @return Initial and (possibly) second pose estimates
+   */
+  public AprilTagPoseEstimate estimateOrthogonalIteration(AprilTagDetection detection, int nIters) {
+    return estimateOrthogonalIteration(detection.getHomography(), detection.getCorners(), nIters);
+  }
+
+  /**
+   * Estimates the pose of the tag. This returns one or two possible poses for the tag, along with
+   * the object-space error of each.
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param corners Corner point array (X and Y for each corner in order)
+   * @param nIters Number of iterations
+   * @return Initial and (possibly) second pose estimates
+   */
+  public AprilTagPoseEstimate estimateOrthogonalIteration(
+      double[] homography, double[] corners, int nIters) {
+    return AprilTagJNI.estimatePoseOrthogonalIteration(
+        homography,
+        corners,
+        m_config.tagSize,
+        m_config.fx,
+        m_config.fy,
+        m_config.cx,
+        m_config.cy,
+        nIters);
+  }
+
+  /**
+   * Estimates tag pose. This method is an easier to use interface to
+   * EstimatePoseOrthogonalIteration(), running 50 iterations and returning the pose with the lower
+   * object-space error.
+   *
+   * @param detection Tag detection
+   * @return Pose estimate
+   */
+  public Transform3d estimate(AprilTagDetection detection) {
+    return estimate(detection.getHomography(), detection.getCorners());
+  }
+
+  /**
+   * Estimates tag pose. This method is an easier to use interface to
+   * EstimatePoseOrthogonalIteration(), running 50 iterations and returning the pose with the lower
+   * object-space error.
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param corners Corner point array (X and Y for each corner in order)
+   * @return Pose estimate
+   */
+  public Transform3d estimate(double[] homography, double[] corners) {
+    return AprilTagJNI.estimatePose(
+        homography, corners, m_config.tagSize, m_config.fx, m_config.fy, m_config.cx, m_config.cy);
+  }
+
+  private final Config m_config;
+}

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

@@ -0,0 +1,229 @@
+// 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.apriltag.AprilTagDetection;
+import edu.wpi.first.apriltag.AprilTagDetector;
+import edu.wpi.first.apriltag.AprilTagPoseEstimate;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.util.RawFrame;
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.IOException;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+/** AprilTag JNI. */
+public class AprilTagJNI {
+  static boolean libraryLoaded = false;
+
+  static RuntimeLoader<AprilTagJNI> loader = null;
+
+  /** Sets whether JNI should be loaded in the static block. */
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    /**
+     * Returns true if the JNI should be loaded in the static block.
+     *
+     * @return True if the JNI should be loaded in the static block.
+     */
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    /**
+     * Sets whether the JNI should be loaded in the static block.
+     *
+     * @param load Whether the JNI should be loaded in the static block.
+     */
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+
+    /** Utility class. */
+    private Helper() {}
+  }
+
+  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;
+    }
+  }
+
+  /**
+   * Constructs an AprilTag detector engine.
+   *
+   * @return The detector engine handle
+   */
+  public static native long createDetector();
+
+  /**
+   * Destroys an AprilTag detector engine.
+   *
+   * @param det The detector engine handle
+   */
+  public static native void destroyDetector(long det);
+
+  /**
+   * Sets the detector engine configuration.
+   *
+   * @param det The detector engine handle
+   * @param config A configuration
+   */
+  public static native void setDetectorConfig(long det, AprilTagDetector.Config config);
+
+  /**
+   * Gets the detector engine configuration.
+   *
+   * @param det The detector engine handle
+   * @return The configuration
+   */
+  public static native AprilTagDetector.Config getDetectorConfig(long det);
+
+  /**
+   * Sets the detector engine quad threshold parameters.
+   *
+   * @param det The detector engine handle
+   * @param params Quad threshold parameters
+   */
+  public static native void setDetectorQTP(
+      long det, AprilTagDetector.QuadThresholdParameters params);
+
+  /**
+   * Gets the detector engine quad threshold parameters.
+   *
+   * @param det The detector engine handle
+   * @return Quad threshold parameters
+   */
+  public static native AprilTagDetector.QuadThresholdParameters getDetectorQTP(long det);
+
+  /**
+   * Adds a family of tags to be detected by the detector engine.
+   *
+   * @param det The detector engine handle
+   * @param fam Family name, e.g. "tag16h5"
+   * @param bitsCorrected Maximum number of bits to correct
+   * @return False if family can't be found
+   */
+  public static native boolean addFamily(long det, String fam, int bitsCorrected);
+
+  /**
+   * Removes a family of tags from the detector.
+   *
+   * @param det The detector engine handle
+   * @param fam Family name, e.g. "tag16h5"
+   */
+  public static native void removeFamily(long det, String fam);
+
+  /**
+   * Unregister all families.
+   *
+   * @param det The detector engine handle
+   */
+  public static native void clearFamilies(long det);
+
+  /**
+   * Detect tags from an 8-bit image.
+   *
+   * @param det The detector engine handle
+   * @param width The width of the image
+   * @param height The height of the image
+   * @param stride The number of bytes between image rows (often the same as width)
+   * @param bufAddr The address of the image buffer
+   * @return The results (array of AprilTagDetection)
+   */
+  public static native AprilTagDetection[] detect(
+      long det, int width, int height, int stride, long bufAddr);
+
+  /**
+   * Estimates the pose of the tag using the homography method described in [1].
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param tagSize The tag size, in meters
+   * @param fx The camera horizontal focal length, in pixels
+   * @param fy The camera vertical focal length, in pixels
+   * @param cx The camera horizontal focal center, in pixels
+   * @param cy The camera vertical focal center, in pixels
+   * @return Pose estimate
+   */
+  public static native Transform3d estimatePoseHomography(
+      double[] homography, double tagSize, double fx, double fy, double cx, double cy);
+
+  /**
+   * Estimates the pose of the tag. This returns one or two possible poses for the tag, along with
+   * the object-space error of each.
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param corners Corner point array (X and Y for each corner in order)
+   * @param tagSize The tag size, in meters
+   * @param fx The camera horizontal focal length, in pixels
+   * @param fy The camera vertical focal length, in pixels
+   * @param cx The camera horizontal focal center, in pixels
+   * @param cy The camera vertical focal center, in pixels
+   * @param nIters Number of iterations
+   * @return Initial and (possibly) second pose estimates
+   */
+  public static native AprilTagPoseEstimate estimatePoseOrthogonalIteration(
+      double[] homography,
+      double[] corners,
+      double tagSize,
+      double fx,
+      double fy,
+      double cx,
+      double cy,
+      int nIters);
+
+  /**
+   * Estimates tag pose. This method is an easier to use interface to
+   * EstimatePoseOrthogonalIteration(), running 50 iterations and returning the pose with the lower
+   * object-space error.
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param corners Corner point array (X and Y for each corner in order)
+   * @param tagSize The tag size, in meters
+   * @param fx The camera horizontal focal length, in pixels
+   * @param fy The camera vertical focal length, in pixels
+   * @param cx The camera horizontal focal center, in pixels
+   * @param cy The camera vertical focal center, in pixels
+   * @return Pose estimate
+   */
+  public static native Transform3d estimatePose(
+      double[] homography,
+      double[] corners,
+      double tagSize,
+      double fx,
+      double fy,
+      double cx,
+      double cy);
+
+  /**
+   * Generates a RawFrame containing the apriltag with the id with family 16h5 passed in.
+   *
+   * @param frameObj generated frame (output parameter).
+   * @param frame raw frame handle
+   * @param id id
+   */
+  public static native void generate16h5AprilTagImage(RawFrame frameObj, long frame, int id);
+
+  /**
+   * Generates a RawFrame containing the apriltag with the id with family 36h11 passed in.
+   *
+   * @param frameObj generated frame (output parameter).
+   * @param frame raw frame handle
+   * @param id id
+   */
+  public static native void generate36h11AprilTagImage(RawFrame frameObj, long frame, int id);
+
+  /** Utility class. */
+  private AprilTagJNI() {}
+}

apriltag/src/main/native/cpp/AprilTag.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/AprilTag.h"
+
+#include <cstring>
+
+#include <wpi/json.h>
+
+#ifdef _WIN32
+#pragma warning(disable : 4200)
+#elif defined(__clang__)
+#pragma clang diagnostic ignored "-Wc99-extensions"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic ignored "-Wpedantic"
+#endif
+
+#include "apriltag.h"
+#include "tag16h5.h"
+#include "tag36h11.h"
+
+using namespace frc;
+
+static bool FamilyToImage(wpi::RawFrame* frame, apriltag_family_t* family,
+                          int id) {
+  image_u8_t* image = apriltag_to_image(family, id);
+  size_t totalDataSize = image->height * image->stride;
+  bool rv = frame->Reserve(totalDataSize);
+  std::memcpy(frame->data, image->buf, totalDataSize);
+  frame->size = totalDataSize;
+  frame->width = image->width;
+  frame->height = image->height;
+  frame->stride = image->stride;
+  frame->pixelFormat = WPI_PIXFMT_GRAY;
+  image_u8_destroy(image);
+  return rv;
+}
+
+bool AprilTag::Generate36h11AprilTagImage(wpi::RawFrame* frame, int id) {
+  apriltag_family_t* tagFamily = tag36h11_create();
+  bool rv = FamilyToImage(frame, tagFamily, id);
+  tag36h11_destroy(tagFamily);
+  return rv;
+}
+
+bool AprilTag::Generate16h5AprilTagImage(wpi::RawFrame* frame, int id) {
+  apriltag_family_t* tagFamily = tag16h5_create();
+  bool rv = FamilyToImage(frame, tagFamily, id);
+  tag16h5_destroy(tagFamily);
+  return rv;
+}
+
+void frc::to_json(wpi::json& json, const AprilTag& apriltag) {
+  json = wpi::json{{"ID", apriltag.ID}, {"pose", apriltag.pose}};
+}
+
+void frc::from_json(const wpi::json& json, AprilTag& apriltag) {
+  apriltag.ID = json.at("ID").get<int>();
+  apriltag.pose = json.at("pose").get<Pose3d>();
+}

apriltag/src/main/native/cpp/AprilTagDetection.cpp

@@ -0,0 +1,37 @@
+// 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/AprilTagDetection.h"
+
+#include <type_traits>
+
+#ifdef _WIN32
+#pragma warning(disable : 4200)
+#elif defined(__clang__)
+#pragma clang diagnostic ignored "-Wc99-extensions"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic ignored "-Wpedantic"
+#endif
+
+#include "apriltag.h"
+
+using namespace frc;
+
+static_assert(sizeof(AprilTagDetection) == sizeof(apriltag_detection_t),
+              "structure sizes don't match");
+static_assert(std::is_standard_layout_v<AprilTagDetection>,
+              "AprilTagDetection is not standard layout?");
+
+std::string_view AprilTagDetection::GetFamily() const {
+  return static_cast<const apriltag_family_t*>(family)->name;
+}
+
+std::span<const double, 9> AprilTagDetection::GetHomography() const {
+  return std::span<const double, 9>{static_cast<matd_t*>(H)->data, 9};
+}
+
+Eigen::Matrix3d AprilTagDetection::GetHomographyMatrix() const {
+  return Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>{
+      static_cast<matd_t*>(H)->data};
+}

apriltag/src/main/native/cpp/AprilTagDetector.cpp

@@ -0,0 +1,200 @@
+// 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/AprilTagDetector.h"
+
+#include <cmath>
+#include <numbers>
+
+#ifdef _WIN32
+#pragma warning(disable : 4200)
+#elif defined(__clang__)
+#pragma clang diagnostic ignored "-Wc99-extensions"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic ignored "-Wpedantic"
+#endif
+
+#include "apriltag.h"
+#include "tag16h5.h"
+#include "tag25h9.h"
+#include "tag36h11.h"
+#include "tagCircle21h7.h"
+#include "tagCircle49h12.h"
+#include "tagCustom48h12.h"
+#include "tagStandard41h12.h"
+#include "tagStandard52h13.h"
+
+using namespace frc;
+
+AprilTagDetector::Results::Results(void* impl, const private_init&)
+    : span{reinterpret_cast<AprilTagDetection**>(
+               static_cast<zarray_t*>(impl)->data),
+           static_cast<size_t>(static_cast<zarray_t*>(impl)->size)},
+      m_impl{impl} {}
+
+AprilTagDetector::Results& AprilTagDetector::Results::operator=(Results&& rhs) {
+  Destroy();
+  m_impl = rhs.m_impl;
+  rhs.m_impl = nullptr;
+  return *this;
+}
+
+void AprilTagDetector::Results::Destroy() {
+  if (m_impl) {
+    apriltag_detections_destroy(static_cast<zarray_t*>(m_impl));
+  }
+}
+
+AprilTagDetector::AprilTagDetector() : m_impl{apriltag_detector_create()} {}
+
+AprilTagDetector& AprilTagDetector::operator=(AprilTagDetector&& rhs) {
+  Destroy();
+  m_impl = rhs.m_impl;
+  rhs.m_impl = nullptr;
+  m_families = std::move(rhs.m_families);
+  rhs.m_families.clear();
+  m_qtpCriticalAngle = rhs.m_qtpCriticalAngle;
+  return *this;
+}
+
+void AprilTagDetector::SetConfig(const Config& config) {
+  auto& impl = *static_cast<apriltag_detector_t*>(m_impl);
+  impl.nthreads = config.numThreads;
+  impl.quad_decimate = config.quadDecimate;
+  impl.quad_sigma = config.quadSigma;
+  impl.refine_edges = config.refineEdges;
+  impl.decode_sharpening = config.decodeSharpening;
+  impl.debug = config.debug;
+}
+
+AprilTagDetector::Config AprilTagDetector::GetConfig() const {
+  auto& impl = *static_cast<apriltag_detector_t*>(m_impl);
+  return {
+      .numThreads = impl.nthreads,
+      .quadDecimate = impl.quad_decimate,
+      .quadSigma = impl.quad_sigma,
+      .refineEdges = impl.refine_edges,
+      .decodeSharpening = impl.decode_sharpening,
+      .debug = impl.debug,
+  };
+}
+
+void AprilTagDetector::SetQuadThresholdParameters(
+    const QuadThresholdParameters& params) {
+  auto& qtp = static_cast<apriltag_detector_t*>(m_impl)->qtp;
+  qtp.min_cluster_pixels = params.minClusterPixels;
+  qtp.max_nmaxima = params.maxNumMaxima;
+  qtp.critical_rad = params.criticalAngle.value();
+  qtp.cos_critical_rad = std::cos(params.criticalAngle.value());
+  qtp.max_line_fit_mse = params.maxLineFitMSE;
+  qtp.min_white_black_diff = params.minWhiteBlackDiff;
+  qtp.deglitch = params.deglitch;
+
+  m_qtpCriticalAngle = params.criticalAngle;
+}
+
+AprilTagDetector::QuadThresholdParameters
+AprilTagDetector::GetQuadThresholdParameters() const {
+  auto& qtp = static_cast<apriltag_detector_t*>(m_impl)->qtp;
+  return {
+      .minClusterPixels = qtp.min_cluster_pixels,
+      .maxNumMaxima = qtp.max_nmaxima,
+      .criticalAngle = m_qtpCriticalAngle,
+      .maxLineFitMSE = qtp.max_line_fit_mse,
+      .minWhiteBlackDiff = qtp.min_white_black_diff,
+      .deglitch = qtp.deglitch != 0,
+  };
+}
+
+bool AprilTagDetector::AddFamily(std::string_view fam, int bitsCorrected) {
+  auto& data = m_families[fam];
+  if (data) {
+    return true;  // already detecting
+  }
+  // create the family
+  if (fam == "tag16h5") {
+    data = tag16h5_create();
+  } else if (fam == "tag25h9") {
+    data = tag25h9_create();
+  } else if (fam == "tag36h11") {
+    data = tag36h11_create();
+  } else if (fam == "tagCircle21h7") {
+    data = tagCircle21h7_create();
+  } else if (fam == "tagCircle49h12") {
+    data = tagCircle49h12_create();
+  } else if (fam == "tagStandard41h12") {
+    data = tagStandard41h12_create();
+  } else if (fam == "tagStandard52h13") {
+    data = tagStandard52h13_create();
+  } else if (fam == "tagCustom48h12") {
+    data = tagCustom48h12_create();
+  }
+  if (!data) {
+    m_families.erase(fam);  // don't keep null value
+    return false;           // can't add
+  }
+  apriltag_detector_add_family_bits(static_cast<apriltag_detector_t*>(m_impl),
+                                    static_cast<apriltag_family_t*>(data),
+                                    bitsCorrected);
+  return true;
+}
+
+void AprilTagDetector::RemoveFamily(std::string_view fam) {
+  auto it = m_families.find(fam);
+  if (it != m_families.end()) {
+    apriltag_detector_remove_family(
+        static_cast<apriltag_detector_t*>(m_impl),
+        static_cast<apriltag_family_t*>(it->second));
+    DestroyFamily(it->getKey(), it->second);
+    m_families.erase(it);
+  }
+}
+
+void AprilTagDetector::ClearFamilies() {
+  apriltag_detector_clear_families(static_cast<apriltag_detector_t*>(m_impl));
+  DestroyFamilies();
+  m_families.clear();
+}
+
+AprilTagDetector::Results AprilTagDetector::Detect(int width, int height,
+                                                   int stride, uint8_t* buf) {
+  image_u8_t img{width, height, stride, buf};
+  return {
+      apriltag_detector_detect(static_cast<apriltag_detector_t*>(m_impl), &img),
+      Results::private_init{}};
+}
+
+void AprilTagDetector::Destroy() {
+  if (m_impl) {
+    apriltag_detector_destroy(static_cast<apriltag_detector_t*>(m_impl));
+  }
+  DestroyFamilies();
+}
+
+void AprilTagDetector::DestroyFamilies() {
+  for (auto&& entry : m_families) {
+    DestroyFamily(entry.getKey(), entry.second);
+  }
+}
+
+void AprilTagDetector::DestroyFamily(std::string_view name, void* data) {
+  auto fam = static_cast<apriltag_family_t*>(data);
+  if (name == "tag16h5") {
+    tag16h5_destroy(fam);
+  } else if (name == "tag25h9") {
+    tag25h9_destroy(fam);
+  } else if (name == "tag36h11") {
+    tag36h11_destroy(fam);
+  } else if (name == "tagCircle21h7") {
+    tagCircle21h7_destroy(fam);
+  } else if (name == "tagCircle49h12") {
+    tagCircle49h12_destroy(fam);
+  } else if (name == "tagStandard41h12") {
+    tagStandard41h12_destroy(fam);
+  } else if (name == "tagStandard52h13") {
+    tagStandard52h13_destroy(fam);
+  } else if (name == "tagCustom48h12") {
+    tagCustom48h12_destroy(fam);
+  }
+}

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

@@ -0,0 +1,161 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/apriltag/AprilTagFieldLayout.h"
+
+#include <system_error>
+
+#include <units/angle.h>
+#include <units/length.h>
+#include <wpi/MemoryBuffer.h>
+#include <wpi/json.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) {
+  std::error_code ec;
+  std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
+      wpi::MemoryBuffer::GetFile(path, ec);
+  if (fileBuffer == nullptr || ec) {
+    throw std::runtime_error(fmt::format("Cannot open file: {}", path));
+  }
+
+  wpi::json json = wpi::json::parse(fileBuffer->GetCharBuffer());
+
+  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("length").get<double>()};
+}
+
+AprilTagFieldLayout::AprilTagFieldLayout(std::vector<AprilTag> apriltags,
+                                         units::meter_t fieldLength,
+                                         units::meter_t fieldWidth)
+    : m_fieldLength(std::move(fieldLength)),
+      m_fieldWidth(std::move(fieldWidth)) {
+  for (const auto& tag : apriltags) {
+    m_apriltags[tag.ID] = tag;
+  }
+}
+
+units::meter_t AprilTagFieldLayout::GetFieldLength() const {
+  return m_fieldLength;
+}
+
+units::meter_t AprilTagFieldLayout::GetFieldWidth() const {
+  return m_fieldWidth;
+}
+
+std::vector<AprilTag> AprilTagFieldLayout::GetTags() const {
+  std::vector<AprilTag> tags;
+  tags.reserve(m_apriltags.size());
+  for (const auto& tag : m_apriltags) {
+    tags.emplace_back(tag.second);
+  }
+  return tags;
+}
+
+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;
+}
+
+Pose3d AprilTagFieldLayout::GetOrigin() const {
+  return m_origin;
+}
+
+std::optional<frc::Pose3d> AprilTagFieldLayout::GetTagPose(int ID) const {
+  const auto& it = m_apriltags.find(ID);
+  if (it == m_apriltags.end()) {
+    return std::nullopt;
+  }
+  return it->second.pose.RelativeTo(m_origin);
+}
+
+void AprilTagFieldLayout::Serialize(std::string_view path) {
+  std::error_code error_code;
+
+  wpi::raw_fd_ostream output{path, error_code};
+  if (error_code) {
+    throw std::runtime_error(fmt::format("Cannot open file: {}", path));
+  }
+
+  wpi::json json = *this;
+  output << json;
+  output.flush();
+}
+
+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", tagVector}};
+}
+
+void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) {
+  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 =
+      units::meter_t{json.at("field").at("width").get<double>()};
+}
+
+// Use namespace declaration for forward declaration
+namespace frc {
+
+// C++ generated from resource files
+std::string_view GetResource_2022_rapidreact_json();
+std::string_view GetResource_2023_chargedup_json();
+std::string_view GetResource_2024_crescendo_json();
+
+}  // namespace frc
+
+AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) {
+  std::string_view fieldString;
+  switch (field) {
+    case AprilTagField::k2022RapidReact:
+      fieldString = GetResource_2022_rapidreact_json();
+      break;
+    case AprilTagField::k2023ChargedUp:
+      fieldString = GetResource_2023_chargedup_json();
+      break;
+    case AprilTagField::k2024Crescendo:
+      fieldString = GetResource_2024_crescendo_json();
+      break;
+    case AprilTagField::kNumFields:
+      throw std::invalid_argument("Invalid Field");
+  }
+
+  wpi::json json = wpi::json::parse(fieldString);
+  return json.get<AprilTagFieldLayout>();
+}
+
+AprilTagFieldLayout frc::LoadAprilTagLayoutField(AprilTagField field) {
+  return AprilTagFieldLayout::LoadField(field);
+}

apriltag/src/main/native/cpp/AprilTagPoseEstimate.cpp

@@ -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.
+
+#include "frc/apriltag/AprilTagPoseEstimate.h"
+
+#include <algorithm>
+
+using namespace frc;
+
+double AprilTagPoseEstimate::GetAmbiguity() const {
+  auto min = (std::min)(error1, error2);
+  auto max = (std::max)(error1, error2);
+
+  if (max > 0) {
+    return min / max;
+  } else {
+    return -1;
+  }
+}

apriltag/src/main/native/cpp/AprilTagPoseEstimator.cpp

@@ -0,0 +1,154 @@
+// 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/AprilTagPoseEstimator.h"
+
+#include <Eigen/QR>
+
+#include "frc/apriltag/AprilTagDetection.h"
+
+#ifdef _WIN32
+#pragma warning(disable : 4200)
+#elif defined(__clang__)
+#pragma clang diagnostic ignored "-Wc99-extensions"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic ignored "-Wpedantic"
+#endif
+
+#include "apriltag.h"
+#include "apriltag_pose.h"
+
+using namespace frc;
+
+static Eigen::Matrix3d OrthogonalizeRotationMatrix(
+    const Eigen::Matrix3d& input) {
+  Eigen::HouseholderQR<Eigen::Matrix3d> qr{input};
+
+  Eigen::Matrix3d Q = qr.householderQ();
+  Eigen::Matrix3d R = qr.matrixQR().triangularView<Eigen::Upper>();
+
+  // 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(colR, colR) < 0) {
+      for (int rowQ = 0; rowQ < 3; ++rowQ) {
+        Q(rowQ, colR) = -Q(rowQ, colR);
+      }
+    }
+  }
+
+  return Q;
+}
+
+static Transform3d MakePose(const apriltag_pose_t& pose) {
+  if (!pose.R || !pose.t) {
+    return {};
+  }
+  return {Translation3d{units::meter_t{pose.t->data[0]},
+                        units::meter_t{pose.t->data[1]},
+                        units::meter_t{pose.t->data[2]}},
+          Rotation3d{OrthogonalizeRotationMatrix(
+              Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>{
+                  pose.R->data})}};
+}
+
+static apriltag_detection_info_t MakeDetectionInfo(
+    const apriltag_detection_t* det,
+    const AprilTagPoseEstimator::Config& config) {
+  return {const_cast<apriltag_detection_t*>(det),
+          config.tagSize.value(),
+          config.fx,
+          config.fy,
+          config.cx,
+          config.cy};
+}
+
+static apriltag_detection_t MakeBasicDet(
+    std::span<const double, 9> homography,
+    const std::span<const double, 8>* corners) {
+  apriltag_detection_t detection;
+  detection.H = matd_create(3, 3);
+  std::memcpy(detection.H->data, homography.data(), 9 * sizeof(double));
+  if (corners) {
+    for (int i = 0; i < 4; i++) {
+      detection.p[i][0] = (*corners)[i * 2];
+      detection.p[i][1] = (*corners)[i * 2 + 1];
+    }
+  }
+  return detection;
+}
+
+static Transform3d DoEstimateHomography(
+    const apriltag_detection_t* detection,
+    const AprilTagPoseEstimator::Config& config) {
+  auto info = MakeDetectionInfo(detection, config);
+  apriltag_pose_t pose;
+  estimate_pose_for_tag_homography(&info, &pose);
+  return MakePose(pose);
+}
+
+Transform3d AprilTagPoseEstimator::EstimateHomography(
+    const AprilTagDetection& detection) const {
+  return DoEstimateHomography(
+      reinterpret_cast<const apriltag_detection_t*>(&detection), m_config);
+}
+
+Transform3d AprilTagPoseEstimator::EstimateHomography(
+    std::span<const double, 9> homography) const {
+  auto detection = MakeBasicDet(homography, nullptr);
+  auto rv = DoEstimateHomography(&detection, m_config);
+  matd_destroy(detection.H);
+  return rv;
+}
+
+static AprilTagPoseEstimate DoEstimateOrthogonalIteration(
+    const apriltag_detection_t* detection,
+    const AprilTagPoseEstimator::Config& config, int nIters) {
+  auto info = MakeDetectionInfo(detection, config);
+  apriltag_pose_t pose1, pose2;
+  double err1, err2;
+  estimate_tag_pose_orthogonal_iteration(&info, &err1, &pose1, &err2, &pose2,
+                                         nIters, 1e-7);
+  return {MakePose(pose1), MakePose(pose2), err1, err2};
+}
+
+AprilTagPoseEstimate AprilTagPoseEstimator::EstimateOrthogonalIteration(
+    const AprilTagDetection& detection, int nIters) const {
+  return DoEstimateOrthogonalIteration(
+      reinterpret_cast<const apriltag_detection_t*>(&detection), m_config,
+      nIters);
+}
+
+AprilTagPoseEstimate AprilTagPoseEstimator::EstimateOrthogonalIteration(
+    std::span<const double, 9> homography, std::span<const double, 8> corners,
+    int nIters) const {
+  auto detection = MakeBasicDet(homography, &corners);
+  auto rv = DoEstimateOrthogonalIteration(&detection, m_config, nIters);
+  matd_destroy(detection.H);
+  return rv;
+}
+
+static Transform3d DoEstimate(const apriltag_detection_t* detection,
+                              const AprilTagPoseEstimator::Config& config) {
+  auto info = MakeDetectionInfo(detection, config);
+  apriltag_pose_t pose;
+  estimate_tag_pose(&info, &pose);
+  return MakePose(pose);
+}
+
+Transform3d AprilTagPoseEstimator::Estimate(
+    const AprilTagDetection& detection) const {
+  return DoEstimate(reinterpret_cast<const apriltag_detection_t*>(&detection),
+                    m_config);
+}
+
+Transform3d AprilTagPoseEstimator::Estimate(
+    std::span<const double, 9> homography,
+    std::span<const double, 8> corners) const {
+  auto detection = MakeBasicDet(homography, &corners);
+  auto rv = DoEstimate(&detection, m_config);
+  matd_destroy(detection.H);
+  return rv;
+}

apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp

@@ -0,0 +1,634 @@
+// 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 <jni.h>
+
+#include <cstdio>
+#include <cstring>
+
+#define WPI_RAWFRAME_JNI
+#include <wpi/RawFrame.h>
+#include <wpi/jni_util.h>
+
+#include "edu_wpi_first_apriltag_jni_AprilTagJNI.h"
+#include "frc/apriltag/AprilTag.h"
+#include "frc/apriltag/AprilTagDetector.h"
+#include "frc/apriltag/AprilTagPoseEstimator.h"
+
+using namespace frc;
+using namespace wpi::java;
+
+static JavaVM* jvm = nullptr;
+
+static JClass detectionCls;
+static JClass detectorConfigCls;
+static JClass detectorQTPCls;
+static JClass poseEstimateCls;
+static JClass quaternionCls;
+static JClass rotation3dCls;
+static JClass transform3dCls;
+static JClass translation3dCls;
+static JClass rawFrameCls;
+static JException illegalArgEx;
+static JException nullPointerEx;
+
+static const JClassInit classes[] = {
+    {"edu/wpi/first/apriltag/AprilTagDetection", &detectionCls},
+    {"edu/wpi/first/apriltag/AprilTagDetector$Config", &detectorConfigCls},
+    {"edu/wpi/first/apriltag/AprilTagDetector$QuadThresholdParameters",
+     &detectorQTPCls},
+    {"edu/wpi/first/apriltag/AprilTagPoseEstimate", &poseEstimateCls},
+    {"edu/wpi/first/math/geometry/Quaternion", &quaternionCls},
+    {"edu/wpi/first/math/geometry/Rotation3d", &rotation3dCls},
+    {"edu/wpi/first/math/geometry/Transform3d", &transform3dCls},
+    {"edu/wpi/first/math/geometry/Translation3d", &translation3dCls},
+    {"edu/wpi/first/util/RawFrame", &rawFrameCls}};
+
+static const JExceptionInit exceptions[] = {
+    {"java/lang/IllegalArgumentException", &illegalArgEx},
+    {"java/lang/NullPointerException", &nullPointerEx}};
+
+extern "C" {
+
+JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
+  jvm = vm;
+
+  JNIEnv* env;
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
+    return JNI_ERR;
+  }
+
+  // Cache references to classes
+  for (auto& c : classes) {
+    *c.cls = JClass(env, c.name);
+    if (!*c.cls) {
+      std::fprintf(stderr, "could not load class %s\n", c.name);
+      return JNI_ERR;
+    }
+  }
+
+  for (auto& c : exceptions) {
+    *c.cls = JException(env, c.name);
+    if (!*c.cls) {
+      std::fprintf(stderr, "could not load exception %s\n", c.name);
+      return JNI_ERR;
+    }
+  }
+
+  return JNI_VERSION_1_6;
+}
+
+JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {
+  JNIEnv* env;
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
+    return;
+  }
+  // Delete global references
+  for (auto& c : classes) {
+    c.cls->free(env);
+  }
+  for (auto& c : exceptions) {
+    c.cls->free(env);
+  }
+  jvm = nullptr;
+}
+
+}  // extern "C"
+
+//
+// Conversions from Java to C++ objects
+//
+
+static AprilTagDetector::Config FromJavaDetectorConfig(JNIEnv* env,
+                                                       jobject jconfig) {
+  if (!jconfig) {
+    return {};
+  }
+#define FIELD(name, sig)                                          \
+  static jfieldID name##Field = nullptr;                          \
+  if (!name##Field) {                                             \
+    name##Field = env->GetFieldID(detectorConfigCls, #name, sig); \
+  }
+
+  FIELD(numThreads, "I");
+  FIELD(quadDecimate, "F");
+  FIELD(quadSigma, "F");
+  FIELD(refineEdges, "Z");
+  FIELD(decodeSharpening, "D");
+  FIELD(debug, "Z");
+
+#undef FIELD
+
+#define FIELD(ctype, jtype, name) \
+  .name = static_cast<ctype>(env->Get##jtype##Field(jconfig, name##Field))
+
+  return {
+      FIELD(int, Int, numThreads),
+      FIELD(float, Float, quadDecimate),
+      FIELD(float, Float, quadSigma),
+      FIELD(bool, Boolean, refineEdges),
+      FIELD(double, Double, decodeSharpening),
+      FIELD(bool, Boolean, debug),
+  };
+
+#undef GET
+#undef FIELD
+}
+
+static AprilTagDetector::QuadThresholdParameters FromJavaDetectorQTP(
+    JNIEnv* env, jobject jparams) {
+  if (!jparams) {
+    return {};
+  }
+#define FIELD(name, sig)                                       \
+  static jfieldID name##Field = nullptr;                       \
+  if (!name##Field) {                                          \
+    name##Field = env->GetFieldID(detectorQTPCls, #name, sig); \
+  }
+
+  FIELD(minClusterPixels, "I");
+  FIELD(maxNumMaxima, "I");
+  FIELD(criticalAngle, "D");
+  FIELD(maxLineFitMSE, "F");
+  FIELD(minWhiteBlackDiff, "I");
+  FIELD(deglitch, "Z");
+
+#undef FIELD
+
+#define FIELD(ctype, jtype, name) \
+  .name = static_cast<ctype>(env->Get##jtype##Field(jparams, name##Field))
+
+  return {
+      FIELD(int, Int, minClusterPixels),
+      FIELD(int, Int, maxNumMaxima),
+      .criticalAngle = units::radian_t{static_cast<double>(
+          env->GetDoubleField(jparams, criticalAngleField))},
+      FIELD(float, Float, maxLineFitMSE),
+      FIELD(int, Int, minWhiteBlackDiff),
+      FIELD(bool, Boolean, deglitch),
+  };
+
+#undef GET
+#undef FIELD
+}
+
+//
+// Conversions from C++ to Java objects
+//
+
+static jobject MakeJObject(JNIEnv* env, const AprilTagDetection& detect) {
+  static jmethodID constructor = env->GetMethodID(
+      detectionCls, "<init>", "(Ljava/lang/String;IIF[DDD[D)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  JLocal<jstring> fam{env, MakeJString(env, detect.GetFamily())};
+
+  auto homography = detect.GetHomography();
+  JLocal<jdoubleArray> harr{
+      env, MakeJDoubleArray(
+               env, {reinterpret_cast<const jdouble*>(homography.data()),
+                     homography.size()})};
+
+  double cornersBuf[8];
+  auto corners = detect.GetCorners(cornersBuf);
+  JLocal<jdoubleArray> carr{
+      env,
+      MakeJDoubleArray(env, {reinterpret_cast<const jdouble*>(corners.data()),
+                             corners.size()})};
+
+  auto center = detect.GetCenter();
+
+  return env->NewObject(detectionCls, constructor, fam.obj(),
+                        static_cast<jint>(detect.GetId()),
+                        static_cast<jint>(detect.GetHamming()),
+                        static_cast<jfloat>(detect.GetDecisionMargin()),
+                        harr.obj(), static_cast<jdouble>(center.x),
+                        static_cast<jdouble>(center.y), carr.obj());
+}
+
+static jobjectArray MakeJObject(JNIEnv* env,
+                                std::span<const AprilTagDetection* const> arr) {
+  jobjectArray jarr = env->NewObjectArray(arr.size(), detectionCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, *arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
+static jobject MakeJObject(JNIEnv* env,
+                           const AprilTagDetector::Config& config) {
+  static jmethodID constructor =
+      env->GetMethodID(detectorConfigCls, "<init>", "(IFFZDZ)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  return env->NewObject(detectorConfigCls, constructor,
+                        static_cast<jint>(config.numThreads),
+                        static_cast<jfloat>(config.quadDecimate),
+                        static_cast<jfloat>(config.quadSigma),
+                        static_cast<jboolean>(config.refineEdges),
+                        static_cast<jdouble>(config.decodeSharpening),
+                        static_cast<jboolean>(config.debug));
+}
+
+static jobject MakeJObject(
+    JNIEnv* env, const AprilTagDetector::QuadThresholdParameters& params) {
+  static jmethodID constructor =
+      env->GetMethodID(detectorQTPCls, "<init>", "(IIDFIZ)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  return env->NewObject(detectorQTPCls, constructor,
+                        static_cast<jint>(params.minClusterPixels),
+                        static_cast<jint>(params.maxNumMaxima),
+                        static_cast<jdouble>(params.criticalAngle),
+                        static_cast<jfloat>(params.maxLineFitMSE),
+                        static_cast<jint>(params.minWhiteBlackDiff),
+                        static_cast<jboolean>(params.deglitch));
+}
+
+static jobject MakeJObject(JNIEnv* env, const Translation3d& xlate) {
+  static jmethodID constructor =
+      env->GetMethodID(translation3dCls, "<init>", "(DDD)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  return env->NewObject(
+      translation3dCls, constructor, static_cast<jdouble>(xlate.X()),
+      static_cast<jdouble>(xlate.Y()), static_cast<jdouble>(xlate.Z()));
+}
+
+static jobject MakeJObject(JNIEnv* env, const Quaternion& q) {
+  static jmethodID constructor =
+      env->GetMethodID(quaternionCls, "<init>", "(DDDD)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  return env->NewObject(quaternionCls, constructor, static_cast<jdouble>(q.W()),
+                        static_cast<jdouble>(q.X()),
+                        static_cast<jdouble>(q.Y()),
+                        static_cast<jdouble>(q.Z()));
+}
+
+static jobject MakeJObject(JNIEnv* env, const Rotation3d& rot) {
+  static jmethodID constructor = env->GetMethodID(
+      rotation3dCls, "<init>", "(Ledu/wpi/first/math/geometry/Quaternion;)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  JLocal<jobject> q{env, MakeJObject(env, rot.GetQuaternion())};
+  return env->NewObject(rotation3dCls, constructor, q.obj());
+}
+
+static jobject MakeJObject(JNIEnv* env, const Transform3d& xform) {
+  static jmethodID constructor =
+      env->GetMethodID(transform3dCls, "<init>",
+                       "(Ledu/wpi/first/math/geometry/Translation3d;"
+                       "Ledu/wpi/first/math/geometry/Rotation3d;)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  JLocal<jobject> xlate{env, MakeJObject(env, xform.Translation())};
+  JLocal<jobject> rot{env, MakeJObject(env, xform.Rotation())};
+  return env->NewObject(transform3dCls, constructor, xlate.obj(), rot.obj());
+}
+
+static jobject MakeJObject(JNIEnv* env, const AprilTagPoseEstimate& est) {
+  static jmethodID constructor =
+      env->GetMethodID(poseEstimateCls, "<init>",
+                       "(Ledu/wpi/first/math/geometry/Transform3d;"
+                       "Ledu/wpi/first/math/geometry/Transform3d;DD)V");
+  if (!constructor) {
+    return nullptr;
+  }
+
+  JLocal<jobject> pose1{env, MakeJObject(env, est.pose1)};
+  JLocal<jobject> pose2{env, MakeJObject(env, est.pose2)};
+  return env->NewObject(poseEstimateCls, constructor, pose1.obj(), pose2.obj(),
+                        static_cast<jdouble>(est.error1),
+                        static_cast<jdouble>(est.error2));
+}
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    createDetector
+ * Signature: ()J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_createDetector
+  (JNIEnv* env, jclass)
+{
+  return reinterpret_cast<jlong>(new AprilTagDetector);
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    destroyDetector
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_destroyDetector
+  (JNIEnv* env, jclass, jlong det)
+{
+  delete reinterpret_cast<AprilTagDetector*>(det);
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    setDetectorConfig
+ * Signature: (JLjava/lang/Object;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_setDetectorConfig
+  (JNIEnv* env, jclass, jlong det, jobject config)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return;
+  }
+  reinterpret_cast<AprilTagDetector*>(det)->SetConfig(
+      FromJavaDetectorConfig(env, config));
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    getDetectorConfig
+ * Signature: (J)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_getDetectorConfig
+  (JNIEnv* env, jclass, jlong det)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return nullptr;
+  }
+  return MakeJObject(env,
+                     reinterpret_cast<AprilTagDetector*>(det)->GetConfig());
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    setDetectorQTP
+ * Signature: (JLjava/lang/Object;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_setDetectorQTP
+  (JNIEnv* env, jclass, jlong det, jobject params)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return;
+  }
+  reinterpret_cast<AprilTagDetector*>(det)->SetQuadThresholdParameters(
+      FromJavaDetectorQTP(env, params));
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    getDetectorQTP
+ * Signature: (J)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_getDetectorQTP
+  (JNIEnv* env, jclass, jlong det)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return nullptr;
+  }
+  return MakeJObject(
+      env,
+      reinterpret_cast<AprilTagDetector*>(det)->GetQuadThresholdParameters());
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    addFamily
+ * Signature: (JLjava/lang/String;I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_addFamily
+  (JNIEnv* env, jclass, jlong det, jstring fam, jint bitsCorrected)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return false;
+  }
+  if (!fam) {
+    nullPointerEx.Throw(env, "fam cannot be null");
+    return false;
+  }
+  return reinterpret_cast<AprilTagDetector*>(det)->AddFamily(
+      JStringRef{env, fam}, bitsCorrected);
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    removeFamily
+ * Signature: (JLjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_removeFamily
+  (JNIEnv* env, jclass, jlong det, jstring fam)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return;
+  }
+  if (!fam) {
+    nullPointerEx.Throw(env, "fam cannot be null");
+    return;
+  }
+  reinterpret_cast<AprilTagDetector*>(det)->RemoveFamily(JStringRef{env, fam});
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    clearFamilies
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_clearFamilies
+  (JNIEnv* env, jclass, jlong det)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return;
+  }
+  reinterpret_cast<AprilTagDetector*>(det)->ClearFamilies();
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    detect
+ * Signature: (JIIIJ)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_detect
+  (JNIEnv* env, jclass, jlong det, jint width, jint height, jint stride,
+   jlong bufAddr)
+{
+  if (det == 0) {
+    nullPointerEx.Throw(env, "det cannot be null");
+    return nullptr;
+  }
+  if (bufAddr == 0) {
+    nullPointerEx.Throw(env, "bufAddr cannot be null");
+    return nullptr;
+  }
+  return MakeJObject(
+      env, reinterpret_cast<AprilTagDetector*>(det)->Detect(
+               width, height, stride, reinterpret_cast<uint8_t*>(bufAddr)));
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    estimatePoseHomography
+ * Signature: ([DDDDDD)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePoseHomography
+  (JNIEnv* env, jclass, jdoubleArray homography, jdouble tagSize, jdouble fx,
+   jdouble fy, jdouble cx, jdouble cy)
+{
+  if (!homography) {
+    nullPointerEx.Throw(env, "homography cannot be null");
+    return nullptr;
+  }
+  JSpan<const jdouble, 9> harr{env, homography};
+  if (harr.size() != 9) {
+    illegalArgEx.Throw(env, "homography array must be size 9");
+    return nullptr;
+  }
+
+  AprilTagPoseEstimator estimator({units::meter_t{tagSize}, fx, fy, cx, cy});
+  return MakeJObject(env, estimator.EstimateHomography(harr));
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    estimatePoseOrthogonalIteration
+ * Signature: ([D[DDDDDDI)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePoseOrthogonalIteration
+  (JNIEnv* env, jclass, jdoubleArray homography, jdoubleArray corners,
+   jdouble tagSize, jdouble fx, jdouble fy, jdouble cx, jdouble cy, jint nIters)
+{
+  // homography
+  if (!homography) {
+    nullPointerEx.Throw(env, "homography cannot be null");
+    return nullptr;
+  }
+  JSpan<const jdouble, 9> harr{env, homography};
+  if (harr.size() != 9) {
+    illegalArgEx.Throw(env, "homography array must be size 9");
+    return nullptr;
+  }
+
+  // corners
+  if (!corners) {
+    nullPointerEx.Throw(env, "corners cannot be null");
+    return nullptr;
+  }
+  JSpan<const jdouble, 8> carr{env, corners};
+  if (carr.size() != 8) {
+    illegalArgEx.Throw(env, "corners array must be size 8");
+    return nullptr;
+  }
+
+  AprilTagPoseEstimator estimator({units::meter_t{tagSize}, fx, fy, cx, cy});
+  return MakeJObject(env,
+                     estimator.EstimateOrthogonalIteration(harr, carr, nIters));
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    estimatePose
+ * Signature: ([D[DDDDDD)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePose
+  (JNIEnv* env, jclass, jdoubleArray homography, jdoubleArray corners,
+   jdouble tagSize, jdouble fx, jdouble fy, jdouble cx, jdouble cy)
+{
+  // homography
+  if (!homography) {
+    nullPointerEx.Throw(env, "homography cannot be null");
+    return nullptr;
+  }
+  JSpan<const jdouble, 9> harr{env, homography};
+  if (harr.size() != 9) {
+    illegalArgEx.Throw(env, "homography array must be size 9");
+    return nullptr;
+  }
+
+  // corners
+  if (!corners) {
+    nullPointerEx.Throw(env, "corners cannot be null");
+    return nullptr;
+  }
+  JSpan<const jdouble, 8> carr{env, corners};
+  if (carr.size() != 8) {
+    illegalArgEx.Throw(env, "corners array must be size 8");
+    return nullptr;
+  }
+
+  AprilTagPoseEstimator estimator({units::meter_t{tagSize}, fx, fy, cx, cy});
+  return MakeJObject(env, estimator.Estimate(harr, carr));
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    generate16h5AprilTagImage
+ * Signature: (Ljava/lang/Object;JI)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_generate16h5AprilTagImage
+  (JNIEnv* env, jclass, jobject frameObj, jlong framePtr, jint id)
+{
+  auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
+  if (!frame) {
+    nullPointerEx.Throw(env, "frame is null");
+    return;
+  }
+  bool newData = AprilTag::Generate16h5AprilTagImage(frame, id);
+  wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, newData);
+}
+
+/*
+ * Class:     edu_wpi_first_apriltag_jni_AprilTagJNI
+ * Method:    generate36h11AprilTagImage
+ * Signature: (Ljava/lang/Object;JI)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_apriltag_jni_AprilTagJNI_generate36h11AprilTagImage
+  (JNIEnv* env, jclass, jobject frameObj, jlong framePtr, jint id)
+{
+  auto* frame = reinterpret_cast<wpi::RawFrame*>(framePtr);
+  if (!frame) {
+    nullPointerEx.Throw(env, "frame is null");
+    return;
+  }
+  // function might reallocate
+  bool newData = AprilTag::Generate36h11AprilTagImage(frame, id);
+  wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, newData);
+}
+
+}  // extern "C"

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

@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/RawFrame.h>
+#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+
+#include "frc/geometry/Pose3d.h"
+
+namespace frc {
+
+/**
+ * Represents an AprilTag's metadata.
+ */
+struct WPILIB_DLLEXPORT AprilTag {
+  /// The tag's ID.
+  int ID;
+
+  /// The tag's pose.
+  Pose3d pose;
+
+  bool operator==(const AprilTag&) const = default;
+
+  static bool Generate36h11AprilTagImage(wpi::RawFrame* frame, int id);
+  static bool Generate16h5AprilTagImage(wpi::RawFrame* frame, int id);
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const AprilTag& apriltag);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, AprilTag& apriltag);
+
+}  // namespace frc

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

@@ -0,0 +1,160 @@
+// 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 <stdint.h>
+
+#include <span>
+#include <string_view>
+
+#include <Eigen/Core>
+#include <wpi/SymbolExports.h>
+
+namespace frc {
+
+/**
+ * A detection of an AprilTag tag.
+ */
+class WPILIB_DLLEXPORT AprilTagDetection final {
+ public:
+  AprilTagDetection() = delete;
+  AprilTagDetection(const AprilTagDetection&) = delete;
+  AprilTagDetection& operator=(const AprilTagDetection&) = delete;
+
+  /** A point. Used for center and corner points. */
+  struct Point {
+    double x;
+    double y;
+  };
+
+  /**
+   * Gets the decoded tag's family name.
+   *
+   * @return Decoded family name
+   */
+  std::string_view GetFamily() const;
+
+  /**
+   * Gets the decoded ID of the tag.
+   *
+   * @return Decoded ID
+   */
+  int GetId() const { return id; }
+
+  /**
+   * Gets how many error bits were corrected. Note: accepting large numbers of
+   * corrected errors leads to greatly increased false positive rates.
+   * NOTE: As of this implementation, the detector cannot detect tags with
+   * a hamming distance greater than 2.
+   *
+   * @return Hamming distance (number of corrected error bits)
+   */
+  int GetHamming() const { return hamming; }
+
+  /**
+   * Gets a measure of the quality of the binary decoding process: the
+   * average difference between the intensity of a data bit versus
+   * the decision threshold. Higher numbers roughly indicate better
+   * decodes. This is a reasonable measure of detection accuracy
+   * only for very small tags-- not effective for larger tags (where
+   * we could have sampled anywhere within a bit cell and still
+   * gotten a good detection.)
+   *
+   * @return Decision margin
+   */
+  float GetDecisionMargin() const { return decision_margin; }
+
+  /**
+   * Gets the 3x3 homography matrix describing the projection from an
+   * "ideal" tag (with corners at (-1,1), (1,1), (1,-1), and (-1,
+   * -1)) to pixels in the image.
+   *
+   * @return Homography matrix data
+   */
+  std::span<const double, 9> GetHomography() const;
+
+  /**
+   * Gets the 3x3 homography matrix describing the projection from an
+   * "ideal" tag (with corners at (-1,1), (1,1), (1,-1), and (-1,
+   * -1)) to pixels in the image.
+   *
+   * @return Homography matrix
+   */
+  Eigen::Matrix3d GetHomographyMatrix() const;
+
+  /**
+   * Gets the center of the detection in image pixel coordinates.
+   *
+   * @return Center point
+   */
+  const Point& GetCenter() const { return *reinterpret_cast<const Point*>(c); }
+
+  /**
+   * Gets a corner of the tag in image pixel coordinates. These always
+   * wrap counter-clock wise around the tag. Index 0 is the bottom left corner.
+   *
+   * @param ndx Corner index (range is 0-3, inclusive)
+   * @return Corner point
+   */
+  const Point& GetCorner(int ndx) const {
+    return *reinterpret_cast<const Point*>(p[ndx]);
+  }
+
+  /**
+   * Gets the corners of the tag in image pixel coordinates. These always
+   * wrap counter-clock wise around the tag. The first set of corner coordinates
+   * are the coordinates for the bottom left corner.
+   *
+   * @param cornersBuf Corner point array (X and Y for each corner in order)
+   * @return Corner point array (copy of cornersBuf span)
+   */
+  std::span<double, 8> GetCorners(std::span<double, 8> cornersBuf) const {
+    for (int i = 0; i < 4; i++) {
+      cornersBuf[i * 2] = p[i][0];
+      cornersBuf[i * 2 + 1] = p[i][1];
+    }
+    return cornersBuf;
+  }
+
+ private:
+  // This class *must* be standard-layout-compatible with apriltag_detection
+  // as we use reinterpret_cast from that structure. This means the below
+  // members must exactly match the contents of the apriltag_detection struct.
+
+  // The tag family.
+  void* family;
+
+  // The decoded ID of the tag.
+  int id;
+
+  // How many error bits were corrected? Note: accepting large numbers of
+  // corrected errors leads to greatly increased false positive rates.
+  // NOTE: As of this implementation, the detector cannot detect tags with
+  // a hamming distance greater than 2.
+  int hamming;
+
+  // A measure of the quality of the binary decoding process: the
+  // average difference between the intensity of a data bit versus
+  // the decision threshold. Higher numbers roughly indicate better
+  // decodes. This is a reasonable measure of detection accuracy
+  // only for very small tags-- not effective for larger tags (where
+  // we could have sampled anywhere within a bit cell and still
+  // gotten a good detection.)
+  float decision_margin;
+
+  // The 3x3 homography matrix describing the projection from an
+  // "ideal" tag (with corners at (-1,1), (1,1), (1,-1), and (-1,
+  // -1)) to pixels in the image.
+  void* H;
+
+  // The center of the detection in image pixel coordinates.
+  double c[2];
+
+  // The corners of the tag in image pixel coordinates. These always
+  // wrap counter-clock wise around the tag.
+  double p[4][2];
+};
+
+}  // namespace frc

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

@@ -0,0 +1,262 @@
+// 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 <stdint.h>
+
+#include <memory>
+#include <span>
+#include <string_view>
+#include <utility>
+
+#include <units/angle.h>
+#include <wpi/StringMap.h>
+#include <wpi/SymbolExports.h>
+
+#include "frc/apriltag/AprilTagDetection.h"
+
+namespace frc {
+
+/**
+ * An AprilTag detector engine. This is expensive to set up and tear down, so
+ * most use cases should only create one of these, add a family to it, set up
+ * any other configuration, and repeatedly call Detect().
+ */
+class WPILIB_DLLEXPORT AprilTagDetector {
+ public:
+  /** Detector configuration. */
+  struct Config {
+    bool operator==(const Config&) const = default;
+
+    /**
+     * How many threads should be used for computation. Default is
+     * single-threaded operation (1 thread).
+     */
+    int numThreads = 1;
+
+    /**
+     * Quad decimation. Detection of quads can be done on a lower-resolution
+     * image, improving speed at a cost of pose accuracy and a slight decrease
+     * in detection rate. Decoding the binary payload is still done at full
+     * resolution. Default is 2.0.
+     */
+    float quadDecimate = 2.0f;
+
+    /**
+     * What Gaussian blur should be applied to the segmented image (used for
+     * quad detection). Very noisy images benefit from non-zero values (e.g.
+     * 0.8). Default is 0.0.
+     */
+    float quadSigma = 0.0f;
+
+    /**
+     * When true, the edges of the each quad are adjusted to "snap to" strong
+     * gradients nearby. This is useful when decimation is employed, as it can
+     * increase the quality of the initial quad estimate substantially.
+     * Generally recommended to be on (true). Default is true.
+     *
+     * Very computationally inexpensive. Option is ignored if
+     * quad_decimate = 1.
+     */
+    bool refineEdges = true;
+
+    /**
+     * How much sharpening should be done to decoded images. This can help
+     * decode small tags but may or may not help in odd lighting conditions or
+     * low light conditions. Default is 0.25.
+     */
+    double decodeSharpening = 0.25;
+
+    /**
+     * Debug mode. When true, the decoder writes a variety of debugging images
+     * to the current working directory at various stages through the detection
+     * process. This is slow and should *not* be used on space-limited systems
+     * such as the RoboRIO. Default is disabled (false).
+     */
+    bool debug = false;
+  };
+
+  /** Quad threshold parameters. */
+  struct QuadThresholdParameters {
+    bool operator==(const QuadThresholdParameters&) const = default;
+
+    /**
+     * Threshold used to reject quads containing too few pixels. Default is 5
+     * pixels.
+     */
+    int minClusterPixels = 5;
+
+    /**
+     * How many corner candidates to consider when segmenting a group of pixels
+     * into a quad. Default is 10.
+     */
+    int maxNumMaxima = 10;
+
+    /**
+     * Critical angle. The detector will reject quads where pairs of edges have
+     * angles that are close to straight or close to 180 degrees. Zero means
+     * that no quads are rejected. Default is 10 degrees.
+     */
+    units::radian_t criticalAngle = 10_deg;
+
+    /**
+     * When fitting lines to the contours, the maximum mean squared error
+     * allowed. This is useful in rejecting contours that are far from being
+     * quad shaped; rejecting these quads "early" saves expensive decoding
+     * processing. Default is 10.0.
+     */
+    float maxLineFitMSE = 10.0f;
+
+    /**
+     * Minimum brightness offset. When we build our model of black & white
+     * pixels, we add an extra check that the white model must be (overall)
+     * brighter than the black model. How much brighter? (in pixel values,
+     * [0,255]). Default is 5.
+     */
+    int minWhiteBlackDiff = 5;
+
+    /**
+     * Whether the thresholded image be should be deglitched. Only useful for
+     * very noisy images. Default is disabled (false).
+     */
+    bool deglitch = false;
+  };
+
+  /**
+   * Array of detection results. Each array element is a pointer to an
+   * AprilTagDetection.
+   */
+  class WPILIB_DLLEXPORT Results
+      : public std::span<AprilTagDetection const* const> {
+    struct private_init {};
+    friend class AprilTagDetector;
+
+   public:
+    Results() = default;
+    Results(void* impl, const private_init&);
+    ~Results() { Destroy(); }
+    Results(const Results&) = delete;
+    Results& operator=(const Results&) = delete;
+    Results(Results&& rhs) : span{std::move(rhs)}, m_impl{rhs.m_impl} {
+      rhs.m_impl = nullptr;
+    }
+    Results& operator=(Results&& rhs);
+
+   private:
+    void Destroy();
+    void* m_impl = nullptr;
+  };
+
+  AprilTagDetector();
+  ~AprilTagDetector() { Destroy(); }
+  AprilTagDetector(const AprilTagDetector&) = delete;
+  AprilTagDetector& operator=(const AprilTagDetector&) = delete;
+  AprilTagDetector(AprilTagDetector&& rhs)
+      : m_impl{rhs.m_impl},
+        m_families{std::move(rhs.m_families)},
+        m_qtpCriticalAngle{rhs.m_qtpCriticalAngle} {
+    rhs.m_impl = nullptr;
+  }
+  AprilTagDetector& operator=(AprilTagDetector&& rhs);
+
+  /**
+   * @{
+   * @name Configuration functions
+   */
+
+  /**
+   * Sets detector configuration.
+   *
+   * @param config Configuration
+   */
+  void SetConfig(const Config& config);
+
+  /**
+   * Gets detector configuration.
+   *
+   * @return Configuration
+   */
+  Config GetConfig() const;
+
+  /**
+   * Sets quad threshold parameters.
+   *
+   * @param params Parameters
+   */
+  void SetQuadThresholdParameters(const QuadThresholdParameters& params);
+
+  /**
+   * Gets quad threshold parameters.
+   *
+   * @return Parameters
+   */
+  QuadThresholdParameters GetQuadThresholdParameters() const;
+
+  /** @} */
+
+  /**
+   * @{
+   * @name Tag family functions
+   */
+
+  /**
+   * Adds a family of tags to be detected.
+   *
+   * @param fam Family name, e.g. "tag16h5"
+   * @param bitsCorrected Maximum number of bits to correct
+   * @return False if family can't be found
+   */
+  bool AddFamily(std::string_view fam, int bitsCorrected = 2);
+
+  /**
+   * Removes a family of tags from the detector.
+   *
+   * @param fam Family name, e.g. "tag16h5"
+   */
+  void RemoveFamily(std::string_view fam);
+
+  /**
+   * Unregister all families.
+   */
+  void ClearFamilies();
+
+  /** @} */
+
+  /**
+   * Detect tags from an 8-bit image.
+   * The image must be grayscale.
+   *
+   * @param width width of the image
+   * @param height height of the image
+   * @param stride number of bytes between image rows (often the same as width)
+   * @param buf image buffer
+   * @return Results (array of AprilTagDetection pointers)
+   */
+  Results Detect(int width, int height, int stride, uint8_t* buf);
+
+  /**
+   * Detect tags from an 8-bit image.
+   * The image must be grayscale.
+   *
+   * @param width width of the image
+   * @param height height of the image
+   * @param buf image buffer
+   * @return Results (array of AprilTagDetection pointers)
+   */
+  Results Detect(int width, int height, uint8_t* buf) {
+    return Detect(width, height, width, buf);
+  }
+
+ private:
+  void Destroy();
+  void DestroyFamilies();
+  void DestroyFamily(std::string_view name, void* data);
+
+  void* m_impl;
+  wpi::StringMap<void*> m_families;
+  units::radian_t m_qtpCriticalAngle = 10_deg;
+};
+
+}  // namespace frc

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

@@ -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.
+
+#pragma once
+
+#include <opencv2/core/mat.hpp>
+
+#include "frc/apriltag/AprilTagDetector.h"
+
+namespace frc {
+
+inline AprilTagDetector::Results AprilTagDetect(AprilTagDetector& detector,
+                                                cv::Mat& image) {
+  return detector.Detect(image.cols, image.rows, image.data);
+}
+
+}  // namespace frc

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

@@ -0,0 +1,173 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <optional>
+#include <string_view>
+#include <unordered_map>
+#include <vector>
+
+#include <units/length.h>
+#include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
+
+#include "frc/apriltag/AprilTag.h"
+#include "frc/apriltag/AprilTagFields.h"
+#include "frc/geometry/Pose3d.h"
+
+namespace frc {
+/**
+ * Class for representing a layout of AprilTags on a field and reading them from
+ * a JSON format.
+ *
+ * The JSON format contains two top-level objects, "tags" and "field".
+ * The "tags" object is a list of all AprilTags contained within a layout. Each
+ * AprilTag serializes to a JSON object containing an ID and a Pose3d. The
+ * "field" object is a descriptor of the size of the field in meters with
+ * "width" and "length" values.  This is to account for arbitrary field sizes
+ * when transforming the poses.
+ *
+ * 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.
+ *
+ * Tag poses represent the center of the tag, with a zero rotation representing
+ * a tag that is upright and facing away from the (blue) alliance wall (that is,
+ * towards the opposing alliance). */
+class WPILIB_DLLEXPORT AprilTagFieldLayout {
+ public:
+  /**
+   * Common origin positions for the AprilTag coordinate system.
+   */
+  enum class OriginPosition {
+    /// Blue alliance wall, right side.
+    kBlueAllianceWallRightSide,
+    /// Red alliance wall, right side.
+    kRedAllianceWallRightSide,
+  };
+
+  /**
+   * Loads an AprilTagFieldLayout from a predefined field
+   *
+   * @param field The predefined field
+   * @return AprilTagFieldLayout of the field
+   */
+  static AprilTagFieldLayout LoadField(AprilTagField field);
+
+  AprilTagFieldLayout() = default;
+
+  /**
+   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
+   *
+   * @param path Path of the JSON file to import from.
+   */
+  explicit AprilTagFieldLayout(std::string_view path);
+
+  /**
+   * Construct a new AprilTagFieldLayout from a vector of AprilTag objects.
+   *
+   * @param apriltags Vector of AprilTags.
+   * @param fieldLength Length of field the layout is representing.
+   * @param fieldWidth Width of field the layout is representing.
+   */
+  AprilTagFieldLayout(std::vector<AprilTag> apriltags,
+                      units::meter_t fieldLength, units::meter_t fieldWidth);
+
+  /**
+   * Returns the length of the field the layout is representing.
+   * @return length
+   */
+  units::meter_t GetFieldLength() const;
+
+  /**
+   * Returns the length of the field the layout is representing.
+   * @return width
+   */
+  units::meter_t GetFieldWidth() const;
+
+  /**
+   * Returns a vector of all the april tags used in this layout.
+   * @return list of tags
+   */
+  std::vector<AprilTag> GetTags() const;
+
+  /**
+   * Sets the origin based on a predefined enumeration of coordinate frame
+   * origins. The origins are calculated from the field dimensions.
+   *
+   * This transforms the Pose3ds returned by GetTagPose(int) to return the
+   * correct pose relative to a predefined coordinate frame.
+   *
+   * @param origin The predefined origin
+   */
+  void SetOrigin(OriginPosition origin);
+
+  /**
+   * Sets the origin for tag pose transformation.
+   *
+   * This transforms 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);
+
+  /**
+   * Returns the origin used for tag pose transformation.
+   * @return the origin
+   */
+  Pose3d GetOrigin() const;
+
+  /**
+   * Gets an AprilTag pose by its ID.
+   *
+   * @param ID The ID of the tag.
+   * @return The pose corresponding to the ID that was passed in or an empty
+   * optional if a tag with that ID is not found.
+   */
+  std::optional<Pose3d> GetTagPose(int ID) const;
+
+  /**
+   * Serializes an AprilTagFieldLayout to a JSON file.
+   *
+   * @param path The path to write the JSON file to.
+   */
+  void Serialize(std::string_view path);
+
+  /*
+   * Checks equality between this AprilTagFieldLayout and another object.
+   */
+  bool operator==(const AprilTagFieldLayout&) const = default;
+
+ private:
+  std::unordered_map<int, AprilTag> m_apriltags;
+  units::meter_t m_fieldLength;
+  units::meter_t m_fieldWidth;
+  Pose3d m_origin;
+
+  friend WPILIB_DLLEXPORT void to_json(wpi::json& json,
+                                       const AprilTagFieldLayout& layout);
+
+  friend WPILIB_DLLEXPORT void from_json(const wpi::json& json,
+                                         AprilTagFieldLayout& layout);
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const AprilTagFieldLayout& layout);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, AprilTagFieldLayout& layout);
+
+/**
+ * Loads an AprilTagFieldLayout from a predefined field
+ *
+ * @param field The predefined field
+ * @return AprilTagFieldLayout of the field
+ */
+WPILIB_DLLEXPORT AprilTagFieldLayout
+LoadAprilTagLayoutField(AprilTagField field);
+
+}  // namespace frc

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

@@ -0,0 +1,29 @@
+// 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>
+
+namespace frc {
+
+/**
+ * Loadable AprilTag field layouts.
+ */
+enum class AprilTagField {
+  /// 2022 Rapid React.
+  k2022RapidReact,
+  /// 2023 Charged Up.
+  k2023ChargedUp,
+  /// 2024 Crescendo.
+  k2024Crescendo,
+
+  // 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,
+};
+
+}  // namespace frc

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

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Transform3d.h"
+
+namespace frc {
+
+/** A pair of AprilTag pose estimates. */
+struct WPILIB_DLLEXPORT AprilTagPoseEstimate {
+  /** Pose 1. */
+  Transform3d pose1;
+
+  /** Pose 2. */
+  Transform3d pose2;
+
+  /** Object-space error of pose 1. */
+  double error1;
+
+  /** Object-space error of pose 2. */
+  double error2;
+
+  /**
+   * Gets the ratio of pose reprojection errors, called ambiguity. Numbers
+   * above 0.2 are likely to be ambiguous.
+   *
+   * @return The ratio of pose reprojection errors.
+   */
+  double GetAmbiguity() const;
+};
+
+}  // namespace frc

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

@@ -0,0 +1,145 @@
+// 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 <span>
+
+#include <units/length.h>
+#include <wpi/SymbolExports.h>
+
+#include "frc/apriltag/AprilTagPoseEstimate.h"
+#include "frc/geometry/Transform3d.h"
+
+namespace frc {
+
+class AprilTagDetection;
+
+/** Pose estimators for AprilTag tags. */
+class WPILIB_DLLEXPORT AprilTagPoseEstimator {
+ public:
+  /** Configuration for the pose estimator. */
+  struct Config {
+    bool operator==(const Config&) const = default;
+
+    /** The tag size. */
+    units::meter_t tagSize;
+
+    /** Camera horizontal focal length, in pixels. */
+    double fx;
+
+    /** Camera vertical focal length, in pixels. */
+    double fy;
+
+    /** Camera horizontal focal center, in pixels. */
+    double cx;
+
+    /** Camera vertical focal center, in pixels. */
+    double cy;
+  };
+
+  /**
+   * Creates estimator.
+   *
+   * @param config Configuration
+   */
+  explicit AprilTagPoseEstimator(const Config& config) : m_config{config} {}
+
+  /**
+   * Sets estimator configuration.
+   *
+   * @param config Configuration
+   */
+  void SetConfig(const Config& config) { m_config = config; }
+
+  /**
+   * Gets estimator configuration.
+   *
+   * @return Configuration
+   */
+  const Config& GetConfig() const { return m_config; }
+
+  /**
+   * Estimates the pose of the tag using the homography method described in [1].
+   *
+   * @param detection Tag detection
+   * @return Pose estimate
+   */
+  Transform3d EstimateHomography(const AprilTagDetection& detection) const;
+
+  /**
+   * Estimates the pose of the tag using the homography method described in [1].
+   *
+   * @param homography Homography 3x3 matrix data
+   * @return Pose estimate
+   */
+  Transform3d EstimateHomography(std::span<const double, 9> homography) const;
+
+  /**
+   * Estimates the pose of the tag. This returns one or two possible poses for
+   * the tag, along with the object-space error of each.
+   *
+   * This uses the homography method described in [1] for the initial estimate.
+   * Then Orthogonal Iteration [2] is used to refine this estimate. Then [3] is
+   * used to find a potential second local minima and Orthogonal Iteration is
+   * used to refine this second estimate.
+   *
+   * [1]: E. Olson, “Apriltag: A robust and flexible visual fiducial system,” in
+   *      2011 IEEE International Conference on Robotics and Automation,
+   *      May 2011, pp. 3400–3407.
+   * [2]: Lu, G. D. Hager and E. Mjolsness, "Fast and globally convergent pose
+   *      estimation from video images," in IEEE Transactions on Pattern
+   * Analysis and Machine Intelligence, vol. 22, no. 6, pp. 610-622, June 2000.
+   *      doi: 10.1109/34.862199
+   * [3]: Schweighofer and A. Pinz, "Robust Pose Estimation from a Planar
+   * Target," in IEEE Transactions on Pattern Analysis and Machine Intelligence,
+   *      vol. 28, no. 12, pp. 2024-2030, Dec. 2006. doi: 10.1109/TPAMI.2006.252
+   *
+   * @param detection Tag detection
+   * @param nIters Number of iterations
+   * @return Initial and (possibly) second pose estimates
+   */
+  AprilTagPoseEstimate EstimateOrthogonalIteration(
+      const AprilTagDetection& detection, int nIters) const;
+
+  /**
+   * Estimates the pose of the tag. This returns one or two possible poses for
+   * the tag, along with the object-space error of each.
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param corners Corner point array (X and Y for each corner in order)
+   * @param nIters Number of iterations
+   * @return Initial and (possibly) second pose estimates
+   */
+  AprilTagPoseEstimate EstimateOrthogonalIteration(
+      std::span<const double, 9> homography, std::span<const double, 8> corners,
+      int nIters) const;
+
+  /**
+   * Estimates tag pose. This method is an easier to use interface to
+   * EstimatePoseOrthogonalIteration(), running 50 iterations and returning the
+   * pose with the lower object-space error.
+   *
+   * @param detection Tag detection
+   * @return Pose estimate
+   */
+  Transform3d Estimate(const AprilTagDetection& detection) const;
+
+  /**
+   * Estimates tag pose. This method is an easier to use interface to
+   * EstimatePoseOrthogonalIteration(), running 50 iterations and returning the
+   * pose with the lower object-space error.
+   *
+   * @param homography Homography 3x3 matrix data
+   * @param corners Corner point array (X and Y for each corner in order)
+   * @return Pose estimate
+   */
+  Transform3d Estimate(std::span<const double, 9> homography,
+                       std::span<const double, 8> corners) const;
+
+ private:
+  Config m_config;
+};
+
+}  // namespace frc

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

@@ -0,0 +1,440 @@
+{
+  "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/main/native/resources/edu/wpi/first/apriltag/2023-chargedup.json

@@ -0,0 +1,152 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 15.513558,
+          "y": 1.071626,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 15.513558,
+          "y": 2.748026,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 15.513558,
+          "y": 4.424426,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 16.178784,
+          "y": 6.749796,
+          "z": 0.695452
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 0.36195,
+          "y": 6.749796,
+          "z": 0.695452
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 1.02743,
+          "y": 4.424426,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": 1.02743,
+          "y": 2.748026,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": 1.02743,
+          "y": 1.071626,
+          "z": 0.462788
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 16.54175,
+    "width": 8.0137
+  }
+}

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv

@@ -0,0 +1,17 @@
+ID,X,Y,Z,Rotation
+1,593.68,9.68,53.38,120
+2,637.21,34.79,53.38,120
+3,652.73,196.17,57.13,180
+4,652.73,218.42,57.13,180
+5,578.77,323.00,53.38,270
+6,72.5,323.00,53.38,270
+7,-1.50,218.42,57.13,0
+8,-1.50,196.17,57.13,0
+9,14.02,34.79,53.38,60
+10,57.54,9.68,53.38,60
+11,468.69,146.19,52.00,300
+12,468.69,177.10,52.00,60
+13,441.74,161.62,52.00,180
+14,209.48,161.62,52.00,0
+15,182.73,177.10,52.00,120
+16,182.73,146.19,52.00,240

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.json

@@ -0,0 +1,296 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 15.079471999999997,
+          "y": 0.24587199999999998,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 16.185134,
+          "y": 0.883666,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 16.579342,
+          "y": 4.982717999999999,
+          "z": 1.4511020000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 16.579342,
+          "y": 5.547867999999999,
+          "z": 1.4511020000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 14.700757999999999,
+          "y": 8.2042,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 1.8415,
+          "y": 8.2042,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": -0.038099999999999995,
+          "y": 5.547867999999999,
+          "z": 1.4511020000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": -0.038099999999999995,
+          "y": 4.982717999999999,
+          "z": 1.4511020000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 9,
+      "pose": {
+        "translation": {
+          "x": 0.356108,
+          "y": 0.883666,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 1.4615159999999998,
+          "y": 0.24587199999999998,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 11.904726,
+          "y": 3.7132259999999997,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8660254037844387,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 11.904726,
+          "y": 4.49834,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 11.220196,
+          "y": 4.105148,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 5.320792,
+          "y": 4.105148,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 4.641342,
+          "y": 4.49834,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 4.641342,
+          "y": 3.7132259999999997,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.4999999999999998,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844387
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 16.541,
+    "width": 8.211
+  }
+}

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

@@ -0,0 +1,264 @@
+// 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.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.fail;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.ByteArrayOutputStream;
+import java.io.IOException;
+import java.io.InputStream;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.opencv.core.Core;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
+import org.opencv.imgcodecs.Imgcodecs;
+import org.opencv.imgproc.Imgproc;
+
+@SuppressWarnings("PMD.MutableStaticState")
+class AprilTagDetectorTest {
+  @SuppressWarnings("MemberName")
+  AprilTagDetector detector;
+
+  static RuntimeLoader<Core> loader;
+
+  @BeforeAll
+  static void beforeAll() {
+    try {
+      loader =
+          new RuntimeLoader<>(
+              Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
+      loader.loadLibrary();
+    } catch (IOException ex) {
+      fail(ex);
+    }
+  }
+
+  @BeforeEach
+  void beforeEach() {
+    detector = new AprilTagDetector();
+  }
+
+  @AfterEach
+  void afterEach() {
+    detector.close();
+  }
+
+  @Test
+  void testConfigDefaults() {
+    var config = detector.getConfig();
+    assertEquals(new AprilTagDetector.Config(), config);
+  }
+
+  @Test
+  void testQtpDefaults() {
+    var params = detector.getQuadThresholdParameters();
+    assertEquals(new AprilTagDetector.QuadThresholdParameters(), params);
+  }
+
+  @Test
+  void testSetConfigNumThreads() {
+    var newConfig = new AprilTagDetector.Config();
+    newConfig.numThreads = 2;
+    detector.setConfig(newConfig);
+    var config = detector.getConfig();
+    assertEquals(2, config.numThreads);
+  }
+
+  @Test
+  void testQtpMinClusterPixels() {
+    var newParams = new AprilTagDetector.QuadThresholdParameters();
+    newParams.minClusterPixels = 8;
+    detector.setQuadThresholdParameters(newParams);
+    var params = detector.getQuadThresholdParameters();
+    assertEquals(8, params.minClusterPixels);
+  }
+
+  @Test
+  void testAdd16h5() {
+    assertDoesNotThrow(() -> detector.addFamily("tag16h5"));
+    // duplicate addition is also okay
+    assertDoesNotThrow(() -> detector.addFamily("tag16h5"));
+  }
+
+  @Test
+  void testAdd25h9() {
+    assertDoesNotThrow(() -> detector.addFamily("tag25h9"));
+  }
+
+  @Test
+  void testAdd36h11() {
+    assertDoesNotThrow(() -> detector.addFamily("tag36h11"));
+  }
+
+  @Test
+  void testAddMultiple() {
+    assertDoesNotThrow(() -> detector.addFamily("tag16h5"));
+    assertDoesNotThrow(() -> detector.addFamily("tag36h11"));
+  }
+
+  @Test
+  void testRemoveFamily() {
+    // okay to remove non-existent family
+    detector.removeFamily("tag16h5");
+
+    // add and remove
+    detector.addFamily("tag16h5");
+    detector.removeFamily("tag16h5");
+  }
+
+  @SuppressWarnings("PMD.AssignmentInOperand")
+  public Mat loadImage(String resource) throws IOException {
+    Mat encoded;
+    try (InputStream is = getClass().getResource(resource).openStream()) {
+      try (ByteArrayOutputStream os = new ByteArrayOutputStream(is.available())) {
+        byte[] buffer = new byte[4096];
+        int bytesRead;
+        while ((bytesRead = is.read(buffer)) != -1) {
+          os.write(buffer, 0, bytesRead);
+        }
+        encoded = new Mat(1, os.size(), CvType.CV_8U);
+        encoded.put(0, 0, os.toByteArray());
+      }
+    }
+    Mat image = Imgcodecs.imdecode(encoded, Imgcodecs.IMREAD_COLOR);
+    encoded.release();
+    Imgproc.cvtColor(image, image, Imgproc.COLOR_BGR2GRAY);
+    return image;
+  }
+
+  @Test
+  void testDecodeAndPose() {
+    detector.addFamily("tag16h5");
+    detector.addFamily("tag36h11");
+
+    Mat image;
+    try {
+      image = loadImage("tag1_640_480.jpg");
+    } catch (IOException ex) {
+      fail(ex);
+      return;
+    }
+    try {
+      AprilTagDetection[] results = detector.detect(image);
+      assertEquals(1, results.length);
+      assertEquals("tag36h11", results[0].getFamily());
+      assertEquals(1, results[0].getId());
+      assertEquals(0, results[0].getHamming());
+
+      var estimator =
+          new AprilTagPoseEstimator(new AprilTagPoseEstimator.Config(0.2, 500, 500, 320, 240));
+      AprilTagPoseEstimate est = estimator.estimateOrthogonalIteration(results[0], 200);
+      assertEquals(new Transform3d(), est.pose2);
+      Transform3d pose = estimator.estimate(results[0]);
+      assertEquals(est.pose1, pose);
+    } finally {
+      image.release();
+    }
+  }
+
+  /**
+   * This tag is rotated such that the top is closer to the camera than the bottom. In the camera
+   * frame, with +x to the right, this is a rotation about +X by 45 degrees.
+   */
+  @Test
+  void testPoseRotatedX() {
+    detector.addFamily("tag16h5");
+
+    Mat image;
+    try {
+      image = loadImage("tag2_45deg_X.png");
+    } catch (IOException ex) {
+      fail(ex);
+      return;
+    }
+    try {
+      AprilTagDetection[] results = detector.detect(image);
+      assertEquals(1, results.length);
+
+      var estimator =
+          new AprilTagPoseEstimator(
+              new AprilTagPoseEstimator.Config(
+                  0.2, 500, 500, image.cols() / 2.0, image.rows() / 2.0));
+      AprilTagPoseEstimate est = estimator.estimateOrthogonalIteration(results[0], 200);
+
+      assertEquals(Units.degreesToRadians(45), est.pose1.getRotation().getX(), 0.1);
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getY(), 0.1);
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getZ(), 0.1);
+    } finally {
+      image.release();
+    }
+  }
+
+  /**
+   * This tag is rotated such that the right is closer to the camera than the left. In the camera
+   * frame, with +y down, this is a rotation of 45 degrees about +y.
+   */
+  @Test
+  void testPoseRotatedY() {
+    detector.addFamily("tag16h5");
+
+    Mat image;
+    try {
+      image = loadImage("tag2_45deg_y.png");
+    } catch (IOException ex) {
+      fail(ex);
+      return;
+    }
+    try {
+      AprilTagDetection[] results = detector.detect(image);
+      assertEquals(1, results.length);
+
+      var estimator =
+          new AprilTagPoseEstimator(
+              new AprilTagPoseEstimator.Config(
+                  0.2, 500, 500, image.cols() / 2.0, image.rows() / 2.0));
+      AprilTagPoseEstimate est = estimator.estimateOrthogonalIteration(results[0], 200);
+
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getX(), 0.1);
+      assertEquals(Units.degreesToRadians(45), est.pose1.getRotation().getY(), 0.1);
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getZ(), 0.1);
+    } finally {
+      image.release();
+    }
+  }
+
+  /** This tag is facing right at the camera -- no rotation should be observed. */
+  @Test
+  void testPoseStraightOn() {
+    detector.addFamily("tag16h5");
+
+    Mat image;
+    try {
+      image = loadImage("tag2_16h5_straight.png");
+    } catch (IOException ex) {
+      fail(ex);
+      return;
+    }
+    try {
+      AprilTagDetection[] results = detector.detect(image);
+      assertEquals(1, results.length);
+
+      var estimator =
+          new AprilTagPoseEstimator(
+              new AprilTagPoseEstimator.Config(
+                  0.2, 500, 500, image.cols() / 2.0, image.rows() / 2.0));
+      AprilTagPoseEstimate est = estimator.estimateOrthogonalIteration(results[0], 200);
+
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getX(), 0.1);
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getY(), 0.1);
+      assertEquals(Units.degreesToRadians(0), est.pose1.getRotation().getZ(), 0.1);
+    } finally {
+      image.release();
+    }
+  }
+}

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

@@ -0,0 +1,34 @@
+// 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 edu.wpi.first.util.PixelFormat;
+import org.junit.jupiter.api.Test;
+
+class AprilTagGenerationTest {
+  @Test
+  void test36h11() {
+    var frame = AprilTag.generate36h11AprilTagImage(1);
+    assertEquals(PixelFormat.kGray, frame.getPixelFormat());
+    assertEquals(10, frame.getWidth());
+    assertEquals(10, frame.getHeight());
+    int stride = frame.getStride();
+    assertEquals(stride * 10, frame.getSize());
+    // check the diagonal values
+    var data = frame.getData();
+    assertEquals(-1, data.get(stride * 0 + 0)); // outer border is white
+    assertEquals(0, data.get(stride * 1 + 1)); // inner border is black
+    assertEquals(-1, data.get(stride * 2 + 2));
+    assertEquals(-1, data.get(stride * 3 + 3));
+    assertEquals(-1, data.get(stride * 4 + 4));
+    assertEquals(0, data.get(stride * 5 + 5));
+    assertEquals(0, data.get(stride * 6 + 6));
+    assertEquals(-1, data.get(stride * 7 + 7));
+    assertEquals(0, data.get(stride * 8 + 8)); // inner border
+    assertEquals(-1, data.get(stride * 9 + 9)); // outer border
+  }
+}

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

@@ -0,0 +1,46 @@
+// 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 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 java.util.List;
+import org.junit.jupiter.api.Test;
+
+class AprilTagPoseSetOriginTest {
+  @Test
+  void transformationMatches() {
+    var layout =
+        new AprilTagFieldLayout(
+            List.of(
+                new AprilTag(1, new Pose3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0))),
+                new AprilTag(
+                    2,
+                    new Pose3d(
+                        new Translation3d(
+                            Units.feetToMeters(4.0), Units.feetToMeters(4), Units.feetToMeters(4)),
+                        new Rotation3d(0, 0, Units.degreesToRadians(180))))),
+            Units.feetToMeters(54.0),
+            Units.feetToMeters(27.0));
+    layout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide);
+
+    assertEquals(
+        new Pose3d(
+            new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(180.0))),
+        layout.getTagPose(1).orElse(null));
+
+    assertEquals(
+        new Pose3d(
+            new Translation3d(
+                Units.feetToMeters(50.0), Units.feetToMeters(23.0), Units.feetToMeters(4)),
+            new Rotation3d(0.0, 0.0, 0)),
+        layout.getTagPose(2).orElse(null));
+  }
+}

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

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

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

@@ -0,0 +1,71 @@
+// 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.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.loadField(field));
+    assertNotNull(layout);
+  }
+
+  @Test
+  void test2022RapidReact() {
+    AprilTagFieldLayout layout = AprilTagFieldLayout.loadField(AprilTagFields.k2022RapidReact);
+
+    // Blue Hangar Truss - Hub
+    Pose3d expectedPose =
+        new Pose3d(
+            Units.inchesToMeters(127.272),
+            Units.inchesToMeters(216.01),
+            Units.inchesToMeters(67.932),
+            new Rotation3d(0, 0, 0));
+    Optional<Pose3d> maybePose = layout.getTagPose(1);
+    assertTrue(maybePose.isPresent());
+    assertEquals(expectedPose, maybePose.get());
+
+    // Blue Terminal Near Station
+    expectedPose =
+        new Pose3d(
+            Units.inchesToMeters(4.768),
+            Units.inchesToMeters(67.631),
+            Units.inchesToMeters(35.063),
+            new Rotation3d(0, 0, Math.toRadians(46.25)));
+    maybePose = layout.getTagPose(5);
+    assertTrue(maybePose.isPresent());
+    assertEquals(expectedPose, maybePose.get());
+
+    // Upper Hub Blue-Near
+    expectedPose =
+        new Pose3d(
+            Units.inchesToMeters(332.321),
+            Units.inchesToMeters(183.676),
+            Units.inchesToMeters(95.186),
+            new Rotation3d(0, Math.toRadians(26.75), Math.toRadians(69)));
+    maybePose = layout.getTagPose(53);
+    assertTrue(maybePose.isPresent());
+    assertEquals(expectedPose, maybePose.get());
+
+    // Doesn't exist
+    maybePose = layout.getTagPose(54);
+    assertFalse(maybePose.isPresent());
+  }
+}

apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp

@@ -0,0 +1,68 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/apriltag/AprilTagDetector.h"
+
+using namespace frc;
+
+TEST(AprilTagDetectorTest, ConfigDefaults) {
+  AprilTagDetector detector;
+  auto config = detector.GetConfig();
+  ASSERT_EQ(config, AprilTagDetector::Config{});
+}
+
+TEST(AprilTagDetectorTest, QtpDefaults) {
+  AprilTagDetector detector;
+  auto params = detector.GetQuadThresholdParameters();
+  ASSERT_EQ(params, AprilTagDetector::QuadThresholdParameters{});
+}
+
+TEST(AprilTagDetectorTest, SetConfigNumThreads) {
+  AprilTagDetector detector;
+  detector.SetConfig({.numThreads = 2});
+  auto config = detector.GetConfig();
+  ASSERT_EQ(config.numThreads, 2);
+}
+
+TEST(AprilTagDetectorTest, QtpMinClusterPixels) {
+  AprilTagDetector detector;
+  detector.SetQuadThresholdParameters({.minClusterPixels = 8});
+  auto params = detector.GetQuadThresholdParameters();
+  ASSERT_EQ(params.minClusterPixels, 8);
+}
+
+TEST(AprilTagDetectorTest, Add16h5) {
+  AprilTagDetector detector;
+  ASSERT_TRUE(detector.AddFamily("tag16h5"));
+  // duplicate addition is also okay
+  ASSERT_TRUE(detector.AddFamily("tag16h5"));
+}
+
+TEST(AprilTagDetectorTest, Add25h9) {
+  AprilTagDetector detector;
+  ASSERT_TRUE(detector.AddFamily("tag25h9"));
+}
+
+TEST(AprilTagDetectorTest, Add36h11) {
+  AprilTagDetector detector;
+  ASSERT_TRUE(detector.AddFamily("tag36h11"));
+}
+
+TEST(AprilTagDetectorTest, AddMultiple) {
+  AprilTagDetector detector;
+  ASSERT_TRUE(detector.AddFamily("tag16h5"));
+  ASSERT_TRUE(detector.AddFamily("tag36h11"));
+}
+
+TEST(AprilTagDetectorTest, RemoveFamily) {
+  AprilTagDetector detector;
+  // okay to remove non-existent family
+  detector.RemoveFamily("tag16h5");
+
+  // add and remove
+  detector.AddFamily("tag16h5");
+  detector.RemoveFamily("tag16h5");
+}

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

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <vector>
+
+#include <gtest/gtest.h>
+#include <wpi/json.h>
+
+#include "frc/apriltag/AprilTag.h"
+#include "frc/apriltag/AprilTagFieldLayout.h"
+#include "frc/geometry/Pose3d.h"
+
+using namespace frc;
+
+TEST(AprilTagJsonTest, DeserializeMatches) {
+  auto layout = AprilTagFieldLayout{
+      std::vector{
+          AprilTag{1, Pose3d{}},
+          AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}},
+      54_ft, 27_ft};
+
+  AprilTagFieldLayout deserialized;
+  wpi::json json = layout;
+  EXPECT_NO_THROW(deserialized = json.get<AprilTagFieldLayout>());
+  EXPECT_EQ(layout, deserialized);
+}

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

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <vector>
+
+#include <gtest/gtest.h>
+#include <wpi/json.h>
+
+#include "frc/apriltag/AprilTag.h"
+#include "frc/apriltag/AprilTagFieldLayout.h"
+#include "frc/geometry/Pose3d.h"
+
+using namespace frc;
+
+TEST(AprilTagPoseSetOriginTest, TransformationMatches) {
+  auto layout = AprilTagFieldLayout{
+      std::vector<AprilTag>{
+          AprilTag{1,
+                   Pose3d{0_ft, 0_ft, 0_ft, Rotation3d{0_deg, 0_deg, 0_deg}}},
+          AprilTag{
+              2, Pose3d{4_ft, 4_ft, 4_ft, Rotation3d{0_deg, 0_deg, 180_deg}}}},
+      54_ft, 27_ft};
+
+  layout.SetOrigin(
+      AprilTagFieldLayout::OriginPosition::kRedAllianceWallRightSide);
+
+  auto mirrorPose =
+      Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}};
+  EXPECT_EQ(mirrorPose, *layout.GetTagPose(1));
+  mirrorPose = Pose3d{50_ft, 23_ft, 4_ft, Rotation3d{0_deg, 0_deg, 0_deg}};
+  EXPECT_EQ(mirrorPose, *layout.GetTagPose(2));
+}

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

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "frc/apriltag/AprilTagFieldLayout.h"
+#include "frc/apriltag/AprilTagFields.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 =
+      AprilTagFieldLayout::LoadField(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(AprilTagFieldLayout::LoadField(field));
+}
+
+INSTANTIATE_TEST_SUITE_P(ValuesEnumTestInstTests, AllFieldsFixtureTest,
+                         ::testing::ValuesIn(GetAllFields()));
+
+}  // namespace frc

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

@@ -0,0 +1,11 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+int main(int argc, char** argv) {
+  ::testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  return ret;
+}

azure-pipelines-testbench.yaml

@@ -15,7 +15,7 @@ stages:
           vmImage: 'ubuntu-latest'
 
         container:
-          image: wpilib/roborio-cross-ubuntu:2022-18.04
+          image: wpilib/roborio-cross-ubuntu:2023-22.04
 
         timeoutInMinutes: 0
 
@@ -38,6 +38,7 @@ stages:
 
   - stage: TestBench
     displayName: Test Bench
+    condition: false
     jobs:
       - job: Cpp
         displayName: C++

build.gradle

@@ -2,26 +2,29 @@ import edu.wpi.first.toolchain.*
 
 buildscript {
     repositories {
-        mavenCentral()
+        maven {
+            url = 'https://frcmaven.wpi.edu/artifactory/ex-mvn'
+        }
     }
     dependencies {
-        classpath 'com.hubspot.jinjava:jinjava:2.6.0'
+        classpath 'com.hubspot.jinjava:jinjava:2.7.1'
     }
 }
 
 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'
-    id 'net.ltgt.errorprone' version '2.0.2' apply false
-    id 'com.github.johnrengelman.shadow' version '7.1.2' apply false
-    id 'com.diffplug.spotless' version '6.1.2' apply false
-    id 'com.github.spotbugs' version '5.0.4' apply false
+    id 'net.ltgt.errorprone' version '3.1.0' apply false
+    id 'com.github.johnrengelman.shadow' version '8.1.1' apply false
+    id 'com.diffplug.spotless' version '6.20.0' apply false
+    id 'com.github.spotbugs' version '6.0.2' apply false
+    id 'com.google.protobuf' version '0.9.3' apply false
 }
 
 wpilibVersioning.buildServerMode = project.hasProperty('buildServer')
@@ -29,7 +32,9 @@ wpilibVersioning.releaseMode = project.hasProperty('releaseMode')
 
 allprojects {
     repositories {
-        mavenCentral()
+        maven {
+        url = 'https://frcmaven.wpi.edu/artifactory/ex-mvn'
+    }
     }
     if (project.hasProperty('releaseMode')) {
         wpilibRepositories.addAllReleaseRepositories(it)
@@ -45,6 +50,9 @@ buildScan {
     publishAlways()
 }
 
+import com.github.spotbugs.snom.Effort
+ext.spotbugsEffort = Effort.MAX
+
 ext.licenseFile = files("$rootDir/LICENSE.md", "$rootDir/ThirdPartyNotices.txt")
 
 if (project.hasProperty("publishVersion")) {
@@ -83,10 +91,15 @@ task copyAllOutputs(type: Copy) {
 build.dependsOn copyAllOutputs
 copyAllOutputs.dependsOn outputVersions
 
+def copyReleaseOnly = project.hasProperty('ciReleaseOnly')
+
 ext.addTaskToCopyAllOutputs = { task ->
+    if (copyReleaseOnly && task.name.contains('debug')) {
+        return
+    }
     copyAllOutputs.dependsOn task
-    copyAllOutputs.inputs.file task.archivePath
-    copyAllOutputs.from task.archivePath
+    copyAllOutputs.inputs.file task.archiveFile
+    copyAllOutputs.from task.archiveFile
 }
 
 subprojects {
@@ -99,6 +112,13 @@ subprojects {
         subproj.apply plugin: MultiBuilds
     }
 
+    plugins.withType(JavaPlugin) {
+        java {
+            sourceCompatibility = 11
+            targetCompatibility = 11
+        }
+    }
+
     apply from: "${rootDir}/shared/java/javastyle.gradle"
 
     // Disables doclint in java 8.
@@ -110,6 +130,11 @@ subprojects {
         }
     }
 
+    tasks.withType(JavaCompile) {
+        options.compilerArgs.add '-XDstringConcat=inline'
+        options.encoding = 'UTF-8'
+    }
+
     // Enables UTF-8 support in Javadoc
     tasks.withType(Javadoc) {
         options.addStringOption("charset", "utf-8")
@@ -118,8 +143,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)) {
@@ -147,5 +174,5 @@ ext.getCurrentArch = {
 }
 
 wrapper {
-    gradleVersion = '7.3.3'
+    gradleVersion = '8.5'
 }

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:2022.7.1"
+    implementation "edu.wpi.first:native-utils:2024.5.2"
 }

buildSrc/src/main/groovy/MultiBuilds.groovy

@@ -9,6 +9,7 @@ import org.gradle.language.base.internal.ProjectLayout;
 import org.gradle.language.base.plugins.ComponentModelBasePlugin;
 import org.gradle.language.nativeplatform.tasks.AbstractNativeSourceCompileTask;
 import org.gradle.model.ModelMap;
+import org.gradle.internal.os.OperatingSystem
 import edu.wpi.first.toolchain.ToolchainExtension
 import org.gradle.model.Mutate;
 import org.gradle.api.plugins.ExtensionContainer;
@@ -60,12 +61,16 @@ class MultiBuilds implements Plugin<Project> {
     @CompileStatic
     void disableReleaseGoogleTest(BinaryContainer binaries, ProjectLayout projectLayout) {
       def project = (Project) projectLayout.projectIdentifier
-      if (project.hasProperty('testRelease')) {
+      if (project.hasProperty('testOther')) {
         return
       }
+      def check_string = 'release'
+      if (project.hasProperty('buildServer') && !OperatingSystem.current().isWindows()) {
+        check_string = 'debug'
+      }
       binaries.withType(GoogleTestTestSuiteBinarySpec) { oSpec ->
         GoogleTestTestSuiteBinarySpec spec = (GoogleTestTestSuiteBinarySpec) oSpec
-        if (spec.buildType.name == 'release') {
+        if (spec.buildType.name == check_string) {
           Rules.setBuildableFalseDynamically(spec)
         }
       }

buildSrc/src/main/groovy/WPIJREArtifact.groovy

@@ -1,13 +1,14 @@
-import groovy.transform.CompileStatic
-import javax.inject.Inject
-import edu.wpi.first.deployutils.deploy.artifact.MavenArtifact
-import edu.wpi.first.deployutils.deploy.context.DeployContext
-import org.gradle.api.Project
-import edu.wpi.first.deployutils.ActionWrapper
-import edu.wpi.first.deployutils.deploy.target.RemoteTarget
-import edu.wpi.first.deployutils.PredicateWrapper
+import groovy.transform.CompileStatic;
+import javax.inject.Inject;
 
-import java.util.function.Function
+import org.gradle.api.Project;
+
+import edu.wpi.first.deployutils.deploy.CommandDeployResult;
+import edu.wpi.first.deployutils.deploy.artifact.MavenArtifact;
+import edu.wpi.first.deployutils.deploy.context.DeployContext;
+import edu.wpi.first.deployutils.deploy.target.RemoteTarget;
+import edu.wpi.first.deployutils.PredicateWrapper;
+import edu.wpi.first.deployutils.ActionWrapper;
 
 @CompileStatic
 public class WPIJREArtifact extends MavenArtifact {
@@ -17,6 +18,18 @@ public class WPIJREArtifact extends MavenArtifact {
         return configName;
     }
 
+    public boolean isCheckJreVersion() {
+        return checkJreVersion;
+    }
+
+    public void setCheckJreVersion(boolean checkJreVersion) {
+        this.checkJreVersion = checkJreVersion;
+    }
+
+    private boolean checkJreVersion = true;
+
+    private final String artifactLocation = "edu.wpi.first.jdk:roborio-2024:17.0.9u7-1"
+
     @Inject
     public WPIJREArtifact(String name, RemoteTarget target) {
         super(name, target);
@@ -24,10 +37,10 @@ public class WPIJREArtifact extends MavenArtifact {
         this.configName = configName;
         Project project = target.getProject();
         getConfiguration().set(project.getConfigurations().create(configName));
-        getDependency().set(project.getDependencies().add(configName, "edu.wpi.first.jdk:roborio-2022:11.0.12u5-1"));
+        getDependency().set(project.getDependencies().add(configName, artifactLocation));
 
         setOnlyIf(new PredicateWrapper({ DeployContext ctx ->
-            return jreMissing(ctx) || project.hasProperty("force-redeploy-jre");
+            return jreMissing(ctx) || jreOutOfDate(ctx) || project.hasProperty("force-redeploy-jre");
         }));
 
         getDirectory().set("/tmp");
@@ -35,7 +48,7 @@ public class WPIJREArtifact extends MavenArtifact {
 
         getPostdeploy().add(new ActionWrapper({ DeployContext ctx ->
             ctx.getLogger().log("Installing JRE...");
-            ctx.execute("opkg remove frc2022-openjdk*; opkg install /tmp/frcjre.ipk; rm /tmp/frcjre.ipk");
+            ctx.execute("opkg remove frc*-openjdk*; opkg install /tmp/frcjre.ipk; rm /tmp/frcjre.ipk");
             ctx.getLogger().log("JRE Deployed!");
         }));
     }
@@ -44,5 +57,21 @@ public class WPIJREArtifact extends MavenArtifact {
         return ctx.execute("if [[ -f \"/usr/local/frc/JRE/bin/java\" ]]; then echo OK; else echo MISSING; fi").getResult().contains("MISSING");
     }
 
-
+    private boolean jreOutOfDate(DeployContext ctx) {
+        if (!checkJreVersion) {
+            return false;
+        }
+        String version = getDependency().get().getVersion();
+        CommandDeployResult cmdResult = ctx.execute("opkg list-installed | grep openjdk");
+        if (cmdResult.getExitCode() != 0) {
+            ctx.getLogger().log("JRE not found");
+            return false;
+        }
+        String result = cmdResult.getResult().trim();
+        ctx.getLogger().log("Searching for JRE " + version);
+        ctx.getLogger().log("Found JRE " + result);
+        boolean matches = result.contains(version);
+        ctx.getLogger().log(matches ? "JRE Is Correct Version" : "JRE is mismatched. Reinstalling");
+        return !matches;
+    }
 }

cameraserver/.styleguide

@@ -13,6 +13,7 @@ repoRootNameOverride {
 
 includeOtherLibs {
   ^fmt/
+  ^gtest/
   ^hal/
   ^networktables/
   ^opencv2/

cameraserver/CMakeLists.txt

@@ -3,10 +3,10 @@ project(cameraserver)
 include(CompileWarnings)
 include(AddTest)
 
-find_package( OpenCV REQUIRED )
+find_package(OpenCV REQUIRED)
 
 # Java bindings
-if (WITH_JAVA)
+if(WITH_JAVA)
     find_package(Java REQUIRED)
     include(UseJava)
     set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
@@ -15,47 +15,69 @@ if (WITH_JAVA)
 
     set(OPENCV_JAVA_INSTALL_DIR ${OpenCV_INSTALL_PATH}/share/OpenCV/java/)
 
-    find_file(OPENCV_JAR_FILE NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin NO_DEFAULT_PATH)
+    find_file(
+        OPENCV_JAR_FILE
+        NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar
+        PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin
+        NO_DEFAULT_PATH
+    )
 
     file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
 
-    add_jar(cameraserver_jar ${JAVA_SOURCES} INCLUDE_JARS wpiutil_jar cscore_jar ntcore_jar ${OPENCV_JAR_FILE} OUTPUT_NAME cameraserver)
-
-    get_property(CAMERASERVER_JAR_FILE TARGET cameraserver_jar PROPERTY JAR_FILE)
-    install(FILES ${CAMERASERVER_JAR_FILE} DESTINATION "${java_lib_dest}")
-
-    set_property(TARGET cameraserver_jar PROPERTY FOLDER "java")
+    add_jar(
+        cameraserver_jar
+        ${JAVA_SOURCES}
+        INCLUDE_JARS wpiutil_jar cscore_jar ntcore_jar ${OPENCV_JAR_FILE}
+        OUTPUT_NAME cameraserver
+    )
 
+    install_jar(cameraserver_jar DESTINATION ${java_lib_dest})
+    install_jar_exports(
+        TARGETS cameraserver_jar
+        FILE cameraserver_jar.cmake
+        DESTINATION share/cameraserver
+    )
 endif()
 
-file(GLOB_RECURSE
-    cameraserver_native_src src/main/native/cpp/*.cpp)
+if(WITH_JAVA_SOURCE)
+    find_package(Java REQUIRED)
+    include(UseJava)
+    file(GLOB CAMERASERVER_SOURCES src/main/java/edu/wpi/first/cameraserver/*.java)
+    file(GLOB VISION_SOURCES src/main/java/edu/wpi/first/vision/*.java)
+    add_jar(
+        cameraserver_src_jar
+        RESOURCES
+        NAMESPACE "edu/wpi/first/cameraserver" ${CAMERASERVER_SOURCES}
+        NAMESPACE "edu/wpi/first/vision" ${VISION_SOURCES}
+        OUTPUT_NAME cameraserver-sources
+    )
+
+    get_property(CAMERASERVER_SRC_JAR_FILE TARGET cameraserver_src_jar PROPERTY JAR_FILE)
+    install(FILES ${CAMERASERVER_SRC_JAR_FILE} DESTINATION "${java_lib_dest}")
+
+    set_property(TARGET cameraserver_src_jar PROPERTY FOLDER "java")
+endif()
+
+file(GLOB_RECURSE cameraserver_native_src src/main/native/cpp/*.cpp)
 add_library(cameraserver ${cameraserver_native_src})
 set_target_properties(cameraserver PROPERTIES DEBUG_POSTFIX "d")
-target_include_directories(cameraserver PUBLIC
-                $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
-                            $<INSTALL_INTERFACE:${include_dest}/cameraserver>)
+target_include_directories(
+    cameraserver
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+        $<INSTALL_INTERFACE:${include_dest}/cameraserver>
+)
 wpilib_target_warnings(cameraserver)
 target_link_libraries(cameraserver PUBLIC ntcore cscore wpiutil ${OpenCV_LIBS})
 
 set_property(TARGET cameraserver PROPERTY FOLDER "libraries")
 
-install(TARGETS cameraserver EXPORT cameraserver DESTINATION "${main_lib_dest}")
+install(TARGETS cameraserver EXPORT cameraserver)
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/cameraserver")
 
-if (WITH_JAVA AND MSVC)
-    install(TARGETS cameraserver RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
-endif()
-
-if (WITH_FLAT_INSTALL)
-    set (cameraserver_config_dir ${wpilib_dest})
-else()
-    set (cameraserver_config_dir share/cameraserver)
-endif()
-
-configure_file(cameraserver-config.cmake.in ${WPILIB_BINARY_DIR}/cameraserver-config.cmake )
-install(FILES ${WPILIB_BINARY_DIR}/cameraserver-config.cmake DESTINATION ${cameraserver_config_dir})
-install(EXPORT cameraserver DESTINATION ${cameraserver_config_dir})
+configure_file(cameraserver-config.cmake.in ${WPILIB_BINARY_DIR}/cameraserver-config.cmake)
+install(FILES ${WPILIB_BINARY_DIR}/cameraserver-config.cmake DESTINATION share/cameraserver)
+install(EXPORT cameraserver DESTINATION share/cameraserver)
 
 file(GLOB multiCameraServer_src multiCameraServer/src/main/native/cpp/*.cpp)
 add_executable(multiCameraServer ${multiCameraServer_src})
@@ -64,7 +86,7 @@ target_link_libraries(multiCameraServer cameraserver)
 
 set_property(TARGET multiCameraServer PROPERTY FOLDER "examples")
 
-if (WITH_TESTS)
+if(WITH_TESTS)
     wpilib_add_test(cameraserver src/test/native/cpp)
     target_link_libraries(cameraserver_test cameraserver gtest)
 endif()

cameraserver/build.gradle

@@ -11,11 +11,9 @@ apply from: "${rootDir}/shared/javacpp/setupBuild.gradle"
 
 dependencies {
     implementation project(':wpiutil')
+    implementation project(':wpinet')
     implementation project(':ntcore')
     implementation project(':cscore')
-    devImplementation project(':wpiutil')
-    devImplementation project(':ntcore')
-    devImplementation project(':cscore')
 }
 
 ext {
@@ -32,19 +30,6 @@ apply from: "${rootDir}/shared/opencv.gradle"
 
 nativeUtils.exportsConfigs {
     cameraserver {
-        x86ExcludeSymbols = [
-            '_CT??_R0?AV_System_error',
-            '_CT??_R0?AVexception',
-            '_CT??_R0?AVfailure',
-            '_CT??_R0?AVruntime_error',
-            '_CT??_R0?AVsystem_error',
-            '_CTA5?AVfailure',
-            '_TI5?AVfailure',
-            '_CT??_R0?AVout_of_range',
-            '_CTA3?AVout_of_range',
-            '_TI3?AVout_of_range',
-            '_CT??_R0?AVbad_cast'
-        ]
         x64ExcludeSymbols = [
             '_CT??_R0?AV_System_error',
             '_CT??_R0?AVexception',
@@ -68,8 +53,9 @@ model {
             if (!it.buildable || !(it instanceof NativeBinarySpec)) {
                 return
             }
-            lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+            project(':ntcore').addNtcoreDependency(it, 'shared')
             lib project: ':cscore', library: 'cscore', linkage: 'shared'
+            lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
             lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
         }
     }

cameraserver/cameraserver-config.cmake.in

@@ -7,3 +7,6 @@ find_dependency(OpenCV)
 
 @FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/cameraserver.cmake)
+if(@WITH_JAVA@)
+    include(${SELF_DIR}/cameraserver_jar.cmake)
+endif()

cameraserver/multiCameraServer/build.gradle

@@ -18,18 +18,23 @@ ext {
 
 apply from: "${rootDir}/shared/opencv.gradle"
 
-mainClassName = 'edu.wpi.Main'
+application {
+    mainClass = 'edu.wpi.Main'
+}
 
 apply plugin: 'com.github.johnrengelman.shadow'
 
 repositories {
-    mavenCentral()
+    maven {
+        url = 'https://frcmaven.wpi.edu/artifactory/ex-mvn'
+    }
 }
 
 dependencies {
-    implementation 'com.google.code.gson:gson:2.8.9'
+    implementation 'com.google.code.gson:gson:2.10.1'
 
     implementation project(':wpiutil')
+    implementation project(':wpinet')
     implementation project(':ntcore')
     implementation project(':cscore')
     implementation project(':cameraserver')
@@ -38,7 +43,6 @@ dependencies {
 model {
     components {
         multiCameraServerCpp(NativeExecutableSpec) {
-            targetBuildTypes 'release'
             sources {
                 cpp {
                     source {
@@ -53,9 +57,13 @@ model {
             }
             binaries.all { binary ->
                 lib project: ':cameraserver', library: 'cameraserver', linkage: 'static'
-                lib project: ':ntcore', library: 'ntcore', linkage: 'static'
+                project(':ntcore').addNtcoreDependency(binary, 'static')
                 lib project: ':cscore', library: 'cscore', linkage: 'static'
+                lib project: ':wpinet', library: 'wpinet', linkage: 'static'
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+                if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                    nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries')
+                }
             }
         }
     }

cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java

@@ -98,7 +98,7 @@ public final class Main {
     // parse file
     JsonElement top;
     try {
-      top = new JsonParser().parse(Files.newBufferedReader(Paths.get(configFile)));
+      top = JsonParser.parseReader(Files.newBufferedReader(Paths.get(configFile)));
     } catch (IOException ex) {
       System.err.println("could not open '" + configFile + "': " + ex);
       return false;
@@ -175,7 +175,8 @@ public final class Main {
       ntinst.startServer();
     } else {
       System.out.println("Setting up NetworkTables client for team " + team);
-      ntinst.startClientTeam(team);
+      ntinst.setServerTeam(team);
+      ntinst.startClient4("multicameraserver");
     }
 
     // start cameras

cameraserver/multiCameraServer/src/main/native/cpp/main.cpp

@@ -5,13 +5,15 @@
 #include <cstdio>
 #include <string>
 #include <string_view>
+#include <thread>
 #include <vector>
 
+#include <fmt/core.h>
 #include <networktables/NetworkTableInstance.h>
+#include <wpi/MemoryBuffer.h>
 #include <wpi/StringExtras.h>
 #include <wpi/fmt/raw_ostream.h>
 #include <wpi/json.h>
-#include <wpi/raw_istream.h>
 #include <wpi/raw_ostream.h>
 
 #include "cameraserver/CameraServer.h"
@@ -62,10 +64,6 @@ struct CameraConfig {
 
 std::vector<CameraConfig> cameras;
 
-wpi::raw_ostream& ParseError() {
-  return wpi::errs() << "config error in '" << configFile << "': ";
-}
-
 bool ReadCameraConfig(const wpi::json& config) {
   CameraConfig c;
 
@@ -73,7 +71,8 @@ bool ReadCameraConfig(const wpi::json& config) {
   try {
     c.name = config.at("name").get<std::string>();
   } catch (const wpi::json::exception& e) {
-    ParseError() << "could not read camera name: " << e.what() << '\n';
+    fmt::print(stderr, "config error in '{}': could not read camera name: {}\n",
+               configFile, e.what());
     return false;
   }
 
@@ -81,8 +80,9 @@ bool ReadCameraConfig(const wpi::json& config) {
   try {
     c.path = config.at("path").get<std::string>();
   } catch (const wpi::json::exception& e) {
-    ParseError() << "camera '" << c.name
-                 << "': could not read path: " << e.what() << '\n';
+    fmt::print(stderr,
+               "config error in '{}': camera '{}': could not read path: {}\n",
+               configFile, c.name, e.what());
     return false;
   }
 
@@ -95,25 +95,27 @@ bool ReadCameraConfig(const wpi::json& config) {
 bool ReadConfig() {
   // open config file
   std::error_code ec;
-  wpi::raw_fd_istream is(configFile, ec);
-  if (ec) {
-    wpi::errs() << "could not open '" << configFile << "': " << ec.message()
-                << '\n';
+  std::unique_ptr<wpi::MemoryBuffer> fileBuffer =
+      wpi::MemoryBuffer::GetFile(configFile, ec);
+  if (fileBuffer == nullptr || ec) {
+    fmt::print(stderr, "could not open '{}': {}\n", configFile, ec.message());
     return false;
   }
 
   // parse file
   wpi::json j;
   try {
-    j = wpi::json::parse(is);
+    j = wpi::json::parse(fileBuffer->GetCharBuffer());
   } catch (const wpi::json::parse_error& e) {
-    fmt::print(ParseError(), "byte {}: {}\n", e.byte, e.what());
+    fmt::print(stderr, "config error in '{}': byte {}: {}\n", configFile,
+               e.byte, e.what());
     return false;
   }
 
   // top level must be an object
   if (!j.is_object()) {
-    ParseError() << "must be JSON object\n";
+    fmt::print(stderr, "config error in '{}': must be JSON object\n",
+               configFile);
     return false;
   }
 
@@ -121,7 +123,8 @@ bool ReadConfig() {
   try {
     team = j.at("team").get<unsigned int>();
   } catch (const wpi::json::exception& e) {
-    ParseError() << "could not read team number: " << e.what() << '\n';
+    fmt::print(stderr, "config error in '{}': could not read team number: {}\n",
+               configFile, e.what());
     return false;
   }
 
@@ -134,10 +137,14 @@ bool ReadConfig() {
       } else if (wpi::equals_lower(str, "server")) {
         server = true;
       } else {
-        ParseError() << "could not understand ntmode value '" << str << "'\n";
+        fmt::print(
+            stderr,
+            "config error in '{}': could not understand ntmode value '{}'\n",
+            configFile, str);
       }
     } catch (const wpi::json::exception& e) {
-      ParseError() << "could not read ntmode: " << e.what() << '\n';
+      fmt::print(stderr, "config error in '{}': could not read ntmode: {}\n",
+                 configFile, e.what());
     }
   }
 
@@ -149,7 +156,8 @@ bool ReadConfig() {
       }
     }
   } catch (const wpi::json::exception& e) {
-    ParseError() << "could not read cameras: " << e.what() << '\n';
+    fmt::print(stderr, "config error in '{}': could not read cameras: {}\n",
+               configFile, e.what());
     return false;
   }
 
@@ -182,7 +190,8 @@ int main(int argc, char* argv[]) {
     ntinst.StartServer();
   } else {
     fmt::print("Setting up NetworkTables client for team {}\n", team);
-    ntinst.StartClientTeam(team);
+    ntinst.StartClient4("multicameraserver");
+    ntinst.SetServerTeam(team);
   }
 
   // start cameras

cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java

@@ -14,14 +14,20 @@ import edu.wpi.first.cscore.VideoEvent;
 import edu.wpi.first.cscore.VideoException;
 import edu.wpi.first.cscore.VideoListener;
 import edu.wpi.first.cscore.VideoMode;
-import edu.wpi.first.cscore.VideoMode.PixelFormat;
-import edu.wpi.first.cscore.VideoProperty;
 import edu.wpi.first.cscore.VideoSink;
 import edu.wpi.first.cscore.VideoSource;
-import edu.wpi.first.networktables.EntryListenerFlags;
+import edu.wpi.first.networktables.BooleanEntry;
+import edu.wpi.first.networktables.BooleanPublisher;
+import edu.wpi.first.networktables.IntegerEntry;
+import edu.wpi.first.networktables.IntegerPublisher;
 import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StringArrayPublisher;
+import edu.wpi.first.networktables.StringArrayTopic;
+import edu.wpi.first.networktables.StringEntry;
+import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.util.PixelFormat;
+import java.lang.ref.Reference;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.HashMap;
@@ -33,36 +39,174 @@ import java.util.concurrent.atomic.AtomicInteger;
  * Singleton class for creating and keeping camera servers. Also publishes camera information to
  * NetworkTables.
  */
-@SuppressWarnings("PMD.UnusedPrivateField")
 public final class CameraServer {
+  /** CameraServer base port. */
   public static final int kBasePort = 1181;
 
-  @Deprecated public static final int kSize640x480 = 0;
-  @Deprecated public static final int kSize320x240 = 1;
-  @Deprecated public static final int kSize160x120 = 2;
-
   private static final String kPublishName = "/CameraPublisher";
-  private static CameraServer server;
 
-  /**
-   * Get the CameraServer instance.
-   *
-   * @return The CameraServer instance.
-   * @deprecated Use the static methods
-   */
-  @Deprecated
-  public static synchronized CameraServer getInstance() {
-    if (server == null) {
-      server = new CameraServer();
+  private static final class PropertyPublisher implements AutoCloseable {
+    @SuppressWarnings({"PMD.MissingBreakInSwitch", "PMD.ImplicitSwitchFallThrough", "fallthrough"})
+    PropertyPublisher(NetworkTable table, VideoEvent event) {
+      String name;
+      String infoName;
+      if (event.name.startsWith("raw_")) {
+        name = "RawProperty/" + event.name;
+        infoName = "RawPropertyInfo/" + event.name;
+      } else {
+        name = "Property/" + event.name;
+        infoName = "PropertyInfo/" + event.name;
+      }
+
+      try {
+        switch (event.propertyKind) {
+          case kBoolean:
+            m_booleanValueEntry = table.getBooleanTopic(name).getEntry(false);
+            m_booleanValueEntry.setDefault(event.value != 0);
+            break;
+          case kEnum:
+            m_choicesTopic = table.getStringArrayTopic(infoName + "/choices");
+            // fall through
+          case kInteger:
+            m_integerValueEntry = table.getIntegerTopic(name).getEntry(0);
+            m_minPublisher = table.getIntegerTopic(infoName + "/min").publish();
+            m_maxPublisher = table.getIntegerTopic(infoName + "/max").publish();
+            m_stepPublisher = table.getIntegerTopic(infoName + "/step").publish();
+            m_defaultPublisher = table.getIntegerTopic(infoName + "/default").publish();
+
+            m_integerValueEntry.setDefault(event.value);
+            m_minPublisher.set(CameraServerJNI.getPropertyMin(event.propertyHandle));
+            m_maxPublisher.set(CameraServerJNI.getPropertyMax(event.propertyHandle));
+            m_stepPublisher.set(CameraServerJNI.getPropertyStep(event.propertyHandle));
+            m_defaultPublisher.set(CameraServerJNI.getPropertyDefault(event.propertyHandle));
+            break;
+          case kString:
+            m_stringValueEntry = table.getStringTopic(name).getEntry("");
+            m_stringValueEntry.setDefault(event.valueStr);
+            break;
+          default:
+            break;
+        }
+      } catch (VideoException ignored) {
+        // ignore
+      }
     }
-    return server;
+
+    void update(VideoEvent event) {
+      switch (event.propertyKind) {
+        case kBoolean:
+          if (m_booleanValueEntry != null) {
+            m_booleanValueEntry.set(event.value != 0);
+          }
+          break;
+        case kInteger:
+        case kEnum:
+          if (m_integerValueEntry != null) {
+            m_integerValueEntry.set(event.value);
+          }
+          break;
+        case kString:
+          if (m_stringValueEntry != null) {
+            m_stringValueEntry.set(event.valueStr);
+          }
+          break;
+        default:
+          break;
+      }
+    }
+
+    @Override
+    public void close() {
+      if (m_booleanValueEntry != null) {
+        m_booleanValueEntry.close();
+      }
+      if (m_integerValueEntry != null) {
+        m_integerValueEntry.close();
+      }
+      if (m_stringValueEntry != null) {
+        m_stringValueEntry.close();
+      }
+      if (m_minPublisher != null) {
+        m_minPublisher.close();
+      }
+      if (m_maxPublisher != null) {
+        m_maxPublisher.close();
+      }
+      if (m_stepPublisher != null) {
+        m_stepPublisher.close();
+      }
+      if (m_defaultPublisher != null) {
+        m_defaultPublisher.close();
+      }
+      if (m_choicesPublisher != null) {
+        m_choicesPublisher.close();
+      }
+      Reference.reachabilityFence(m_videoListener);
+    }
+
+    BooleanEntry m_booleanValueEntry;
+    IntegerEntry m_integerValueEntry;
+    StringEntry m_stringValueEntry;
+    IntegerPublisher m_minPublisher;
+    IntegerPublisher m_maxPublisher;
+    IntegerPublisher m_stepPublisher;
+    IntegerPublisher m_defaultPublisher;
+    StringArrayTopic m_choicesTopic;
+    StringArrayPublisher m_choicesPublisher;
+  }
+
+  private static final class SourcePublisher implements AutoCloseable {
+    SourcePublisher(NetworkTable table, int sourceHandle) {
+      this.m_table = table;
+      m_sourcePublisher = table.getStringTopic("source").publish();
+      m_descriptionPublisher = table.getStringTopic("description").publish();
+      m_connectedPublisher = table.getBooleanTopic("connected").publish();
+      m_streamsPublisher = table.getStringArrayTopic("streams").publish();
+      m_modeEntry = table.getStringTopic("mode").getEntry("");
+      m_modesPublisher = table.getStringArrayTopic("modes").publish();
+
+      m_sourcePublisher.set(makeSourceValue(sourceHandle));
+      m_descriptionPublisher.set(CameraServerJNI.getSourceDescription(sourceHandle));
+      m_connectedPublisher.set(CameraServerJNI.isSourceConnected(sourceHandle));
+      m_streamsPublisher.set(getSourceStreamValues(sourceHandle));
+
+      try {
+        VideoMode mode = CameraServerJNI.getSourceVideoMode(sourceHandle);
+        m_modeEntry.setDefault(videoModeToString(mode));
+        m_modesPublisher.set(getSourceModeValues(sourceHandle));
+      } catch (VideoException ignored) {
+        // Do nothing. Let the other event handlers update this if there is an error.
+      }
+    }
+
+    @Override
+    public void close() throws Exception {
+      m_sourcePublisher.close();
+      m_descriptionPublisher.close();
+      m_connectedPublisher.close();
+      m_streamsPublisher.close();
+      m_modeEntry.close();
+      m_modesPublisher.close();
+      for (PropertyPublisher pp : m_properties.values()) {
+        pp.close();
+      }
+    }
+
+    final NetworkTable m_table;
+    final StringPublisher m_sourcePublisher;
+    final StringPublisher m_descriptionPublisher;
+    final BooleanPublisher m_connectedPublisher;
+    final StringArrayPublisher m_streamsPublisher;
+    final StringEntry m_modeEntry;
+    final StringArrayPublisher m_modesPublisher;
+    final Map<Integer, PropertyPublisher> m_properties = new HashMap<>();
   }
 
   private static final AtomicInteger m_defaultUsbDevice = new AtomicInteger();
   private static String m_primarySourceName;
   private static final Map<String, VideoSource> m_sources = new HashMap<>();
   private static final Map<String, VideoSink> m_sinks = new HashMap<>();
-  private static final Map<Integer, NetworkTable> m_tables =
+  private static final Map<Integer, SourcePublisher> m_publishers =
       new HashMap<>(); // indexed by source handle
   // source handle indexed by sink handle
   private static final Map<Integer, Integer> m_fixedSources = new HashMap<>();
@@ -81,190 +225,132 @@ public final class CameraServer {
   // - "PropertyInfo/{Property}" - Property supporting information
 
   // Listener for video events
+  @SuppressWarnings("PMD.AvoidCatchingGenericException")
   private static final VideoListener m_videoListener =
       new VideoListener(
           event -> {
-            switch (event.kind) {
-              case kSourceCreated:
-                {
-                  // Create subtable for the camera
-                  NetworkTable table = m_publishTable.getSubTable(event.name);
-                  m_tables.put(event.sourceHandle, table);
-                  table.getEntry("source").setString(makeSourceValue(event.sourceHandle));
-                  table
-                      .getEntry("description")
-                      .setString(CameraServerJNI.getSourceDescription(event.sourceHandle));
-                  table
-                      .getEntry("connected")
-                      .setBoolean(CameraServerJNI.isSourceConnected(event.sourceHandle));
-                  table
-                      .getEntry("streams")
-                      .setStringArray(getSourceStreamValues(event.sourceHandle));
-                  try {
-                    VideoMode mode = CameraServerJNI.getSourceVideoMode(event.sourceHandle);
-                    table.getEntry("mode").setDefaultString(videoModeToString(mode));
-                    table.getEntry("modes").setStringArray(getSourceModeValues(event.sourceHandle));
-                  } catch (VideoException ignored) {
-                    // Do nothing. Let the other event handlers update this if there is an error.
+            synchronized (CameraServer.class) {
+              switch (event.kind) {
+                case kSourceCreated:
+                  {
+                    // Create subtable for the camera
+                    NetworkTable table = m_publishTable.getSubTable(event.name);
+                    m_publishers.put(
+                        event.sourceHandle, new SourcePublisher(table, event.sourceHandle));
+                    break;
                   }
-                  break;
-                }
-              case kSourceDestroyed:
-                {
-                  NetworkTable table = m_tables.get(event.sourceHandle);
-                  if (table != null) {
-                    table.getEntry("source").setString("");
-                    table.getEntry("streams").setStringArray(new String[0]);
-                    table.getEntry("modes").setStringArray(new String[0]);
-                  }
-                  break;
-                }
-              case kSourceConnected:
-                {
-                  NetworkTable table = m_tables.get(event.sourceHandle);
-                  if (table != null) {
-                    // update the description too (as it may have changed)
-                    table
-                        .getEntry("description")
-                        .setString(CameraServerJNI.getSourceDescription(event.sourceHandle));
-                    table.getEntry("connected").setBoolean(true);
-                  }
-                  break;
-                }
-              case kSourceDisconnected:
-                {
-                  NetworkTable table = m_tables.get(event.sourceHandle);
-                  if (table != null) {
-                    table.getEntry("connected").setBoolean(false);
-                  }
-                  break;
-                }
-              case kSourceVideoModesUpdated:
-                {
-                  NetworkTable table = m_tables.get(event.sourceHandle);
-                  if (table != null) {
-                    table.getEntry("modes").setStringArray(getSourceModeValues(event.sourceHandle));
-                  }
-                  break;
-                }
-              case kSourceVideoModeChanged:
-                {
-                  NetworkTable table = m_tables.get(event.sourceHandle);
-                  if (table != null) {
-                    table.getEntry("mode").setString(videoModeToString(event.mode));
-                  }
-                  break;
-                }
-              case kSourcePropertyCreated:
-                {
-                  NetworkTable table = m_tables.get(event.sourceHandle);
-                  if (table != null) {
-                    putSourcePropertyValue(table, event, true);
-                  }
-                  break;
-                }
-              case kSourcePropertyValueUpdated:
-                {
-                  NetworkTable table = m_tables.get(event.sourceHandle);
-                  if (table != null) {
-                    putSourcePropertyValue(table, event, false);
-                  }
-                  break;
-                }
-              case kSourcePropertyChoicesUpdated:
-                {
-                  NetworkTable table = m_tables.get(event.sourceHandle);
-                  if (table != null) {
-                    try {
-                      String[] choices =
-                          CameraServerJNI.getEnumPropertyChoices(event.propertyHandle);
-                      table
-                          .getEntry("PropertyInfo/" + event.name + "/choices")
-                          .setStringArray(choices);
-                    } catch (VideoException ignored) {
-                      // ignore
+                case kSourceDestroyed:
+                  {
+                    SourcePublisher publisher = m_publishers.remove(event.sourceHandle);
+                    if (publisher != null) {
+                      try {
+                        publisher.close();
+                      } catch (Exception e) {
+                        // ignore (nothing we can do about it)
+                      }
                     }
+                    break;
                   }
+                case kSourceConnected:
+                  {
+                    SourcePublisher publisher = m_publishers.get(event.sourceHandle);
+                    if (publisher != null) {
+                      // update the description too (as it may have changed)
+                      publisher.m_descriptionPublisher.set(
+                          CameraServerJNI.getSourceDescription(event.sourceHandle));
+                      publisher.m_connectedPublisher.set(true);
+                    }
+                    break;
+                  }
+                case kSourceDisconnected:
+                  {
+                    SourcePublisher publisher = m_publishers.get(event.sourceHandle);
+                    if (publisher != null) {
+                      publisher.m_connectedPublisher.set(false);
+                    }
+                    break;
+                  }
+                case kSourceVideoModesUpdated:
+                  {
+                    SourcePublisher publisher = m_publishers.get(event.sourceHandle);
+                    if (publisher != null) {
+                      publisher.m_modesPublisher.set(getSourceModeValues(event.sourceHandle));
+                    }
+                    break;
+                  }
+                case kSourceVideoModeChanged:
+                  {
+                    SourcePublisher publisher = m_publishers.get(event.sourceHandle);
+                    if (publisher != null) {
+                      publisher.m_modeEntry.set(videoModeToString(event.mode));
+                    }
+                    break;
+                  }
+                case kSourcePropertyCreated:
+                  {
+                    SourcePublisher publisher = m_publishers.get(event.sourceHandle);
+                    if (publisher != null) {
+                      publisher.m_properties.put(
+                          event.propertyHandle, new PropertyPublisher(publisher.m_table, event));
+                    }
+                    break;
+                  }
+                case kSourcePropertyValueUpdated:
+                  {
+                    SourcePublisher publisher = m_publishers.get(event.sourceHandle);
+                    if (publisher != null) {
+                      PropertyPublisher pp = publisher.m_properties.get(event.propertyHandle);
+                      if (pp != null) {
+                        pp.update(event);
+                      }
+                    }
+                    break;
+                  }
+                case kSourcePropertyChoicesUpdated:
+                  {
+                    SourcePublisher publisher = m_publishers.get(event.sourceHandle);
+                    if (publisher != null) {
+                      PropertyPublisher pp = publisher.m_properties.get(event.propertyHandle);
+                      if (pp != null && pp.m_choicesTopic != null) {
+                        try {
+                          String[] choices =
+                              CameraServerJNI.getEnumPropertyChoices(event.propertyHandle);
+                          if (pp.m_choicesPublisher == null) {
+                            pp.m_choicesPublisher = pp.m_choicesTopic.publish();
+                          }
+                          pp.m_choicesPublisher.set(choices);
+                        } catch (VideoException ignored) {
+                          // ignore (just don't publish choices if we can't get them)
+                        }
+                      }
+                    }
+                    break;
+                  }
+                case kSinkSourceChanged:
+                case kSinkCreated:
+                case kSinkDestroyed:
+                case kNetworkInterfacesChanged:
+                  {
+                    m_addresses = CameraServerJNI.getNetworkInterfaces();
+                    updateStreamValues();
+                    break;
+                  }
+                default:
                   break;
-                }
-              case kSinkSourceChanged:
-              case kSinkCreated:
-              case kSinkDestroyed:
-              case kNetworkInterfacesChanged:
-                {
-                  m_addresses = CameraServerJNI.getNetworkInterfaces();
-                  updateStreamValues();
-                  break;
-                }
-              default:
-                break;
+              }
             }
           },
           0x4fff,
           true);
 
-  private static final int m_tableListener =
-      NetworkTableInstance.getDefault()
-          .addEntryListener(
-              kPublishName + "/",
-              event -> {
-                String relativeKey = event.name.substring(kPublishName.length() + 1);
-
-                // get source (sourceName/...)
-                int subKeyIndex = relativeKey.indexOf('/');
-                if (subKeyIndex == -1) {
-                  return;
-                }
-                String sourceName = relativeKey.substring(0, subKeyIndex);
-                VideoSource source = m_sources.get(sourceName);
-                if (source == null) {
-                  return;
-                }
-
-                // get subkey
-                relativeKey = relativeKey.substring(subKeyIndex + 1);
-
-                // handle standard names
-                String propName;
-                if ("mode".equals(relativeKey)) {
-                  // reset to current mode
-                  event.getEntry().setString(videoModeToString(source.getVideoMode()));
-                  return;
-                } else if (relativeKey.startsWith("Property/")) {
-                  propName = relativeKey.substring(9);
-                } else if (relativeKey.startsWith("RawProperty/")) {
-                  propName = relativeKey.substring(12);
-                } else {
-                  return; // ignore
-                }
-
-                // everything else is a property
-                VideoProperty property = source.getProperty(propName);
-                switch (property.getKind()) {
-                  case kNone:
-                    return;
-                  case kBoolean:
-                    // reset to current setting
-                    event.getEntry().setBoolean(property.get() != 0);
-                    return;
-                  case kInteger:
-                  case kEnum:
-                    // reset to current setting
-                    event.getEntry().setDouble(property.get());
-                    return;
-                  case kString:
-                    // reset to current setting
-                    event.getEntry().setString(property.getString());
-                    return;
-                  default:
-                    return;
-                }
-              },
-              EntryListenerFlags.kImmediate | EntryListenerFlags.kUpdate);
   private static int m_nextPort = kBasePort;
   private static String[] m_addresses = new String[0];
 
-  @SuppressWarnings("MissingJavadocMethod")
+  /**
+   * Return URI of source with the given index.
+   *
+   * @param source Source index.
+   */
   private static String makeSourceValue(int source) {
     switch (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))) {
       case kUsb:
@@ -285,12 +371,21 @@ public final class CameraServer {
     }
   }
 
-  @SuppressWarnings("MissingJavadocMethod")
+  /**
+   * Return URI of stream with the given address and port.
+   *
+   * @param address Stream IP address.
+   * @param port Stream remote port.
+   */
   private static String makeStreamValue(String address, int port) {
     return "mjpg:http://" + address + ":" + port + "/?action=stream";
   }
 
-  @SuppressWarnings("MissingJavadocMethod")
+  /**
+   * Return URI of sink stream with the given index.
+   *
+   * @param sink Sink index.
+   */
   private static synchronized String[] getSinkStreamValues(int sink) {
     // Ignore all but MjpegServer
     if (VideoSink.getKindFromInt(CameraServerJNI.getSinkKind(sink)) != VideoSink.Kind.kMjpeg) {
@@ -320,7 +415,11 @@ public final class CameraServer {
     return values.toArray(new String[0]);
   }
 
-  @SuppressWarnings("MissingJavadocMethod")
+  /**
+   * Return list of stream source URIs for the given source index.
+   *
+   * @param source Source index.
+   */
   private static synchronized String[] getSourceStreamValues(int source) {
     // Ignore all but HttpCamera
     if (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))
@@ -355,7 +454,7 @@ public final class CameraServer {
     return values;
   }
 
-  @SuppressWarnings("MissingJavadocMethod")
+  /** Update list of stream URIs. */
   private static synchronized void updateStreamValues() {
     // Over all the sinks...
     for (VideoSink i : m_sinks.values()) {
@@ -369,8 +468,8 @@ public final class CameraServer {
       if (source == 0) {
         continue;
       }
-      NetworkTable table = m_tables.get(source);
-      if (table != null) {
+      SourcePublisher publisher = m_publishers.get(source);
+      if (publisher != null) {
         // Don't set stream values if this is a HttpCamera passthrough
         if (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))
             == VideoSource.Kind.kHttp) {
@@ -380,7 +479,7 @@ public final class CameraServer {
         // Set table value
         String[] values = getSinkStreamValues(sink);
         if (values.length > 0) {
-          table.getEntry("streams").setStringArray(values);
+          publisher.m_streamsPublisher.set(values);
         }
       }
     }
@@ -390,18 +489,18 @@ public final class CameraServer {
       int source = i.getHandle();
 
       // Get the source's subtable (if none exists, we're done)
-      NetworkTable table = m_tables.get(source);
-      if (table != null) {
+      SourcePublisher publisher = m_publishers.get(source);
+      if (publisher != null) {
         // Set table value
         String[] values = getSourceStreamValues(source);
         if (values.length > 0) {
-          table.getEntry("streams").setStringArray(values);
+          publisher.m_streamsPublisher.set(values);
         }
       }
     }
   }
 
-  @SuppressWarnings("MissingJavadocMethod")
+  /** Provide string description of pixel format. */
   private static String pixelFormatToString(PixelFormat pixelFormat) {
     switch (pixelFormat) {
       case kMJPEG:
@@ -419,9 +518,11 @@ public final class CameraServer {
     }
   }
 
-  /// Provide string description of video mode.
-  /// The returned string is "{width}x{height} {format} {fps} fps".
-  @SuppressWarnings("MissingJavadocMethod")
+  /**
+   * Provide string description of video mode.
+   *
+   * <p>The returned string is "{width}x{height} {format} {fps} fps".
+   */
   private static String videoModeToString(VideoMode mode) {
     return mode.width
         + "x"
@@ -433,7 +534,11 @@ public final class CameraServer {
         + " fps";
   }
 
-  @SuppressWarnings("MissingJavadocMethod")
+  /**
+   * Get list of video modes for the given source handle.
+   *
+   * @param sourceHandle Source handle.
+   */
   private static String[] getSourceModeValues(int sourceHandle) {
     VideoMode[] modes = CameraServerJNI.enumerateSourceVideoModes(sourceHandle);
     String[] modeStrings = new String[modes.length];
@@ -443,63 +548,6 @@ public final class CameraServer {
     return modeStrings;
   }
 
-  @SuppressWarnings("MissingJavadocMethod")
-  private static void putSourcePropertyValue(NetworkTable table, VideoEvent event, boolean isNew) {
-    String name;
-    String infoName;
-    if (event.name.startsWith("raw_")) {
-      name = "RawProperty/" + event.name;
-      infoName = "RawPropertyInfo/" + event.name;
-    } else {
-      name = "Property/" + event.name;
-      infoName = "PropertyInfo/" + event.name;
-    }
-
-    NetworkTableEntry entry = table.getEntry(name);
-    try {
-      switch (event.propertyKind) {
-        case kBoolean:
-          if (isNew) {
-            entry.setDefaultBoolean(event.value != 0);
-          } else {
-            entry.setBoolean(event.value != 0);
-          }
-          break;
-        case kInteger:
-        case kEnum:
-          if (isNew) {
-            entry.setDefaultDouble(event.value);
-            table
-                .getEntry(infoName + "/min")
-                .setDouble(CameraServerJNI.getPropertyMin(event.propertyHandle));
-            table
-                .getEntry(infoName + "/max")
-                .setDouble(CameraServerJNI.getPropertyMax(event.propertyHandle));
-            table
-                .getEntry(infoName + "/step")
-                .setDouble(CameraServerJNI.getPropertyStep(event.propertyHandle));
-            table
-                .getEntry(infoName + "/default")
-                .setDouble(CameraServerJNI.getPropertyDefault(event.propertyHandle));
-          } else {
-            entry.setDouble(event.value);
-          }
-          break;
-        case kString:
-          if (isNew) {
-            entry.setDefaultString(event.valueStr);
-          } else {
-            entry.setString(event.valueStr);
-          }
-          break;
-        default:
-          break;
-      }
-    } catch (VideoException ignored) {
-      // ignore
-    }
-  }
-
   private CameraServer() {}
 
   /**
@@ -641,7 +689,7 @@ public final class CameraServer {
    */
   public static MjpegServer addSwitchedCamera(String name) {
     // create a dummy CvSource
-    CvSource source = new CvSource(name, VideoMode.PixelFormat.kMJPEG, 160, 120, 30);
+    CvSource source = new CvSource(name, PixelFormat.kMJPEG, 160, 120, 30);
     MjpegServer server = startAutomaticCapture(source);
     synchronized (CameraServer.class) {
       m_fixedSources.put(server.getHandle(), source.getHandle());
@@ -700,6 +748,34 @@ public final class CameraServer {
     return newsink;
   }
 
+  /**
+   * Get OpenCV access to the specified camera. This allows you to get images from the camera for
+   * image processing on the roboRIO.
+   *
+   * @param camera Camera (e.g. as returned by startAutomaticCapture).
+   * @param pixelFormat Desired pixelFormat of the camera
+   * @return OpenCV sink for the specified camera
+   */
+  public static CvSink getVideo(VideoSource camera, PixelFormat pixelFormat) {
+    String name = "opencv_" + camera.getName();
+
+    synchronized (CameraServer.class) {
+      VideoSink sink = m_sinks.get(name);
+      if (sink != null) {
+        VideoSink.Kind kind = sink.getKind();
+        if (kind != VideoSink.Kind.kCv) {
+          throw new VideoException("expected OpenCV sink, but got " + kind);
+        }
+        return (CvSink) sink;
+      }
+    }
+
+    CvSink newsink = new CvSink(name, pixelFormat);
+    newsink.setSource(camera);
+    addServer(newsink);
+    return newsink;
+  }
+
   /**
    * Get OpenCV access to the specified camera. This allows you to get images from the camera for
    * image processing on the roboRIO.
@@ -728,7 +804,7 @@ public final class CameraServer {
    * @return OpenCV source for the MJPEG stream
    */
   public static CvSource putVideo(String name, int width, int height) {
-    CvSource source = new CvSource(name, VideoMode.PixelFormat.kMJPEG, width, height, 30);
+    CvSource source = new CvSource(name, PixelFormat.kMJPEG, width, height, 30);
     startAutomaticCapture(source);
     return source;
   }

cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerShared.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.cameraserver;
 
+/** CameraServer shared functions. */
 public interface CameraServerShared {
   /**
    * get the main thread id func.

cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerSharedStore.java

@@ -4,6 +4,7 @@
 
 package edu.wpi.first.cameraserver;
 
+/** Storage for CameraServerShared instance. */
 public final class CameraServerSharedStore {
   private static CameraServerShared cameraServerShared;
 

cameraserver/src/main/java/edu/wpi/first/vision/VisionRunner.java

@@ -14,6 +14,7 @@ import org.opencv.core.Mat;
  * code. The easiest way to use this is to run it in a {@link VisionThread} and use the listener to
  * take snapshots of the pipeline's outputs.
  *
+ * @param <P> Vision pipeline type.
  * @see VisionPipeline
  * @see VisionThread
  * @see <a href="package-summary.html">vision</a>

cameraserver/src/main/java/edu/wpi/first/vision/VisionThread.java

@@ -21,6 +21,7 @@ public class VisionThread extends Thread {
    *
    * @param visionRunner the runner for a vision pipeline
    */
+  @SuppressWarnings("this-escape")
   public VisionThread(VisionRunner<?> visionRunner) {
     super(visionRunner::runForever, "WPILib Vision Thread");
     setDaemon(true);

cameraserver/src/main/java/edu/wpi/first/vision/package-info.java

@@ -9,7 +9,7 @@
  * <p>An example use case for grabbing a yellow tote from 2015 in autonomous: <br>
  *
  * <pre><code>
- * public class Robot extends IterativeRobot
+ * public class Robot extends TimedRobot
  *     implements VisionRunner.Listener&lt;MyFindTotePipeline&gt; {
  *
  *      // A USB camera connected to the roboRIO.

cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp

@@ -8,8 +8,12 @@
 #include <vector>
 
 #include <fmt/format.h>
+#include <networktables/BooleanTopic.h>
+#include <networktables/IntegerTopic.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
+#include <networktables/StringArrayTopic.h>
+#include <networktables/StringTopic.h>
 #include <wpi/DenseMap.h>
 #include <wpi/SmallString.h>
 #include <wpi/StringExtras.h>
@@ -24,9 +28,42 @@ using namespace frc;
 static constexpr char const* kPublishName = "/CameraPublisher";
 
 namespace {
+
+struct Instance;
+
+struct PropertyPublisher {
+  PropertyPublisher(nt::NetworkTable& table, const cs::VideoEvent& event);
+
+  void Update(const cs::VideoEvent& event);
+
+  nt::BooleanEntry booleanValueEntry;
+  nt::IntegerEntry integerValueEntry;
+  nt::StringEntry stringValueEntry;
+  nt::IntegerPublisher minPublisher;
+  nt::IntegerPublisher maxPublisher;
+  nt::IntegerPublisher stepPublisher;
+  nt::IntegerPublisher defaultPublisher;
+  nt::StringArrayTopic choicesTopic;
+  nt::StringArrayPublisher choicesPublisher;
+};
+
+struct SourcePublisher {
+  SourcePublisher(Instance& inst, std::shared_ptr<nt::NetworkTable> table,
+                  CS_Source source);
+
+  std::shared_ptr<nt::NetworkTable> table;
+  nt::StringPublisher sourcePublisher;
+  nt::StringPublisher descriptionPublisher;
+  nt::BooleanPublisher connectedPublisher;
+  nt::StringArrayPublisher streamsPublisher;
+  nt::StringEntry modeEntry;
+  nt::StringArrayPublisher modesPublisher;
+  wpi::DenseMap<CS_Property, PropertyPublisher> properties;
+};
+
 struct Instance {
   Instance();
-  std::shared_ptr<nt::NetworkTable> GetSourceTable(CS_Source source);
+  SourcePublisher* GetPublisher(CS_Source source);
   std::vector<std::string> GetSinkStreamValues(CS_Sink sink);
   std::vector<std::string> GetSourceStreamValues(CS_Source source);
   void UpdateStreamValues();
@@ -37,7 +74,7 @@ struct Instance {
   wpi::StringMap<cs::VideoSource> m_sources;
   wpi::StringMap<cs::VideoSink> m_sinks;
   wpi::DenseMap<CS_Sink, CS_Source> m_fixedSources;
-  wpi::DenseMap<CS_Source, std::shared_ptr<nt::NetworkTable>> m_tables;
+  wpi::DenseMap<CS_Source, SourcePublisher> m_publishers;
   std::shared_ptr<nt::NetworkTable> m_publishTable{
       nt::NetworkTableInstance::GetDefault().GetTable(kPublishName)};
   cs::VideoListener m_videoListener;
@@ -45,6 +82,7 @@ struct Instance {
   int m_nextPort{CameraServer::kBasePort};
   std::vector<std::string> m_addresses;
 };
+
 }  // namespace
 
 static Instance& GetInstance() {
@@ -52,12 +90,6 @@ static Instance& GetInstance() {
   return instance;
 }
 
-CameraServer* CameraServer::GetInstance() {
-  ::GetInstance();
-  static CameraServer instance;
-  return &instance;
-}
-
 static std::string_view MakeSourceValue(CS_Source source,
                                         wpi::SmallVectorImpl<char>& buf) {
   CS_Status status = 0;
@@ -92,9 +124,13 @@ static std::string MakeStreamValue(std::string_view address, int port) {
   return fmt::format("mjpg:http://{}:{}/?action=stream", address, port);
 }
 
-std::shared_ptr<nt::NetworkTable> Instance::GetSourceTable(CS_Source source) {
-  std::scoped_lock lock(m_mutex);
-  return m_tables.lookup(source);
+SourcePublisher* Instance::GetPublisher(CS_Source source) {
+  auto it = m_publishers.find(source);
+  if (it != m_publishers.end()) {
+    return &it->second;
+  } else {
+    return nullptr;
+  }
 }
 
 std::vector<std::string> Instance::GetSinkStreamValues(CS_Sink sink) {
@@ -164,7 +200,6 @@ std::vector<std::string> Instance::GetSourceStreamValues(CS_Source source) {
 }
 
 void Instance::UpdateStreamValues() {
-  std::scoped_lock lock(m_mutex);
   // Over all the sinks...
   for (const auto& i : m_sinks) {
     CS_Status status = 0;
@@ -178,8 +213,7 @@ void Instance::UpdateStreamValues() {
     if (source == 0) {
       continue;
     }
-    auto table = m_tables.lookup(source);
-    if (table) {
+    if (auto publisher = GetPublisher(source)) {
       // Don't set stream values if this is a HttpCamera passthrough
       if (cs::GetSourceKind(source, &status) == CS_SOURCE_HTTP) {
         continue;
@@ -188,7 +222,7 @@ void Instance::UpdateStreamValues() {
       // Set table value
       auto values = GetSinkStreamValues(sink);
       if (!values.empty()) {
-        table->GetEntry("streams").SetStringArray(values);
+        publisher->streamsPublisher.Set(values);
       }
     }
   }
@@ -198,12 +232,11 @@ void Instance::UpdateStreamValues() {
     CS_Source source = i.second.GetHandle();
 
     // Get the source's subtable (if none exists, we're done)
-    auto table = m_tables.lookup(source);
-    if (table) {
+    if (auto publisher = GetPublisher(source)) {
       // Set table value
       auto values = GetSourceStreamValues(source);
       if (!values.empty()) {
-        table->GetEntry("streams").SetStringArray(values);
+        publisher->streamsPublisher.Set(values);
       }
     }
   }
@@ -240,51 +273,71 @@ static std::vector<std::string> GetSourceModeValues(int source) {
   return rv;
 }
 
-static void PutSourcePropertyValue(nt::NetworkTable* table,
-                                   const cs::VideoEvent& event, bool isNew) {
-  std::string_view namePrefix;
-  std::string_view infoPrefix;
+PropertyPublisher::PropertyPublisher(nt::NetworkTable& table,
+                                     const cs::VideoEvent& event) {
+  std::string name;
+  std::string infoName;
   if (wpi::starts_with(event.name, "raw_")) {
-    namePrefix = "RawProperty";
-    infoPrefix = "RawPropertyInfo";
+    name = fmt::format("RawProperty/{}", event.name);
+    infoName = fmt::format("RawPropertyInfo/{}", event.name);
   } else {
-    namePrefix = "Property";
-    infoPrefix = "PropertyInfo";
+    name = fmt::format("Property/{}", event.name);
+    infoName = fmt::format("PropertyInfo/{}", event.name);
   }
 
-  wpi::SmallString<64> buf;
   CS_Status status = 0;
-  nt::NetworkTableEntry entry =
-      table->GetEntry(fmt::format("{}/{}", namePrefix, event.name));
   switch (event.propertyKind) {
     case CS_PROP_BOOLEAN:
-      if (isNew) {
-        entry.SetDefaultBoolean(event.value != 0);
-      } else {
-        entry.SetBoolean(event.value != 0);
+      booleanValueEntry = table.GetBooleanTopic(name).GetEntry(false);
+      booleanValueEntry.SetDefault(event.value != 0);
+      break;
+    case CS_PROP_ENUM:
+      choicesTopic =
+          table.GetStringArrayTopic(fmt::format("{}/choices", infoName));
+      [[fallthrough]];
+    case CS_PROP_INTEGER:
+      integerValueEntry = table.GetIntegerTopic(name).GetEntry(0);
+      minPublisher =
+          table.GetIntegerTopic(fmt::format("{}/min", infoName)).Publish();
+      maxPublisher =
+          table.GetIntegerTopic(fmt::format("{}/max", infoName)).Publish();
+      stepPublisher =
+          table.GetIntegerTopic(fmt::format("{}/step", infoName)).Publish();
+      defaultPublisher =
+          table.GetIntegerTopic(fmt::format("{}/default", infoName)).Publish();
+
+      integerValueEntry.SetDefault(event.value);
+      minPublisher.Set(cs::GetPropertyMin(event.propertyHandle, &status));
+      maxPublisher.Set(cs::GetPropertyMax(event.propertyHandle, &status));
+      stepPublisher.Set(cs::GetPropertyStep(event.propertyHandle, &status));
+      defaultPublisher.Set(
+          cs::GetPropertyDefault(event.propertyHandle, &status));
+      break;
+    case CS_PROP_STRING:
+      stringValueEntry = table.GetStringTopic(name).GetEntry("");
+      stringValueEntry.SetDefault(event.valueStr);
+      break;
+    default:
+      break;
+  }
+}
+
+void PropertyPublisher::Update(const cs::VideoEvent& event) {
+  switch (event.propertyKind) {
+    case CS_PROP_BOOLEAN:
+      if (booleanValueEntry) {
+        booleanValueEntry.Set(event.value != 0);
       }
       break;
     case CS_PROP_INTEGER:
     case CS_PROP_ENUM:
-      if (isNew) {
-        entry.SetDefaultDouble(event.value);
-        table->GetEntry(fmt::format("{}/{}/min", infoPrefix, event.name))
-            .SetDouble(cs::GetPropertyMin(event.propertyHandle, &status));
-        table->GetEntry(fmt::format("{}/{}/max", infoPrefix, event.name))
-            .SetDouble(cs::GetPropertyMax(event.propertyHandle, &status));
-        table->GetEntry(fmt::format("{}/{}/step", infoPrefix, event.name))
-            .SetDouble(cs::GetPropertyStep(event.propertyHandle, &status));
-        table->GetEntry(fmt::format("{}/{}/default", infoPrefix, event.name))
-            .SetDouble(cs::GetPropertyDefault(event.propertyHandle, &status));
-      } else {
-        entry.SetDouble(event.value);
+      if (integerValueEntry) {
+        integerValueEntry.Set(event.value);
       }
       break;
     case CS_PROP_STRING:
-      if (isNew) {
-        entry.SetDefaultString(event.valueStr);
-      } else {
-        entry.SetString(event.valueStr);
+      if (stringValueEntry) {
+        stringValueEntry.Set(event.valueStr);
       }
       break;
     default:
@@ -292,6 +345,28 @@ static void PutSourcePropertyValue(nt::NetworkTable* table,
   }
 }
 
+SourcePublisher::SourcePublisher(Instance& inst,
+                                 std::shared_ptr<nt::NetworkTable> table,
+                                 CS_Source source)
+    : table{table},
+      sourcePublisher{table->GetStringTopic("source").Publish()},
+      descriptionPublisher{table->GetStringTopic("description").Publish()},
+      connectedPublisher{table->GetBooleanTopic("connected").Publish()},
+      streamsPublisher{table->GetStringArrayTopic("streams").Publish()},
+      modeEntry{table->GetStringTopic("mode").GetEntry("")},
+      modesPublisher{table->GetStringArrayTopic("modes").Publish()} {
+  CS_Status status = 0;
+  wpi::SmallString<64> buf;
+  sourcePublisher.Set(MakeSourceValue(source, buf));
+  wpi::SmallString<64> descBuf;
+  descriptionPublisher.Set(cs::GetSourceDescription(source, descBuf, &status));
+  connectedPublisher.Set(cs::IsSourceConnected(source, &status));
+  streamsPublisher.Set(inst.GetSourceStreamValues(source));
+  auto mode = cs::GetSourceVideoMode(source, &status);
+  modeEntry.SetDefault(VideoModeToString(mode));
+  modesPublisher.Set(GetSourceModeValues(source));
+}
+
 Instance::Instance() {
   // We publish sources to NetworkTables using the following structure:
   // "/CameraPublisher/{Source.Name}/" - root
@@ -306,177 +381,88 @@ Instance::Instance() {
 
   // Listener for video events
   m_videoListener = cs::VideoListener{
-      [=](const cs::VideoEvent& event) {
+      [=, this](const cs::VideoEvent& event) {
+        std::scoped_lock lock(m_mutex);
         CS_Status status = 0;
         switch (event.kind) {
           case cs::VideoEvent::kSourceCreated: {
             // Create subtable for the camera
             auto table = m_publishTable->GetSubTable(event.name);
-            {
-              std::scoped_lock lock(m_mutex);
-              m_tables.insert(std::make_pair(event.sourceHandle, table));
-            }
-            wpi::SmallString<64> buf;
-            table->GetEntry("source").SetString(
-                MakeSourceValue(event.sourceHandle, buf));
-            wpi::SmallString<64> descBuf;
-            table->GetEntry("description")
-                .SetString(cs::GetSourceDescription(event.sourceHandle, descBuf,
-                                                    &status));
-            table->GetEntry("connected")
-                .SetBoolean(cs::IsSourceConnected(event.sourceHandle, &status));
-            table->GetEntry("streams").SetStringArray(
-                GetSourceStreamValues(event.sourceHandle));
-            auto mode = cs::GetSourceVideoMode(event.sourceHandle, &status);
-            table->GetEntry("mode").SetDefaultString(VideoModeToString(mode));
-            table->GetEntry("modes").SetStringArray(
-                GetSourceModeValues(event.sourceHandle));
+            m_publishers.insert(
+                {event.sourceHandle,
+                 SourcePublisher{*this, table, event.sourceHandle}});
             break;
           }
-          case cs::VideoEvent::kSourceDestroyed: {
-            auto table = GetSourceTable(event.sourceHandle);
-            if (table) {
-              table->GetEntry("source").SetString("");
-              table->GetEntry("streams").SetStringArray(
-                  std::vector<std::string>{});
-              table->GetEntry("modes").SetStringArray(
-                  std::vector<std::string>{});
-            }
+          case cs::VideoEvent::kSourceDestroyed:
+            m_publishers.erase(event.sourceHandle);
             break;
-          }
-          case cs::VideoEvent::kSourceConnected: {
-            auto table = GetSourceTable(event.sourceHandle);
-            if (table) {
+          case cs::VideoEvent::kSourceConnected:
+            if (auto publisher = GetPublisher(event.sourceHandle)) {
               // update the description too (as it may have changed)
               wpi::SmallString<64> descBuf;
-              table->GetEntry("description")
-                  .SetString(cs::GetSourceDescription(event.sourceHandle,
-                                                      descBuf, &status));
-              table->GetEntry("connected").SetBoolean(true);
+              publisher->descriptionPublisher.Set(cs::GetSourceDescription(
+                  event.sourceHandle, descBuf, &status));
+              publisher->connectedPublisher.Set(true);
             }
             break;
-          }
-          case cs::VideoEvent::kSourceDisconnected: {
-            auto table = GetSourceTable(event.sourceHandle);
-            if (table) {
-              table->GetEntry("connected").SetBoolean(false);
+          case cs::VideoEvent::kSourceDisconnected:
+            if (auto publisher = GetPublisher(event.sourceHandle)) {
+              publisher->connectedPublisher.Set(false);
             }
             break;
-          }
-          case cs::VideoEvent::kSourceVideoModesUpdated: {
-            auto table = GetSourceTable(event.sourceHandle);
-            if (table) {
-              table->GetEntry("modes").SetStringArray(
+          case cs::VideoEvent::kSourceVideoModesUpdated:
+            if (auto publisher = GetPublisher(event.sourceHandle)) {
+              publisher->modesPublisher.Set(
                   GetSourceModeValues(event.sourceHandle));
             }
             break;
-          }
-          case cs::VideoEvent::kSourceVideoModeChanged: {
-            auto table = GetSourceTable(event.sourceHandle);
-            if (table) {
-              table->GetEntry("mode").SetString(VideoModeToString(event.mode));
+          case cs::VideoEvent::kSourceVideoModeChanged:
+            if (auto publisher = GetPublisher(event.sourceHandle)) {
+              publisher->modeEntry.Set(VideoModeToString(event.mode));
             }
             break;
-          }
-          case cs::VideoEvent::kSourcePropertyCreated: {
-            auto table = GetSourceTable(event.sourceHandle);
-            if (table) {
-              PutSourcePropertyValue(table.get(), event, true);
+          case cs::VideoEvent::kSourcePropertyCreated:
+            if (auto publisher = GetPublisher(event.sourceHandle)) {
+              publisher->properties.insert(
+                  {event.propertyHandle,
+                   PropertyPublisher{*publisher->table, event}});
             }
             break;
-          }
-          case cs::VideoEvent::kSourcePropertyValueUpdated: {
-            auto table = GetSourceTable(event.sourceHandle);
-            if (table) {
-              PutSourcePropertyValue(table.get(), event, false);
+          case cs::VideoEvent::kSourcePropertyValueUpdated:
+            if (auto publisher = GetPublisher(event.sourceHandle)) {
+              auto ppIt = publisher->properties.find(event.propertyHandle);
+              if (ppIt != publisher->properties.end()) {
+                ppIt->second.Update(event);
+              }
             }
             break;
-          }
-          case cs::VideoEvent::kSourcePropertyChoicesUpdated: {
-            auto table = GetSourceTable(event.sourceHandle);
-            if (table) {
-              auto choices =
-                  cs::GetEnumPropertyChoices(event.propertyHandle, &status);
-              table
-                  ->GetEntry(fmt::format("PropertyInfo/{}/choices", event.name))
-                  .SetStringArray(choices);
+          case cs::VideoEvent::kSourcePropertyChoicesUpdated:
+            if (auto publisher = GetPublisher(event.sourceHandle)) {
+              auto ppIt = publisher->properties.find(event.propertyHandle);
+              if (ppIt != publisher->properties.end() &&
+                  ppIt->second.choicesTopic) {
+                auto choices =
+                    cs::GetEnumPropertyChoices(event.propertyHandle, &status);
+                if (!ppIt->second.choicesPublisher) {
+                  ppIt->second.choicesPublisher =
+                      ppIt->second.choicesTopic.Publish();
+                }
+                ppIt->second.choicesPublisher.Set(choices);
+              }
             }
             break;
-          }
           case cs::VideoEvent::kSinkSourceChanged:
           case cs::VideoEvent::kSinkCreated:
           case cs::VideoEvent::kSinkDestroyed:
-          case cs::VideoEvent::kNetworkInterfacesChanged: {
+          case cs::VideoEvent::kNetworkInterfacesChanged:
             m_addresses = cs::GetNetworkInterfaces();
             UpdateStreamValues();
             break;
-          }
           default:
             break;
         }
       },
       0x4fff, true};
-
-  // Listener for NetworkTable events
-  // We don't currently support changing settings via NT due to
-  // synchronization issues, so just update to current setting if someone
-  // else tries to change it.
-  wpi::SmallString<64> buf;
-  m_tableListener = nt::NetworkTableInstance::GetDefault().AddEntryListener(
-      fmt::format("{}/", kPublishName),
-      [=](const nt::EntryNotification& event) {
-        auto relativeKey = wpi::drop_front(
-            event.name, std::string_view{kPublishName}.size() + 1);
-
-        // get source (sourceName/...)
-        auto subKeyIndex = relativeKey.find('/');
-        if (subKeyIndex == std::string_view::npos) {
-          return;
-        }
-        auto sourceName = wpi::slice(relativeKey, 0, subKeyIndex);
-        auto sourceIt = m_sources.find(sourceName);
-        if (sourceIt == m_sources.end()) {
-          return;
-        }
-
-        // get subkey
-        relativeKey.remove_prefix(subKeyIndex + 1);
-
-        // handle standard names
-        std::string_view propName;
-        nt::NetworkTableEntry entry{event.entry};
-        if (relativeKey == "mode") {
-          // reset to current mode
-          entry.SetString(VideoModeToString(sourceIt->second.GetVideoMode()));
-          return;
-        } else if (wpi::starts_with(relativeKey, "Property/")) {
-          propName = wpi::substr(relativeKey, 9);
-        } else if (wpi::starts_with(relativeKey, "RawProperty/")) {
-          propName = wpi::substr(relativeKey, 12);
-        } else {
-          return;  // ignore
-        }
-
-        // everything else is a property
-        auto property = sourceIt->second.GetProperty(propName);
-        switch (property.GetKind()) {
-          case cs::VideoProperty::kNone:
-            return;
-          case cs::VideoProperty::kBoolean:
-            entry.SetBoolean(property.Get() != 0);
-            return;
-          case cs::VideoProperty::kInteger:
-          case cs::VideoProperty::kEnum:
-            entry.SetDouble(property.Get());
-            return;
-          case cs::VideoProperty::kString:
-            entry.SetString(property.GetString());
-            return;
-          default:
-            return;
-        }
-      },
-      NT_NOTIFY_IMMEDIATE | NT_NOTIFY_UPDATE);
 }
 
 cs::UsbCamera CameraServer::StartAutomaticCapture() {
@@ -488,6 +474,7 @@ cs::UsbCamera CameraServer::StartAutomaticCapture() {
 }
 
 cs::UsbCamera CameraServer::StartAutomaticCapture(int dev) {
+  ::GetInstance();
   cs::UsbCamera camera{fmt::format("USB Camera {}", dev), dev};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -497,6 +484,7 @@ cs::UsbCamera CameraServer::StartAutomaticCapture(int dev) {
 
 cs::UsbCamera CameraServer::StartAutomaticCapture(std::string_view name,
                                                   int dev) {
+  ::GetInstance();
   cs::UsbCamera camera{name, dev};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -506,6 +494,7 @@ cs::UsbCamera CameraServer::StartAutomaticCapture(std::string_view name,
 
 cs::UsbCamera CameraServer::StartAutomaticCapture(std::string_view name,
                                                   std::string_view path) {
+  ::GetInstance();
   cs::UsbCamera camera{name, path};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -525,12 +514,13 @@ cs::AxisCamera CameraServer::AddAxisCamera(const std::string& host) {
   return AddAxisCamera("Axis Camera", host);
 }
 
-cs::AxisCamera CameraServer::AddAxisCamera(wpi::span<const std::string> hosts) {
+cs::AxisCamera CameraServer::AddAxisCamera(std::span<const std::string> hosts) {
   return AddAxisCamera("Axis Camera", hosts);
 }
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            std::string_view host) {
+  ::GetInstance();
   cs::AxisCamera camera{name, host};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -540,6 +530,7 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            const char* host) {
+  ::GetInstance();
   cs::AxisCamera camera{name, host};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -549,6 +540,7 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            const std::string& host) {
+  ::GetInstance();
   cs::AxisCamera camera{name, host};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -557,7 +549,8 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
 }
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
-                                           wpi::span<const std::string> hosts) {
+                                           std::span<const std::string> hosts) {
+  ::GetInstance();
   cs::AxisCamera camera{name, hosts};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -566,10 +559,11 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
 }
 
 cs::MjpegServer CameraServer::AddSwitchedCamera(std::string_view name) {
+  auto& inst = ::GetInstance();
   // create a dummy CvSource
   cs::CvSource source{name, cs::VideoMode::PixelFormat::kMJPEG, 160, 120, 30};
   cs::MjpegServer server = StartAutomaticCapture(source);
-  ::GetInstance().m_fixedSources[server.GetHandle()] = source.GetHandle();
+  inst.m_fixedSources[server.GetHandle()] = source.GetHandle();
 
   return server;
 }
@@ -615,7 +609,7 @@ cs::CvSink CameraServer::GetVideo(const cs::VideoSource& camera) {
       if (kind != cs::VideoSink::kCv) {
         auto csShared = GetCameraServerShared();
         csShared->SetCameraServerError("expected OpenCV sink, but got {}",
-                                       kind);
+                                       static_cast<int>(kind));
         return cs::CvSink{};
       }
       return *static_cast<cs::CvSink*>(&it->second);
@@ -628,6 +622,33 @@ cs::CvSink CameraServer::GetVideo(const cs::VideoSource& camera) {
   return newsink;
 }
 
+cs::CvSink CameraServer::GetVideo(const cs::VideoSource& camera,
+                                  cs::VideoMode::PixelFormat pixelFormat) {
+  auto& inst = ::GetInstance();
+  wpi::SmallString<64> name{"opencv_"};
+  name += camera.GetName();
+
+  {
+    std::scoped_lock lock(inst.m_mutex);
+    auto it = inst.m_sinks.find(name);
+    if (it != inst.m_sinks.end()) {
+      auto kind = it->second.GetKind();
+      if (kind != cs::VideoSink::kCv) {
+        auto csShared = GetCameraServerShared();
+        csShared->SetCameraServerError("expected OpenCV sink, but got {}",
+                                       static_cast<int>(kind));
+        return cs::CvSink{};
+      }
+      return *static_cast<cs::CvSink*>(&it->second);
+    }
+  }
+
+  cs::CvSink newsink{name.str(), pixelFormat};
+  newsink.SetSource(camera);
+  AddServer(newsink);
+  return newsink;
+}
+
 cs::CvSink CameraServer::GetVideo(std::string_view name) {
   auto& inst = ::GetInstance();
   cs::VideoSource source;
@@ -644,8 +665,26 @@ cs::CvSink CameraServer::GetVideo(std::string_view name) {
   return GetVideo(source);
 }
 
+cs::CvSink CameraServer::GetVideo(std::string_view name,
+                                  cs::VideoMode::PixelFormat pixelFormat) {
+  auto& inst = ::GetInstance();
+  cs::VideoSource source;
+  {
+    std::scoped_lock lock(inst.m_mutex);
+    auto it = inst.m_sources.find(name);
+    if (it == inst.m_sources.end()) {
+      auto csShared = GetCameraServerShared();
+      csShared->SetCameraServerError("could not find camera {}", name);
+      return cs::CvSink{};
+    }
+    source = it->second;
+  }
+  return GetVideo(source, pixelFormat);
+}
+
 cs::CvSource CameraServer::PutVideo(std::string_view name, int width,
                                     int height) {
+  ::GetInstance();
   cs::CvSource source{name, cs::VideoMode::kMJPEG, width, height, 30};
   StartAutomaticCapture(source);
   return source;
@@ -662,6 +701,7 @@ cs::MjpegServer CameraServer::AddServer(std::string_view name) {
 }
 
 cs::MjpegServer CameraServer::AddServer(std::string_view name, int port) {
+  ::GetInstance();
   cs::MjpegServer server{name, port};
   AddServer(server);
   return server;
@@ -721,22 +761,3 @@ void CameraServer::RemoveCamera(std::string_view name) {
   std::scoped_lock lock(inst.m_mutex);
   inst.m_sources.erase(name);
 }
-
-void CameraServer::SetSize(int size) {
-  auto& inst = ::GetInstance();
-  std::scoped_lock lock(inst.m_mutex);
-  if (inst.m_primarySourceName.empty()) {
-    return;
-  }
-  auto it = inst.m_sources.find(inst.m_primarySourceName);
-  if (it == inst.m_sources.end()) {
-    return;
-  }
-  if (size == kSize160x120) {
-    it->second.SetResolution(160, 120);
-  } else if (size == kSize320x240) {
-    it->second.SetResolution(320, 240);
-  } else if (size == kSize640x480) {
-    it->second.SetResolution(640, 480);
-  }
-}

cameraserver/src/main/native/include/cameraserver/CameraServer.h

@@ -6,12 +6,10 @@
 
 #include <stdint.h>
 
+#include <span>
 #include <string>
 #include <string_view>
 
-#include <wpi/deprecated.h>
-#include <wpi/span.h>
-
 #include "cscore.h"
 #include "cscore_cv.h"
 
@@ -24,17 +22,8 @@ namespace frc {
  */
 class CameraServer {
  public:
+  /// CameraServer base port.
   static constexpr uint16_t kBasePort = 1181;
-  static constexpr int kSize640x480 = 0;
-  static constexpr int kSize320x240 = 1;
-  static constexpr int kSize160x120 = 2;
-
-  /**
-   * Get the CameraServer instance.
-   * @deprecated Use the static methods
-   */
-  WPI_DEPRECATED("Use static methods")
-  static CameraServer* GetInstance();
 
   /**
    * Start automatically capturing images to send to the dashboard.
@@ -118,7 +107,7 @@ class CameraServer {
    *
    * @param hosts Array of Camera host IPs/DNS names
    */
-  static cs::AxisCamera AddAxisCamera(wpi::span<const std::string> hosts);
+  static cs::AxisCamera AddAxisCamera(std::span<const std::string> hosts);
 
   /**
    * Adds an Axis IP camera.
@@ -163,7 +152,7 @@ class CameraServer {
    * @param hosts Array of Camera host IPs/DNS names
    */
   static cs::AxisCamera AddAxisCamera(std::string_view name,
-                                      wpi::span<const std::string> hosts);
+                                      std::span<const std::string> hosts);
 
   /**
    * Adds an Axis IP camera.
@@ -200,6 +189,17 @@ class CameraServer {
    */
   static cs::CvSink GetVideo(const cs::VideoSource& camera);
 
+  /**
+   * Get OpenCV access to the specified camera.  This allows you to get
+   * images from the camera for image processing on the roboRIO.
+   *
+   * @param camera Camera (e.g. as returned by startAutomaticCapture).
+   * @param pixelFormat The desired pixelFormat of captured frames from the
+   * camera
+   */
+  static cs::CvSink GetVideo(const cs::VideoSource& camera,
+                             cs::VideoMode::PixelFormat pixelFormat);
+
   /**
    * Get OpenCV access to the specified camera.  This allows you to get
    * images from the camera for image processing on the roboRIO.
@@ -208,6 +208,17 @@ class CameraServer {
    */
   static cs::CvSink GetVideo(std::string_view name);
 
+  /**
+   * Get OpenCV access to the specified camera.  This allows you to get
+   * images from the camera for image processing on the roboRIO.
+   *
+   * @param name Camera name
+   * @param pixelFormat The desired pixelFormat of captured frames from the
+   * camera
+   */
+  static cs::CvSink GetVideo(std::string_view name,
+                             cs::VideoMode::PixelFormat pixelFormat);
+
   /**
    * Create a MJPEG stream with OpenCV input. This can be called to pass custom
    * annotated images to the dashboard.
@@ -276,17 +287,6 @@ class CameraServer {
    */
   static void RemoveCamera(std::string_view name);
 
-  /**
-   * Sets the size of the image to use. Use the public kSize constants to set
-   * the correct mode, or set it directly on a camera and call the appropriate
-   * StartAutomaticCapture method.
-   *
-   * @deprecated Use SetResolution on the UsbCamera returned by
-   *             StartAutomaticCapture() instead.
-   * @param size The size to use
-   */
-  static void SetSize(int size);
-
  private:
   CameraServer() = default;
 };

cameraserver/src/main/native/include/cameraserver/CameraServerShared.h

@@ -27,20 +27,17 @@ class CameraServerShared {
 
   template <typename S, typename... Args>
   inline void SetCameraServerError(const S& format, Args&&... args) {
-    SetCameraServerErrorV(format,
-                          fmt::make_args_checked<Args...>(format, args...));
+    SetCameraServerErrorV(format, fmt::make_format_args(args...));
   }
 
   template <typename S, typename... Args>
   inline void SetVisionRunnerError(const S& format, Args&&... args) {
-    SetVisionRunnerErrorV(format,
-                          fmt::make_args_checked<Args...>(format, args...));
+    SetVisionRunnerErrorV(format, fmt::make_format_args(args...));
   }
 
   template <typename S, typename... Args>
   inline void ReportDriverStationError(const S& format, Args&&... args) {
-    ReportDriverStationErrorV(format,
-                              fmt::make_args_checked<Args...>(format, args...));
+    ReportDriverStationErrorV(format, fmt::make_format_args(args...));
   }
 };
 

cameraserver/src/main/native/include/vision/VisionRunner.inc

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <functional>
+
 #include "vision/VisionRunner.h"
 
 namespace frc {

cmake/modules/AddTest.cmake

@@ -4,10 +4,10 @@ macro(wpilib_add_test name srcdir)
     file(GLOB_RECURSE test_src ${srcdir}/*.cpp)
     add_executable(${name}_test ${test_src})
     wpilib_target_warnings(${name}_test)
-    if (BUILD_SHARED_LIBS)
+    if(BUILD_SHARED_LIBS)
         target_compile_definitions(${name}_test PRIVATE -DGTEST_LINKED_AS_SHARED_LIBRARY)
     endif()
-    if (MSVC)
+    if(MSVC)
         target_compile_options(${name}_test PRIVATE /wd4101 /wd4251)
     endif()
     add_test(NAME ${name} COMMAND ${name}_test)