[wpilib] Counter: Fix default distance per pulse, add distance and rate to C++ (#5796)

- Default distance per pulse in java was 0; 1 is a more reasonable default
- C++ was missing this functionality

.clang-format

@@ -3,110 +3,169 @@ 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:   ''
 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 +192,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,

.gitattributes

@@ -1,4 +1,5 @@
 *.gradle text eol=lf
 *.java text eol=lf
+*.json text eol=lf
 *.md text eol=lf
 *.xml text eol=lf

.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

@@ -14,25 +14,28 @@ jobs:
         include:
           - os: ubuntu-22.04
             name: Linux
-            container: wpilib/roborio-cross-ubuntu:2023-22.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@v3
-
       - 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
+        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.2/protoc-gen-quickbuf_1.3.2_amd64.deb && sudo apt install ./protoc-gen-quickbuf_1.3.2_amd64.deb
 
       - name: Install opencv (macOS)
-        run: brew install opencv
+        run: brew install opencv protobuf@3 ninja
         if: runner.os == 'macOS'
 
       - name: Set up Python 3.8 (macOS)
@@ -41,16 +44,79 @@ jobs:
         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@v3
+
       - 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 . --parallel $(nproc)
+        env:
+          SCCACHE_GHA_ENABLED: "true"
 
       - name: test
         working-directory: build
         run: ctest --output-on-failure
+
+  build-windows:
+    name: "Build - Windows"
+    runs-on: windows-2022
+    steps:
+      - uses: ilammy/msvc-dev-cmd@v1
+
+      - name: Install CMake
+        uses: lukka/get-cmake@v3.27.6
+
+      - name: Run vcpkg
+        uses: lukka/run-vcpkg@v11.1
+        with:
+          vcpkgDirectory: ${{ runner.workspace }}/vcpkg
+          vcpkgGitCommitId: 78b61582c9e093fda56a01ebb654be15a0033897 # HEAD on 2023-08-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@v3
+
+      - 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=ON -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"
+
+      # Build wpiutil at full speed, wpimath depends on wpiutil
+      - name: build wpiutil
+        working-directory: build
+        run: cmake --build . --parallel $(nproc) --target wpiutil/all
+        env:
+          SCCACHE_GHA_ENABLED: "true"
+
+      # Build wpimath slow to prevent OOM
+      - name: build wpimath
+        working-directory: build
+        run: cmake --build . --parallel 1 --target wpimath/all
+        env:
+          SCCACHE_GHA_ENABLED: "true"
+
+      # Build everything else fast
+      - 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

@@ -42,16 +42,10 @@ jobs:
         with:
           distribution: 'zulu'
           java-version: 11
-      - name: Install clang-format
-        run: |
-          wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key | sudo apt-key add -
-          sudo sh -c "echo 'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
-          sudo apt-get update -q
-          sudo apt-get install -y clang-format-14
       - name: Install wpiformat
         run: pip3 install wpiformat
       - name: Run wpiformat
-        run: wpiformat -clang 14
+        run: wpiformat
       - name: Run spotlessApply
         run: ./gradlew spotlessApply
       - name: Commit

.github/workflows/documentation.yml

@@ -26,39 +26,41 @@ jobs:
           java-version: 13
       - 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.7.0
+        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/gradle.yml

@@ -12,7 +12,7 @@ jobs:
       fail-fast: false
       matrix:
         include:
-          - container: wpilib/roborio-cross-ubuntu:2023-22.04
+          - container: wpilib/roborio-cross-ubuntu:2024-22.04
             artifact-name: Athena
             build-options: "-Ponlylinuxathena"
           - container: wpilib/raspbian-cross-ubuntu:bullseye-22.04
@@ -26,8 +26,17 @@ jobs:
             build-options: "-Ponlylinuxx86-64"
     name: "Build - ${{ matrix.artifact-name }}"
     runs-on: ubuntu-22.04
-    container: ${{ matrix.container }}
     steps:
+      - 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@v3
         with:
           fetch-depth: 0
@@ -35,10 +44,16 @@ jobs:
         run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
         if: startsWith(github.ref, 'refs/tags/v')
       - name: Build with Gradle
-        run: ./gradlew build --build-cache -PbuildServer -PskipJavaFormat ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
+        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@v3
         with:
           name: ${{ matrix.artifact-name }}
@@ -46,7 +61,7 @@ jobs:
 
   build-host:
     env:
-      MACOSX_DEPLOYMENT_TARGET: 10.14
+      MACOSX_DEPLOYMENT_TARGET: 11
     strategy:
       fail-fast: false
       matrix:
@@ -57,22 +72,40 @@ jobs:
             task: "build"
             build-options: "-PciDebugOnly --max-workers 1"
             outputs: "build/allOutputs"
+            build-dir: "c:\\work"
           - os: windows-2022
             artifact-name: Win64Release
             architecture: x64
             build-options: "-PciReleaseOnly --max-workers 1"
             task: "copyAllOutputs"
             outputs: "build/allOutputs"
-          - os: macOS-11
+            build-dir: "c:\\work"
+          - os: windows-2022
+            artifact-name: WinArm64Debug
+            architecture: x64
+            task: "build"
+            build-options: "-PciDebugOnly -Pbuildwinarm64 -Ponlywindowsarm64 --max-workers 1"
+            outputs: "build/allOutputs"
+            build-dir: "c:\\work"
+          - os: windows-2022
+            artifact-name: WinArm64Release
+            architecture: x64
+            build-options: "-PciReleaseOnly -Pbuildwinarm64 -Ponlywindowsarm64 --max-workers 1"
+            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:
@@ -105,20 +138,31 @@ jobs:
       - 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 ${{ 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 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')))
+      - name: Check disk free space (Windows)
+        run: wmic logicaldisk get caption, freespace
+        if: matrix.os == 'windows-2022'
       - uses: actions/upload-artifact@v3
         with:
           name: ${{ matrix.artifact-name }}
-          path: ${{ matrix.outputs }}
+          path: ${{ matrix.build-dir }}/${{ matrix.outputs }}
 
   build-documentation:
     name: "Build - Documentation"
@@ -150,27 +194,37 @@ jobs:
     runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v3
+        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@v3
+        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@v3
+        if: |
+          github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
         with:
           distribution: 'zulu'
           java-version: 11
-      - name: Combine
-        if: |
-          !startsWith(github.ref, 'refs/tags/v') &&
-          github.ref != 'refs/heads/main'
-        run: cd combiner && ./gradlew publish -Pallwpilib
-      - name: Combine (Master)
+      - name: Combine (Main)
         if: |
           github.repository_owner == 'wpilibsuite' &&
           github.ref == 'refs/heads/main'
@@ -189,6 +243,9 @@ jobs:
           ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
           ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
       - uses: actions/upload-artifact@v3
+        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

@@ -26,16 +26,10 @@ jobs:
         uses: actions/setup-python@v4
         with:
           python-version: 3.8
-      - name: Install clang-format
-        run: |
-          wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key | sudo apt-key add -
-          sudo sh -c "echo 'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
-          sudo apt-get update -q
-          sudo apt-get install -y clang-format-14
       - name: Install wpiformat
         run: pip3 install wpiformat
       - name: Run
-        run: wpiformat -clang 14
+        run: wpiformat
       - name: Check output
         run: git --no-pager diff --exit-code HEAD
       - name: Generate diff
@@ -70,12 +64,6 @@ jobs:
         uses: actions/setup-python@v4
         with:
           python-version: 3.8
-      - name: Install clang-tidy
-        run: |
-          wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key | sudo apt-key add -
-          sudo sh -c "echo 'deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-14 main' >> /etc/apt/sources.list.d/proposed-repositories.list"
-          sudo apt-get update -q
-          sudo apt-get install -y clang-tidy-14 clang-format-14
       - name: Install wpiformat
         run: pip3 install wpiformat
       - name: Create compile_commands.json
@@ -83,7 +71,7 @@ jobs:
       - name: List changed files
         run: wpiformat -list-changed-files
       - name: Run clang-tidy
-        run: wpiformat -clang 14 -no-format -tidy-changed -compile-commands=build/compile_commands/linuxx86-64 -vv
+        run: wpiformat -no-format -tidy-changed -compile-commands=build/compile_commands/linuxx86-64 -vv
   javaformat:
     name: "Java format"
     runs-on: ubuntu-22.04

.github/workflows/sanitizers.yml

@@ -26,22 +26,33 @@ jobs:
             ctest-flags: ""
     name: "${{ matrix.name }}"
     runs-on: ubuntu-22.04
-    container: wpilib/roborio-cross-ubuntu:2023-22.04
+    container: wpilib/roborio-cross-ubuntu:2024-22.04
     steps:
-      - uses: actions/checkout@v3
-
       - name: Install Dependencies
-        run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv4.5-java python-is-python3 clang-14
+        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.2/protoc-gen-quickbuf_1.3.2_amd64.deb && sudo apt install ./protoc-gen-quickbuf_1.3.2_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@v3
+
       - name: configure
-        run: mkdir build && cd build && cmake -DCMAKE_C_COMPILER:FILEPATH=/usr/bin/clang-14 -DCMAKE_CXX_COMPILER:FILEPATH=/usr/bin/clang++-14 ${{ 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 ${{ matrix.cmake-flags }} ..
+        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

.github/workflows/upstream-utils.yml

@@ -29,10 +29,6 @@ jobs:
         run: |
           git config --global user.email "you@example.com"
           git config --global user.name "Your Name"
-      - name: Run update_drake.py
-        run: |
-          cd upstream_utils
-          ./update_drake.py
       - name: Run update_eigen.py
         run: |
           cd upstream_utils
@@ -41,6 +37,14 @@ jobs:
         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
@@ -61,6 +65,10 @@ jobs:
         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

.gitignore

@@ -9,6 +9,8 @@ simgui-ds.json
 simgui-window.json
 simgui.json
 
+networktables.json
+
 # Created by the jenkins test script
 test-reports
 
@@ -19,6 +21,9 @@ test-reports
 .idea/
 out/
 
+# Fleet
+.fleet
+
 # Created by http://www.gitignore.io
 
 ### Linux ###
@@ -200,6 +205,7 @@ NO
 *.user
 *.userosscache
 *.sln.docstates
+CMakeSettings.json
 
 # Visual C++ cache files
 ipch/

.styleguide

@@ -18,6 +18,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 +29,9 @@ includeOtherLibs {
   ^Eigen/
   ^cameraserver/
   ^cscore
-  ^drake/
   ^fmt/
+  ^gtest/
+  ^google/
   ^hal/
   ^imgui
   ^implot

CMakeLists.txt

@@ -11,10 +11,15 @@ if ("${CMAKE_HOST_SYSTEM_NAME}" STREQUAL "Windows")
     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})
@@ -81,11 +86,6 @@ 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 "
 FATAL: Cannot build static libs with Java enabled.
@@ -155,7 +155,6 @@ endif()
 
 set( wpilib_dest "")
 set( include_dest include )
-set( main_lib_dest lib )
 set( java_lib_dest java )
 set( jni_lib_dest jni )
 
@@ -167,8 +166,7 @@ endif()
 
 if (USE_SYSTEM_LIBUV)
 set (LIBUV_SYSTEM_REPLACE "
-find_package(PkgConfig REQUIRED)
-pkg_check_modules(libuv REQUIRED IMPORTED_TARGET libuv)
+find_dependency(libuv CONFIG)
 ")
 endif()
 
@@ -178,6 +176,12 @@ endif()
 
 find_package(LIBSSH 0.7.1)
 
+find_package(Protobuf REQUIRED)
+find_program(Quickbuf_EXECUTABLE
+    NAMES protoc-gen-quickbuf
+    DOC "The Quickbuf protoc plugin"
+)
+
 if (WITH_FLAT_INSTALL)
 set(WPIUTIL_DEP_REPLACE "include($\{SELF_DIR\}/wpiutil-config.cmake)")
 set(WPINET_DEP_REPLACE "include($\{SELF_DIR\}/wpinet-config.cmake)")
@@ -295,6 +299,7 @@ if (WITH_GUI)
     add_subdirectory(wpigui)
     add_subdirectory(glass)
     add_subdirectory(outlineviewer)
+    add_subdirectory(sysid)
     if (LIBSSH_FOUND)
         add_subdirectory(roborioteamnumbersetter)
         add_subdirectory(datalogtool)
@@ -319,6 +324,8 @@ if (WITH_WPILIB)
     add_subdirectory(wpilibj)
     add_subdirectory(wpilibc)
     add_subdirectory(wpilibNewCommands)
+    add_subdirectory(romiVendordep)
+    add_subdirectory(xrpVendordep)
     if (WITH_EXAMPLES)
         add_subdirectory(wpilibcExamples)
     endif()

CONTRIBUTING.md

@@ -40,8 +40,39 @@ So you want to contribute your changes back to WPILib. Great! We have a few cont
 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

@@ -4,11 +4,16 @@ This article contains instructions on building projects using a development buil
 
 **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 2023 GradleRIO version, ie `2023.0.0-alpha-1`
+To build a project using a development build, find the build.gradle file and open it. Then, add the following code below the plugin section and replace YEAR with the year of the development version. It is also necessary to use a 2024 GradleRIO version, ie `2024.0.0-alpha-1`
 
 ```groovy
 wpi.maven.useLocal = false
@@ -23,13 +28,13 @@ Java
 ```groovy
 plugins {
   id "java"
-  id "edu.wpi.first.GradleRIO" version "2023.0.0-alpha-1"
+  id "edu.wpi.first.GradleRIO" version "2024.0.0-alpha-1"
 }
 
 wpi.maven.useLocal = false
 wpi.maven.useDevelopment = true
-wpi.versions.wpilibVersion = '2023.+'
-wpi.versions.wpimathVersion = '2023.+'
+wpi.versions.wpilibVersion = '2024.+'
+wpi.versions.wpimathVersion = '2024.+'
 ```
 
 C++
@@ -37,13 +42,13 @@ C++
 plugins {
   id "cpp"
   id "google-test-test-suite"
-  id "edu.wpi.first.GradleRIO" version "2023.0.0-alpha-1"
+  id "edu.wpi.first.GradleRIO" version "2024.0.0-alpha-1"
 }
 
 wpi.maven.useLocal = false
 wpi.maven.useDevelopment = true
-wpi.versions.wpilibVersion = '2023.+'
-wpi.versions.wpimathVersion = '2023.+'
+wpi.versions.wpilibVersion = '2024.+'
+wpi.versions.wpimathVersion = '2024.+'
 ```
 
 ### Development Build Documentation
@@ -59,7 +64,7 @@ Java
 ```groovy
 plugins {
   id "java"
-  id "edu.wpi.first.GradleRIO" version "2023.0.0-alpha-1"
+  id "edu.wpi.first.GradleRIO" version "2024.0.0-alpha-1"
 }
 
 wpi.maven.useLocal = false
@@ -73,7 +78,7 @@ C++
 plugins {
   id "cpp"
   id "google-test-test-suite"
-  id "edu.wpi.first.GradleRIO" version "2023.0.0-alpha-1"
+  id "edu.wpi.first.GradleRIO" version "2024.0.0-alpha-1"
 }
 
 wpi.maven.useLocal = false

LICENSE.md

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

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
 

README-CMAKE.md

@@ -12,14 +12,19 @@ WPILib is normally built with Gradle, however for some systems, such as Linux ba
 * halsim
 * wpigui
 * wpimath
+* 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.
 
@@ -93,7 +98,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

@@ -17,6 +17,7 @@ Welcome to the WPILib project. This repository contains the HAL, WPILibJ, and WP
     - [Custom toolchain location](#custom-toolchain-location)
     - [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)
@@ -40,7 +41,7 @@ Using Gradle makes building WPILib very straightforward. It only has a few depen
 
 ## 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
@@ -48,7 +49,7 @@ Using Gradle makes building WPILib very straightforward. It only has a few depen
 - C++ compiler
     - 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`
+    - 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
@@ -61,7 +62,7 @@ On macOS ARM, run `softwareupdate --install-rosetta`. This is necessary to be ab
 
 Clone the WPILib repository and follow the instructions above for installing any required tooling.
 
-See the [styleguide README](https://github.com/wpilibsuite/styleguide/blob/main/README.md) for wpiformat setup instructions. We use clang-format 14.
+See the [styleguide README](https://github.com/wpilibsuite/styleguide/blob/main/README.md) for wpiformat setup instructions.
 
 ## Building
 
@@ -119,7 +120,7 @@ Once a PR has been submitted, formatting can be run in CI by commenting `/format
 
 #### wpiformat
 
-wpiformat can be executed anywhere in the repository via `py -3 -m wpiformat -clang 14` on Windows or `python3 -m wpiformat -clang 14` on other platforms.
+wpiformat can be executed anywhere in the repository via `py -3 -m wpiformat` on Windows or `python3 -m wpiformat` on other platforms.
 
 #### Java Code Quality Tools
 
@@ -131,6 +132,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:

ThirdPartyNotices.txt

@@ -41,9 +41,6 @@ Team 254 Library      wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineP
                       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/thirdparty/drake/
-                      wpimath/src/test/native/cpp/drake/
-                      wpimath/src/test/native/include/drake/
 V8 export-template    wpiutil/src/main/native/include/wpi/SymbolExports.h
 GCEM                  wpimath/src/main/native/thirdparty/gcem/include/
 
@@ -1069,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
 =====================

apriltag/CMakeLists.txt

@@ -7,7 +7,7 @@ include(FetchContent)
 FetchContent_Declare(
     apriltaglib
     GIT_REPOSITORY  https://github.com/wpilibsuite/apriltag.git
-    GIT_TAG         ad31e33d20f9782b7239cb15cde57c56c91383ad
+    GIT_TAG         64be6ab26abf5e995321997fd0752c609a7e30f4
 )
 
 # Don't use apriltag's CMakeLists.txt due to conflicting naming and JNI
@@ -40,7 +40,7 @@ if (WITH_JAVA)
   set(CMAKE_JAVA_INCLUDE_PATH apriltag.jar ${EJML_JARS} ${JACKSON_JARS})
 
   file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
-  file(GLOB_RECURSE JAVA_RESOURCES src/main/native/resources/*.json)
+  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}
@@ -62,15 +62,11 @@ if (WITH_JAVA)
   target_link_libraries(apriltagjni PRIVATE apriltag_jni_headers)
   add_dependencies(apriltagjni apriltag_jar)
 
-  if (MSVC)
-    install(TARGETS apriltagjni RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
-  endif()
-
-  install(TARGETS apriltagjni EXPORT apriltagjni DESTINATION "${main_lib_dest}")
+  install(TARGETS apriltagjni EXPORT apriltagjni)
 
 endif()
 
-GENERATE_RESOURCES(src/main/native/resources/edu/wpi/first/apriltag generated/main/cpp APRILTAG frc apriltag_resources_src)
+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)
 
@@ -82,9 +78,9 @@ 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)
+  target_compile_options(apriltag PRIVATE /wd4018 /wd4005 /wd4996)
 else()
-  target_compile_options(apriltag PRIVATE -Wno-sign-compare -Wno-gnu-zero-variadic-macro-arguments)
+  target_compile_options(apriltag PRIVATE -Wno-sign-compare -Wno-gnu-zero-variadic-macro-arguments -Wno-type-limits)
 endif()
 
 target_link_libraries(apriltag wpimath)
@@ -94,13 +90,9 @@ target_include_directories(apriltag PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
   $<INSTALL_INTERFACE:${include_dest}/apriltag>)
 
-install(TARGETS apriltag EXPORT apriltag DESTINATION "${main_lib_dest}")
+install(TARGETS apriltag EXPORT apriltag)
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/apriltag")
 
-if (WITH_JAVA AND MSVC)
-    install(TARGETS apriltag RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
-endif()
-
 if (WITH_FLAT_INSTALL)
     set (apriltag_config_dir ${wpilib_dest})
 else()

apriltag/build.gradle

@@ -4,6 +4,11 @@ ext {
     nativeName = 'apriltag'
     devMain = 'edu.wpi.first.apriltag.DevMain'
     useJava = true
+    useCpp = true
+    sharedCvConfigs = [
+        apriltagDev : [],
+        apriltagTest: []]
+    staticCvConfigs = []
 
     def generateTask = createGenerateResourcesTask('main', 'APRILTAG', 'frc', project)
 
@@ -30,7 +35,6 @@ apply from: "${rootDir}/shared/opencv.gradle"
 
 dependencies {
     implementation project(':wpimath')
-    devImplementation project(':wpimath')
 }
 
 sourceSets {
@@ -41,7 +45,6 @@ sourceSets {
     }
 }
 
-
 model {
     components {}
     binaries {

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

@@ -4,14 +4,16 @@
 
 package edu.wpi.first.apriltag;
 
-import edu.wpi.first.apriltag.jni.AprilTagJNI;
-
 public final class DevMain {
   /** Main entry point. */
   public static void main(String[] args) {
     System.out.println("Hello World!");
-    var detector = AprilTagJNI.aprilTagCreate("tag16h5", 2.0, 0.0, 1, false, false);
-    AprilTagJNI.aprilTagDestroy(detector);
+    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

@@ -2,4 +2,10 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-int main() {}
+#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/AprilTagDetection.java

@@ -0,0 +1,190 @@
+// 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 new MatBuilder<>(Nat.N3(), Nat.N3()).fill(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.
+   *
+   * @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.
+   *
+   * @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.
+   *
+   * @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,281 @@
+// 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;
+
+    public Config() {}
+
+    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;
+
+    public QuadThresholdParameters() {}
+
+    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;
+    }
+  }
+
+  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.
+   *
+   * @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

@@ -37,6 +37,9 @@ import java.util.Optional;
  * 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)
@@ -114,8 +117,8 @@ public class AprilTagFieldLayout {
    * Sets the origin based on a predefined enumeration of coordinate frame origins. The origins are
    * calculated from the field dimensions.
    *
-   * <p>This transforms the Pose3ds returned by {@link #getTagPose(int)} to return the correct pose
-   * relative to a predefined coordinate frame.
+   * <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
    */
@@ -139,8 +142,8 @@ public class AprilTagFieldLayout {
   /**
    * Sets the origin for tag pose transformation.
    *
-   * <p>This transforms the Pose3ds returned by {@link #getTagPose(int)} to return the correct pose
-   * relative to the provided origin.
+   * <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
    */
@@ -186,16 +189,26 @@ public class AprilTagFieldLayout {
   }
 
   /**
-   * Deserializes a field layout from a resource within a jar file.
+   * 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 {
-    try (InputStream stream = AprilTagFieldLayout.class.getResourceAsStream(resourcePath);
-        InputStreamReader reader = new InputStreamReader(stream)) {
+    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);
     }
   }
 

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

@@ -4,17 +4,36 @@
 
 package edu.wpi.first.apriltag;
 
+import java.io.IOException;
+import java.io.UncheckedIOException;
+
 public enum AprilTagFields {
-  k2022RapidReact("2022-rapidreact.json");
+  k2022RapidReact("2022-rapidreact.json"),
+  k2023ChargedUp("2023-chargedup.json");
 
   public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/";
 
   /** Alias to the current game. */
-  public static final AprilTagFields kDefaultField = k2022RapidReact;
+  public static final AprilTagFields kDefaultField = k2023ChargedUp;
 
   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() {
+    try {
+      return AprilTagFieldLayout.loadFromResource(m_resourceFile);
+    } catch (IOException e) {
+      throw new UncheckedIOException(
+          "Could not load AprilTagFieldLayout from " + m_resourceFile, e);
+    }
+  }
 }

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,190 @@
+// 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;
+    }
+
+    public double tagSize;
+    public double fx;
+    public double fy;
+    public double cx;
+    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

@@ -4,10 +4,13 @@
 
 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.RuntimeLoader;
 import java.io.IOException;
 import java.util.concurrent.atomic.AtomicBoolean;
-import org.opencv.core.Mat;
 
 public class AprilTagJNI {
   static boolean libraryLoaded = false;
@@ -41,49 +44,47 @@ public class AprilTagJNI {
     }
   }
 
-  // Returns a pointer to a apriltag_detector_t
-  public static native long aprilTagCreate(
-      String fam, double decimate, double blur, int threads, boolean debug, boolean refine_edges);
+  public static native long createDetector();
 
-  // Destroy and free a previously created detector.
-  public static native void aprilTagDestroy(long detector);
+  public static native void destroyDetector(long det);
 
-  private static native Object[] aprilTagDetectInternal(
-      long detector,
-      long imgAddr,
-      int rows,
-      int cols,
-      boolean doPoseEstimation,
-      double tagWidth,
+  public static native void setDetectorConfig(long det, AprilTagDetector.Config config);
+
+  public static native AprilTagDetector.Config getDetectorConfig(long det);
+
+  public static native void setDetectorQTP(
+      long det, AprilTagDetector.QuadThresholdParameters params);
+
+  public static native AprilTagDetector.QuadThresholdParameters getDetectorQTP(long det);
+
+  public static native boolean addFamily(long det, String fam, int bitsCorrected);
+
+  public static native void removeFamily(long det, String fam);
+
+  public static native void clearFamilies(long det);
+
+  public static native AprilTagDetection[] detect(
+      long det, int width, int height, int stride, long bufAddr);
+
+  public static native Transform3d estimatePoseHomography(
+      double[] homography, double tagSize, double fx, double fy, double cx, double cy);
+
+  public static native AprilTagPoseEstimate estimatePoseOrthogonalIteration(
+      double[] homography,
+      double[] corners,
+      double tagSize,
       double fx,
       double fy,
       double cx,
       double cy,
       int nIters);
 
-  // Detect targets given a GRAY frame. Returns a pointer toa zarray
-  public static DetectionResult[] aprilTagDetect(
-      long detector,
-      Mat img,
-      boolean doPoseEstimation,
-      double tagWidth,
+  public static native Transform3d estimatePose(
+      double[] homography,
+      double[] corners,
+      double tagSize,
       double fx,
       double fy,
       double cx,
-      double cy,
-      int nIters) {
-    return (DetectionResult[])
-        aprilTagDetectInternal(
-            detector,
-            img.dataAddr(),
-            img.rows(),
-            img.cols(),
-            doPoseEstimation,
-            tagWidth,
-            fx,
-            fy,
-            cx,
-            cy,
-            nIters);
-  }
+      double cy);
 }

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

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

apriltag/src/main/native/cpp/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

@@ -8,22 +8,21 @@
 
 #include <units/angle.h>
 #include <units/length.h>
+#include <wpi/MemoryBuffer.h>
 #include <wpi/json.h>
-#include <wpi/raw_istream.h>
 #include <wpi/raw_ostream.h>
 
 using namespace frc;
 
 AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) {
-  std::error_code error_code;
-
-  wpi::raw_fd_istream input{path, error_code};
-  if (error_code) {
+  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;
-  input >> json;
+  wpi::json json = wpi::json::parse(fileBuffer->begin(), fileBuffer->end());
 
   for (const auto& tag : json.at("tags").get<std::vector<AprilTag>>()) {
     m_apriltags[tag.ID] = tag;

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

@@ -10,6 +10,7 @@ namespace frc {
 
 // C++ generated from resource files
 std::string_view GetResource_2022_rapidreact_json();
+std::string_view GetResource_2023_chargedup_json();
 
 AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) {
   std::string_view fieldString;
@@ -17,6 +18,9 @@ AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) {
     case AprilTagField::k2022RapidReact:
       fieldString = GetResource_2022_rapidreact_json();
       break;
+    case AprilTagField::k2023ChargedUp:
+      fieldString = GetResource_2023_chargedup_json();
+      break;
     case AprilTagField::kNumFields:
       throw std::invalid_argument("Invalid 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

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

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

@@ -5,13 +5,10 @@
 #pragma once
 
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
 
 #include "frc/geometry/Pose3d.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace frc {
 
 struct WPILIB_DLLEXPORT AprilTag {

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

@@ -0,0 +1,159 @@
+// 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.
+   *
+   * @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.
+   *
+   * @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,260 @@
+// 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
+   * @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.
+   *
+   * @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.
+   *
+   * @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

@@ -11,14 +11,11 @@
 
 #include <units/length.h>
 #include <wpi/SymbolExports.h>
+#include <wpi/json_fwd.h>
 
 #include "frc/apriltag/AprilTag.h"
 #include "frc/geometry/Pose3d.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace frc {
 /**
  * Class for representing a layout of AprilTags on a field and reading them from
@@ -34,7 +31,11 @@ namespace frc {
  * 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. */
+ * 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:
   enum class OriginPosition {
@@ -75,7 +76,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout {
   /**
    * Sets the origin for tag pose transformation.
    *
-   * This tranforms the Pose3ds returned by GetTagPose(int) to return the
+   * 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

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

@@ -14,6 +14,7 @@ namespace frc {
 
 enum class AprilTagField {
   k2022RapidReact,
+  k2023ChargedUp,
 
   // 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

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

@@ -1,415 +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
+  "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
+          }
         }
       }
     }
-  }, {
-    "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
+  ],
+  "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/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/LoadConfigTest.java

@@ -23,16 +23,13 @@ class LoadConfigTest {
   @ParameterizedTest
   @EnumSource(AprilTagFields.class)
   void testLoad(AprilTagFields field) {
-    AprilTagFieldLayout layout =
-        Assertions.assertDoesNotThrow(
-            () -> AprilTagFieldLayout.loadFromResource(field.m_resourceFile));
+    AprilTagFieldLayout layout = Assertions.assertDoesNotThrow(field::loadAprilTagLayoutField);
     assertNotNull(layout);
   }
 
   @Test
   void test2022RapidReact() throws IOException {
-    AprilTagFieldLayout layout =
-        AprilTagFieldLayout.loadFromResource(AprilTagFields.k2022RapidReact.m_resourceFile);
+    AprilTagFieldLayout layout = AprilTagFields.k2022RapidReact.loadAprilTagLayoutField();
 
     // Blue Hangar Truss - Hub
     Pose3d expectedPose =

apriltag/src/test/java/edu/wpi/first/apriltag/jni/JNITest.java

@@ -1,16 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package edu.wpi.first.apriltag.jni;
-
-import org.junit.jupiter.api.Test;
-
-public class JNITest {
-  @Test
-  void jniLinkTest() {
-    // Test to verify that the JNI test link works correctly.
-    var detector = AprilTagJNI.aprilTagCreate("tag16h5", 2.0, 0.0, 1, false, false);
-    AprilTagJNI.aprilTagDestroy(detector);
-  }
-}

apriltag/src/test/native/cpp/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

@@ -4,12 +4,12 @@
 
 #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"
-#include "gtest/gtest.h"
 
 using namespace frc;
 

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

@@ -4,12 +4,12 @@
 
 #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"
-#include "gtest/gtest.h"
 
 using namespace frc;
 

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

@@ -2,8 +2,9 @@
 // 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/AprilTagFields.h"
-#include "gtest/gtest.h"
 
 namespace frc {
 

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

@@ -2,7 +2,7 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "gtest/gtest.h"
+#include <gtest/gtest.h>
 
 int main(int argc, char** argv) {
   ::testing::InitGoogleTest(&argc, argv);

build.gradle

@@ -7,7 +7,7 @@ buildscript {
         }
     }
     dependencies {
-        classpath 'com.hubspot.jinjava:jinjava:2.6.0'
+        classpath 'com.hubspot.jinjava:jinjava:2.7.1'
     }
 }
 
@@ -20,10 +20,11 @@ plugins {
     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.4.2' apply false
-    id 'com.github.spotbugs' version '5.0.8' 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 '5.1.3' apply false
+    id 'com.google.protobuf' version '0.9.3' apply false
 }
 
 wpilibVersioning.buildServerMode = project.hasProperty('buildServer')
@@ -94,8 +95,8 @@ ext.addTaskToCopyAllOutputs = { task ->
         return
     }
     copyAllOutputs.dependsOn task
-    copyAllOutputs.inputs.file task.archivePath
-    copyAllOutputs.from task.archivePath
+    copyAllOutputs.inputs.file task.archiveFile
+    copyAllOutputs.from task.archiveFile
 }
 
 subprojects {
@@ -109,8 +110,10 @@ subprojects {
     }
 
     plugins.withType(JavaPlugin) {
-        sourceCompatibility = 11
-        targetCompatibility = 11
+        java {
+            sourceCompatibility = 11
+            targetCompatibility = 11
+        }
     }
 
     apply from: "${rootDir}/shared/java/javastyle.gradle"
@@ -126,6 +129,7 @@ subprojects {
 
     tasks.withType(JavaCompile) {
         options.compilerArgs.add '-XDstringConcat=inline'
+        options.encoding = 'UTF-8'
     }
 
     // Enables UTF-8 support in Javadoc
@@ -167,5 +171,5 @@ ext.getCurrentArch = {
 }
 
 wrapper {
-    gradleVersion = '7.5.1'
+    gradleVersion = '8.4'
 }

buildSrc/build.gradle

@@ -9,5 +9,5 @@ repositories {
     }
 }
 dependencies {
-    implementation "edu.wpi.first:native-utils:2023.9.0"
+    implementation "edu.wpi.first:native-utils:2024.3.1"
 }

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)
         }
       }

cameraserver/.styleguide

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

cameraserver/CMakeLists.txt

@@ -40,13 +40,9 @@ 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()

cameraserver/build.gradle

@@ -14,10 +14,6 @@ dependencies {
     implementation project(':wpinet')
     implementation project(':ntcore')
     implementation project(':cscore')
-    devImplementation project(':wpiutil')
-    devImplementation project(':wpinet')
-    devImplementation project(':ntcore')
-    devImplementation project(':cscore')
 }
 
 ext {

cameraserver/multiCameraServer/build.gradle

@@ -18,7 +18,9 @@ ext {
 
 apply from: "${rootDir}/shared/opencv.gradle"
 
-mainClassName = 'edu.wpi.Main'
+application {
+    mainClass = 'edu.wpi.Main'
+}
 
 apply plugin: 'com.github.johnrengelman.shadow'
 
@@ -29,7 +31,7 @@ repositories {
 }
 
 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')
@@ -59,6 +61,9 @@ model {
                 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/native/cpp/main.cpp

@@ -8,11 +8,12 @@
 #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"
@@ -63,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;
 
@@ -74,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;
   }
 
@@ -82,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;
   }
 
@@ -96,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->begin(), fileBuffer->end());
   } 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;
   }
 
@@ -122,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;
   }
 
@@ -135,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());
     }
   }
 
@@ -150,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;
   }
 

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

@@ -474,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();
@@ -483,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();
@@ -492,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();
@@ -517,6 +520,7 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::span<const std::string> hosts) {
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            std::string_view host) {
+  ::GetInstance();
   cs::AxisCamera camera{name, host};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -526,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();
@@ -535,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();
@@ -544,6 +550,7 @@ cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
 
 cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            std::span<const std::string> hosts) {
+  ::GetInstance();
   cs::AxisCamera camera{name, hosts};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -552,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;
 }
@@ -632,6 +640,7 @@ cs::CvSink CameraServer::GetVideo(std::string_view name) {
 
 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;
@@ -648,6 +657,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;
@@ -707,22 +717,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

@@ -267,17 +267,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/vision/VisionRunner.inc

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

cmake/modules/CompileWarnings.cmake

@@ -12,4 +12,17 @@ macro(wpilib_target_warnings target)
     elseif(UNIX AND APPLE)
         target_compile_options(${target} PRIVATE $<$<COMPILE_LANGUAGE:CXX>:-Wno-deprecated-anon-enum-enum-conversion>)
     endif()
+
+    # Suppress warning "enumeration types with a fixed underlying type are a
+    # Clang extension"
+    if(APPLE)
+      target_compile_options(${target} PRIVATE $<$<COMPILE_LANGUAGE:C>:-Wno-fixed-enum-extension>)
+    endif()
+
+    # Compress debug info with GCC
+    if ((${CMAKE_BUILD_TYPE} STREQUAL "Debug" OR
+         ${CMAKE_BUILD_TYPE} STREQUAL "RelWithDebInfo") AND
+        ${CMAKE_CXX_COMPILER_ID} STREQUAL "GNU")
+      target_compile_options(${target} PRIVATE -gz=zlib)
+    endif()
 endmacro()

cmake/modules/DownloadAndCheck.cmake

@@ -0,0 +1,11 @@
+macro(download_and_check source destination)
+  file(DOWNLOAD ${source} ${destination} STATUS download_status)
+  list(GET download_status 0 status_code)
+  list(GET download_status 1 status_message)
+
+  if(${status_code} EQUAL 0)
+    message(VERBOSE "Download of \"${source}\" successful.")
+  else()
+    message(FATAL_ERROR "Download of \"${source}\" failed: ${status_message}")
+  endif()
+endmacro()

cmake/modules/GenResources.cmake

@@ -1,26 +1,26 @@
-set(SCRIPTS_DIR "${CMAKE_CURRENT_LIST_DIR}/../scripts")
-MACRO(GENERATE_RESOURCES inputDir outputDir prefix namespace outputFiles)
-  FILE(GLOB inputFiles ${inputDir}/*)
-  SET(${outputFiles})
-  FOREACH(input ${inputFiles})
-    GET_FILENAME_COMPONENT(inputBase ${input} NAME)
-    IF("${inputBase}" MATCHES "^\\.")
-      CONTINUE()
-    ENDIF()
-    SET(output "${outputDir}/${inputBase}.cpp")
-    LIST(APPEND ${outputFiles} "${output}")
+set(scripts_dir "${CMAKE_CURRENT_LIST_DIR}/../scripts")
+macro(generate_resources inputDir outputDir prefix namespace outputFiles)
+  file(GLOB inputFiles ${inputDir}/*)
+  set(${outputFiles})
+  foreach(input ${inputFiles})
+    get_filename_component(inputBase ${input} NAME)
+    if("${inputBase}" MATCHES "^\\.")
+      continue()
+    endif()
+    set(output "${outputDir}/${inputBase}.cpp")
+    list(APPEND ${outputFiles} "${output}")
 
-    ADD_CUSTOM_COMMAND(
+    add_custom_command(
       OUTPUT ${output}
       COMMAND ${CMAKE_COMMAND}
         "-Dinput=${input}"
         "-Doutput=${output}"
         "-Dprefix=${prefix}"
         "-Dnamespace=${namespace}"
-        -P "${SCRIPTS_DIR}/GenResource.cmake"
+        -P "${scripts_dir}/GenResource.cmake"
       MAIN_DEPENDENCY ${input}
-      DEPENDS ${SCRIPTS_DIR}/GenResource.cmake
+      DEPENDS ${scripts_dir}/GenResource.cmake
       VERBATIM
     )
-  ENDFOREACH()
-ENDMACRO()
+  endforeach()
+endmacro()

cmake/modules/SubDirList.cmake

@@ -1,17 +1,17 @@
-MACRO(SUBDIR_LIST result curdir)
-  FILE(GLOB children RELATIVE ${curdir} ${curdir}/*)
-  SET(dirlist "")
-  FOREACH(child ${children})
-    IF(IS_DIRECTORY ${curdir}/${child})
-      LIST(APPEND dirlist ${child})
-    ENDIF()
-  ENDFOREACH()
-  SET(${result} ${dirlist})
-ENDMACRO()
+macro(subdir_list result curdir)
+  file(GLOB children RELATIVE ${curdir} ${curdir}/*)
+  set(dirlist "")
+  foreach(child ${children})
+    if(IS_DIRECTORY ${curdir}/${child})
+      list(APPEND dirlist ${child})
+    endif()
+  endforeach()
+  set(${result} ${dirlist})
+endmacro()
 
-MACRO(ADD_ALL_SUBDIRECTORIES curdir)
-  SUBDIR_LIST (_SUBPROJECTS ${curdir})
-  FOREACH (dir ${_SUBPROJECTS})
-    ADD_SUBDIRECTORY (${dir})
-  ENDFOREACH ()
-ENDMACRO()
+macro(add_all_subdirectories curdir)
+  subdir_list(_SUBPROJECTS ${curdir})
+  foreach(dir ${_SUBPROJECTS})
+    add_subdirectory(${dir})
+  endforeach()
+endmacro()

cmake/toolchains/gnu.toolchain.cmake

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.3.0)
+cmake_minimum_required(VERSION 3.11)
 
 # load settings in case of "try compile"
 set(TOOLCHAIN_CONFIG_FILE "${WPILIB_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/toolchain.config.cmake")

crossConnIntegrationTests/src/main/native/cpp/AnalogTest.cpp

@@ -2,13 +2,13 @@
 // 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 <hal/AnalogInput.h>
 #include <hal/AnalogOutput.h>
 #include <wpi/SmallVector.h>
 
 #include "CrossConnects.h"
 #include "LifetimeWrappers.h"
-#include "gtest/gtest.h"
 
 using namespace hlt;
 

crossConnIntegrationTests/src/main/native/cpp/DIOTest.cpp

@@ -2,12 +2,12 @@
 // 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 <hal/DIO.h>
 #include <wpi/SmallVector.h>
 
 #include "CrossConnects.h"
 #include "LifetimeWrappers.h"
-#include "gtest/gtest.h"
 
 using namespace hlt;
 

crossConnIntegrationTests/src/main/native/cpp/DutyCycleTest.cpp

@@ -2,11 +2,11 @@
 // 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 <hal/HAL.h>
 
 #include "CrossConnects.h"
 #include "LifetimeWrappers.h"
-#include "gtest/gtest.h"
 
 using namespace hlt;
 
@@ -21,10 +21,10 @@ TEST_P(DutyCycleTest, DutyCycle) {
   ASSERT_EQ(0, status);
 
   // Ensure our PWM is disabled, and set up properly
-  HAL_SetPWMRaw(pwmHandle, 0, &status);
+  HAL_SetPWMPulseTimeMicroseconds(pwmHandle, 0, &status);
   ASSERT_EQ(0, status);
-  HAL_SetPWMConfig(pwmHandle, 2.0, 1.0, 1.0, 0, 0, &status);
-  HAL_SetPWMConfig(pwmHandle, 5.05, 2.525, 2.525, 2.525, 0, &status);
+  HAL_SetPWMConfigMicroseconds(pwmHandle, 2000, 1000, 1000, 0, 0, &status);
+  HAL_SetPWMConfigMicroseconds(pwmHandle, 5050, 2525, 2525, 2525, 0, &status);
   ASSERT_EQ(0, status);
   HAL_SetPWMPeriodScale(pwmHandle, 0, &status);
   ASSERT_EQ(0, status);
@@ -41,8 +41,9 @@ TEST_P(DutyCycleTest, DutyCycle) {
   // Sleep enough time for the frequency to converge
   usleep(3500000);
 
-  ASSERT_NEAR(1000 / 5.05,
-              (double)HAL_GetDutyCycleFrequency(dutyCycle, &status), 1);
+  ASSERT_NEAR(
+      1000 / 5.05,
+      static_cast<double>(HAL_GetDutyCycleFrequency(dutyCycle, &status)), 1);
 
   // TODO measure output
 }

crossConnIntegrationTests/src/main/native/cpp/Main.cpp

@@ -2,7 +2,7 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-#include "gtest/gtest.h"
+#include <gtest/gtest.h>
 
 int main(int argc, char** argv) {
   testing::InitGoogleTest(&argc, argv);

crossConnIntegrationTests/src/main/native/cpp/PWMTest.cpp

@@ -5,6 +5,7 @@
 #include <atomic>
 #include <thread>
 
+#include <gtest/gtest.h>
 #include <hal/DMA.h>
 #include <hal/HAL.h>
 #include <wpi/SmallVector.h>
@@ -13,7 +14,6 @@
 
 #include "CrossConnects.h"
 #include "LifetimeWrappers.h"
-#include "gtest/gtest.h"
 
 using namespace hlt;
 
@@ -32,8 +32,8 @@ void TestTimingDMA(int squelch, std::pair<int, int> param) {
   ASSERT_EQ(0, status);
 
   // Ensure our PWM is disabled, and set up properly
-  HAL_SetPWMRaw(pwmHandle, 0, &status);
-  HAL_SetPWMConfig(pwmHandle, 2.0, 1.0, 1.0, 0, 0, &status);
+  HAL_SetPWMPulseTimeMicroseconds(pwmHandle, 0, &status);
+  HAL_SetPWMConfigMicroseconds(pwmHandle, 2000, 1000, 1000, 0, 0, &status);
   HAL_SetPWMPeriodScale(pwmHandle, squelch, &status);
 
   unsigned int checkPeriod = 0;
@@ -163,8 +163,8 @@ void TestTiming(int squelch, std::pair<int, int> param) {
   ASSERT_NE(pwmHandle, HAL_kInvalidHandle);
 
   // Ensure our PWM is disabled, and set up properly
-  HAL_SetPWMRaw(pwmHandle, 0, &status);
-  HAL_SetPWMConfig(pwmHandle, 2.0, 1.0, 1.0, 0, 0, &status);
+  HAL_SetPWMPulseTimeMicroseconds(pwmHandle, 0, &status);
+  HAL_SetPWMConfigMicroseconds(pwmHandle, 2000, 1000, 1000, 0, 0, &status);
   HAL_SetPWMPeriodScale(pwmHandle, squelch, &status);
 
   unsigned int checkPeriod = 0;
@@ -251,7 +251,7 @@ void TestTiming(int squelch, std::pair<int, int> param) {
       }
     }
 
-    HAL_SetPWMRaw(pwmHandle, 0, &status);
+    HAL_SetPWMPulseTimeMicroseconds(pwmHandle, 0, &status);
 
     // Ensure our interrupts have the proper counts
     ASSERT_EQ(interruptData.risingStamps.size(),

crossConnIntegrationTests/src/main/native/cpp/RelayAnalogTest.cpp

@@ -2,13 +2,13 @@
 // 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 <hal/AnalogInput.h>
 #include <hal/Relay.h>
 #include <wpi/SmallVector.h>
 
 #include "CrossConnects.h"
 #include "LifetimeWrappers.h"
-#include "gtest/gtest.h"
 
 using namespace hlt;
 

crossConnIntegrationTests/src/main/native/cpp/RelayDigitalTest.cpp

@@ -2,12 +2,12 @@
 // 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 <hal/Relay.h>
 #include <wpi/SmallVector.h>
 
 #include "CrossConnects.h"
 #include "LifetimeWrappers.h"
-#include "gtest/gtest.h"
 
 using namespace hlt;
 

crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp

@@ -6,9 +6,9 @@
 #include <thread>
 
 #include <fmt/core.h>
+#include <gtest/gtest.h>
 #include <hal/HAL.h>
 
-#include "gtest/gtest.h"
 #include "mockds/MockDS.h"
 
 using namespace std::chrono_literals;

cscore/.styleguide

@@ -13,6 +13,10 @@ cppSrcFileInclude {
   \.cpp$
 }
 
+modifiableFileExclude {
+  objcpp
+}
+
 licenseUpdateExclude {
   src/main/native/cpp/default_init_allocator\.h$
 }
@@ -32,6 +36,7 @@ repoRootNameOverride {
 
 includeOtherLibs {
   ^fmt/
+  ^gtest/
   ^opencv2/
   ^support/
   ^tcpsockets/

cscore/CMakeLists.txt

@@ -11,6 +11,7 @@ file(GLOB
     cscore_native_src src/main/native/cpp/*.cpp)
 file(GLOB cscore_linux_src src/main/native/linux/*.cpp)
 file(GLOB cscore_osx_src src/main/native/osx/*.cpp)
+file(GLOB cscore_osx_objc_src src/main/native/objcpp/*.mm)
 file(GLOB cscore_windows_src src/main/native/windows/*.cpp)
 
 add_library(cscore ${cscore_native_src})
@@ -18,7 +19,9 @@ set_target_properties(cscore PROPERTIES DEBUG_POSTFIX "d")
 
 if(NOT MSVC)
     if (APPLE)
-        target_sources(cscore PRIVATE ${cscore_osx_src})
+        target_sources(cscore PRIVATE ${cscore_osx_src} ${cscore_osx_objc_src})
+        target_compile_options(cscore PRIVATE "-fobjc-arc")
+        set_target_properties(cscore PROPERTIES LINK_FLAGS "-framework CoreFoundation -framework AVFoundation -framework Foundation -framework CoreMedia -framework CoreVideo")
     else()
         target_sources(cscore PRIVATE ${cscore_linux_src})
     endif()
@@ -37,7 +40,7 @@ target_link_libraries(cscore PUBLIC wpinet wpiutil ${OpenCV_LIBS})
 
 set_property(TARGET cscore PROPERTY FOLDER "libraries")
 
-install(TARGETS cscore EXPORT cscore DESTINATION "${main_lib_dest}")
+install(TARGETS cscore EXPORT cscore)
 install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/cscore")
 
 if (WITH_FLAT_INSTALL)
@@ -50,7 +53,7 @@ configure_file(cscore-config.cmake.in ${WPILIB_BINARY_DIR}/cscore-config.cmake )
 install(FILES ${WPILIB_BINARY_DIR}/cscore-config.cmake DESTINATION ${cscore_config_dir})
 install(EXPORT cscore DESTINATION ${cscore_config_dir})
 
-SUBDIR_LIST(cscore_examples "${CMAKE_CURRENT_SOURCE_DIR}/examples")
+subdir_list(cscore_examples "${CMAKE_CURRENT_SOURCE_DIR}/examples")
 foreach(example ${cscore_examples})
     file(GLOB cscore_example_src examples/${example}/*.cpp)
     unset(add_libs)
@@ -99,12 +102,7 @@ if (WITH_JAVA)
     file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
     set(CMAKE_JNI_TARGET true)
 
-    if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
-        set(CMAKE_JAVA_COMPILE_FLAGS "-h" "${CMAKE_CURRENT_BINARY_DIR}/jniheaders")
-        add_jar(cscore_jar ${JAVA_SOURCES} INCLUDE_JARS wpiutil_jar ${OPENCV_JAR_FILE} OUTPUT_NAME cscore)
-    else()
-        add_jar(cscore_jar ${JAVA_SOURCES} INCLUDE_JARS wpiutil_jar ${OPENCV_JAR_FILE} OUTPUT_NAME cscore GENERATE_NATIVE_HEADERS cscore_jni_headers)
-    endif()
+    add_jar(cscore_jar ${JAVA_SOURCES} INCLUDE_JARS wpiutil_jar ${OPENCV_JAR_FILE} OUTPUT_NAME cscore GENERATE_NATIVE_HEADERS cscore_jni_headers)
 
     get_property(CSCORE_JAR_FILE TARGET cscore_jar PROPERTY JAR_FILE)
     install(FILES ${CSCORE_JAR_FILE} DESTINATION "${java_lib_dest}")
@@ -128,19 +126,10 @@ if (WITH_JAVA)
 
     set_property(TARGET cscorejni PROPERTY FOLDER "libraries")
 
-    if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
-        target_include_directories(cscorejni PRIVATE ${JNI_INCLUDE_DIRS})
-        target_include_directories(cscorejni PRIVATE "${CMAKE_CURRENT_BINARY_DIR}/jniheaders")
-    else()
-        target_link_libraries(cscorejni PRIVATE cscore_jni_headers)
-    endif()
+    target_link_libraries(cscorejni PRIVATE cscore_jni_headers)
     add_dependencies(cscorejni cscore_jar)
 
-    if (MSVC)
-        install(TARGETS cscorejni RUNTIME DESTINATION "${jni_lib_dest}" COMPONENT Runtime)
-    endif()
-
-    install(TARGETS cscorejni EXPORT cscorejni DESTINATION "${main_lib_dest}")
+    install(TARGETS cscorejni EXPORT cscorejni)
 
 endif()
 

cscore/build.gradle

@@ -2,16 +2,16 @@ import org.gradle.internal.os.OperatingSystem
 
 ext {
     nativeName = 'cscore'
-    devMain = 'edu.wpi.cscore.DevMain'
+    devMain = 'edu.wpi.first.cscore.DevMain'
 }
 
 // Removed because having the objective-cpp plugin added breaks
 // embedded tools and its toolchain check. It causes an obj-cpp
 // source set to be added to all binaries, even cross binaries
 // with no support.
-// if (OperatingSystem.current().isMacOsX()) {
-//     apply plugin: 'objective-cpp'
-// }
+if (OperatingSystem.current().isMacOsX()) {
+    apply plugin: 'objective-cpp'
+}
 
 apply from: "${rootDir}/shared/jni/setupBuild.gradle"
 
@@ -36,7 +36,6 @@ model {
                         srcDir 'src/main/native/include'
                         include '**/*.h'
                     }
-
                 }
             }
             binaries.all {
@@ -87,16 +86,16 @@ ext {
     splitSetup = {
         if (it.targetPlatform.operatingSystem.isMacOsX()) {
             it.sources {
-                // macObjCpp(ObjectiveCppSourceSet) {
-                //     source {
-                //         srcDirs = ['src/main/native/objcpp']
-                //         include '**/*.mm'
-                //     }
-                //     exportedHeaders {
-                //         srcDirs 'src/main/native/include'
-                //         include '**/*.h'
-                //     }
-                // }
+                macObjCpp(ObjectiveCppSourceSet) {
+                    source {
+                        srcDirs = ['src/main/native/objcpp']
+                        include '**/*.mm'
+                    }
+                    exportedHeaders {
+                        srcDirs 'src/main/native/include', 'src/main/native/cpp'
+                        include '**/*.h'
+                    }
+                }
                 cscoreMacCpp(CppSourceSet) {
                     source {
                         srcDirs 'src/main/native/osx'
@@ -157,6 +156,12 @@ Action<List<String>> symbolFilter = { symbols ->
     symbols.removeIf({ !it.startsWith('CS_') })
 } as Action<List<String>>;
 
+run {
+    if (OperatingSystem.current().isMacOsX()) {
+        jvmArgs("-XstartOnFirstThread");
+    }
+}
+
 nativeUtils.exportsConfigs {
     cscore {
         x64ExcludeSymbols = [
@@ -210,15 +215,14 @@ model {
                                     it.linker.args << '-lGL'
                                 }
                             }
-                        }
-                        sources {
-                            cpp {
-                                source {
-                                    srcDirs 'examples/' + "${key}"
-                                    include '**/*.cpp'
-                                }
+                            if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                                nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
                             }
                         }
+                        sources.cpp.source {
+                            srcDirs 'examples/' + "${key}"
+                            include '**/*.cpp'
+                        }
                     }
                 }
             } else {
@@ -228,15 +232,14 @@ model {
                         lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                         lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
                         lib library: 'cscore', linkage: 'shared'
-                    }
-                    sources {
-                        cpp {
-                            source {
-                                srcDirs 'examples/' + "${key}"
-                                include '**/*.cpp'
-                            }
+                        if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                            nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
                         }
                     }
+                    sources.cpp.source {
+                        srcDirs 'examples/' + "${key}"
+                        include '**/*.cpp'
+                    }
                 }
             }
         }

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

@@ -65,6 +65,7 @@ public class CameraServerCvJNI {
   public static native void putSourceFrame(int source, long imageNativeObj);
 
   public static native int createCvSink(String name);
+
   // public static native int createCvSinkCallback(String name,
   //                            void (*processFrame)(long time));
 

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

@@ -143,7 +143,7 @@ public class CameraServerJNI {
   public static native void releaseSource(int source);
 
   //
-  // Camera Source Common Property Fuctions
+  // Camera Source Common Property Functions
   //
   public static native void setCameraBrightness(int source, int brightness);
 
@@ -390,4 +390,10 @@ public class CameraServerJNI {
   public static native long allocateRawFrame();
 
   public static native void freeRawFrame(long frame);
+
+  public static native void runMainRunLoop();
+
+  public static native int runMainRunLoopTimeout(double timeoutSeconds);
+
+  public static native void stopMainRunLoop();
 }

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

@@ -4,6 +4,8 @@
 
 package edu.wpi.first.cscore;
 
+import java.util.Objects;
+
 /** Video mode. */
 @SuppressWarnings("MemberName")
 public class VideoMode {
@@ -75,4 +77,28 @@ public class VideoMode {
 
   /** Frames per second. */
   public int fps;
+
+  @Override
+  public boolean equals(Object other) {
+    if (this == other) {
+      return true;
+    }
+    if (other == null) {
+      return false;
+    }
+    if (getClass() != other.getClass()) {
+      return false;
+    }
+    VideoMode mode = (VideoMode) other;
+
+    return pixelFormat == mode.pixelFormat
+        && width == mode.width
+        && height == mode.height
+        && fps == mode.fps;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(pixelFormat, width, height, fps);
+  }
 }

cscore/src/main/native/cpp/HttpCameraImpl.cpp

@@ -193,7 +193,8 @@ wpi::HttpConnection* HttpCameraImpl::DeviceStreamConnect(
   auto [mediaType, contentType] = wpi::split(conn->contentType.str(), ';');
   mediaType = wpi::trim(mediaType);
   if (mediaType != "multipart/x-mixed-replace") {
-    SWARNING("\"{}\": unrecognized Content-Type \"{}\"", req.host, mediaType);
+    SWARNING("\"{}\": unrecognized Content-Type \"{}\"", req.host.str(),
+             mediaType);
     std::scoped_lock lock(m_mutex);
     m_streamConn = nullptr;
     return nullptr;
@@ -216,7 +217,8 @@ wpi::HttpConnection* HttpCameraImpl::DeviceStreamConnect(
   }
 
   if (boundary.empty()) {
-    SWARNING("\"{}\": empty multi-part boundary or no Content-Type", req.host);
+    SWARNING("\"{}\": empty multi-part boundary or no Content-Type",
+             req.host.str());
     std::scoped_lock lock(m_mutex);
     m_streamConn = nullptr;
     return nullptr;
@@ -281,8 +283,8 @@ bool HttpCameraImpl::DeviceStreamFrame(wpi::raw_istream& is,
   // Check the content type (if present)
   if (!contentTypeBuf.str().empty() &&
       !wpi::starts_with(contentTypeBuf, "image/jpeg")) {
-    auto errMsg =
-        fmt::format("received unknown Content-Type \"{}\"", contentTypeBuf);
+    auto errMsg = fmt::format("received unknown Content-Type \"{}\"",
+                              contentTypeBuf.str());
     SWARNING("{}", errMsg);
     PutError(errMsg, wpi::Now());
     return false;

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

@@ -22,9 +22,7 @@ class Image {
 
  public:
 #ifndef __linux__
-  explicit Image(size_t capacity) {
-    m_data.reserve(capacity);
-  }
+  explicit Image(size_t capacity) { m_data.reserve(capacity); }
 #else
   explicit Image(size_t capacity)
       : m_data{capacity, default_init_allocator<uchar>{}} {
@@ -39,35 +37,19 @@ class Image {
   operator std::string_view() const {  // NOLINT
     return str();
   }
-  std::string_view str() const {
-    return {data(), size()};
-  }
-  size_t capacity() const {
-    return m_data.capacity();
-  }
+  std::string_view str() const { return {data(), size()}; }
+  size_t capacity() const { return m_data.capacity(); }
   const char* data() const {
     return reinterpret_cast<const char*>(m_data.data());
   }
-  char* data() {
-    return reinterpret_cast<char*>(m_data.data());
-  }
-  size_t size() const {
-    return m_data.size();
-  }
+  char* data() { return reinterpret_cast<char*>(m_data.data()); }
+  size_t size() const { return m_data.size(); }
 
-  const std::vector<uchar>& vec() const {
-    return m_data;
-  }
-  std::vector<uchar>& vec() {
-    return m_data;
-  }
+  const std::vector<uchar>& vec() const { return m_data; }
+  std::vector<uchar>& vec() { return m_data; }
 
-  void resize(size_t size) {
-    m_data.resize(size);
-  }
-  void SetSize(size_t size) {
-    m_data.resize(size);
-  }
+  void resize(size_t size) { m_data.resize(size); }
+  void SetSize(size_t size) { m_data.resize(size); }
 
   cv::Mat AsMat() {
     int type;
@@ -90,9 +72,7 @@ class Image {
     return cv::Mat{height, width, type, m_data.data()};
   }
 
-  cv::_InputArray AsInputArray() {
-    return cv::_InputArray{m_data};
-  }
+  cv::_InputArray AsInputArray() { return cv::_InputArray{m_data}; }
 
   bool Is(int width_, int height_) {
     return width == width_ && height == height_;
@@ -114,12 +94,8 @@ class Image {
   bool IsLarger(const Image& oth) {
     return width >= oth.width && height >= oth.height;
   }
-  bool IsSmaller(int width_, int height_) {
-    return !IsLarger(width_, height_);
-  }
-  bool IsSmaller(const Image& oth) {
-    return !IsLarger(oth);
-  }
+  bool IsSmaller(int width_, int height_) { return !IsLarger(width_, height_); }
+  bool IsSmaller(const Image& oth) { return !IsLarger(oth); }
 
  private:
   std::vector<uchar> m_data;

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

@@ -400,12 +400,12 @@ void MjpegServerImpl::ConnThread::SendHTML(wpi::raw_ostream& os,
           fmt::print(os,
                      "<input id=\"{0}{1}\" type=\"radio\" name=\"{0}\" "
                      "value=\"{2}\" onclick=\"update('{0}', {1})\"",
-                     name, j, ch_name);
+                     name, j, ch_name.str());
           if (j == valE) {
             os << " checked";
           }
           fmt::print(os, " /><label for=\"{}{}\">{}</label>\n", name, j,
-                     ch_name);
+                     ch_name.str());
         }
         break;
       }
@@ -543,7 +543,7 @@ void MjpegServerImpl::ConnThread::SendJSON(wpi::raw_ostream& os,
         for (char ch : *choice) {
           ch_name.push_back(std::isprint(ch) ? ch : ' ');
         }
-        fmt::print(os, "\"{}\": \"{}\"", j, ch_name);
+        fmt::print(os, "\"{}\": \"{}\"", j, ch_name.str());
       }
       os << "}\n";
     }

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

@@ -14,6 +14,7 @@
 #include <vector>
 
 #include <wpi/StringMap.h>
+#include <wpi/json_fwd.h>
 #include <wpi/mutex.h>
 
 #include "PropertyImpl.h"
@@ -23,7 +24,6 @@ namespace wpi {
 class Logger;
 template <typename T>
 class SmallVectorImpl;
-class json;
 }  // namespace wpi
 
 namespace cs {

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

@@ -10,14 +10,11 @@
 #include <string_view>
 
 #include <wpi/Logger.h>
+#include <wpi/json_fwd.h>
 #include <wpi/mutex.h>
 
 #include "SourceImpl.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace cs {
 
 class Frame;

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

@@ -14,6 +14,7 @@
 
 #include <wpi/Logger.h>
 #include <wpi/condition_variable.h>
+#include <wpi/json_fwd.h>
 #include <wpi/mutex.h>
 
 #include "Frame.h"
@@ -22,10 +23,6 @@
 #include "PropertyContainer.h"
 #include "cscore_cpp.h"
 
-namespace wpi {
-class json;
-}  // namespace wpi
-
 namespace cs {
 
 class Notifier;
@@ -50,6 +47,7 @@ class SourceImpl : public PropertyContainer {
 
   void SetConnectionStrategy(CS_ConnectionStrategy strategy) {
     m_strategy = static_cast<int>(strategy);
+    NumSinksChanged();
   }
   bool IsEnabled() const {
     return m_strategy == CS_CONNECTION_KEEP_OPEN ||

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

@@ -16,7 +16,7 @@
 namespace cs {
 
 // The UnlimitedHandleResource class is a way to track handles. This version
-// allows an unlimted number of handles that are allocated sequentially. When
+// allows an unlimited number of handles that are allocated sequentially. When
 // possible, indices are reused to save memory usage and keep the array length
 // down.
 // However, automatic array management has not been implemented, but might be in
@@ -154,7 +154,7 @@ template <typename T>
 inline std::span<T>
 UnlimitedHandleResource<THandle, TStruct, typeValue, TMutex>::GetAll(
     wpi::SmallVectorImpl<T>& vec) {
-  ForEach([&](THandle handle, const TStruct& data) { vec.push_back(handle); });
+  ForEach([&](THandle handle, const TStruct&) { vec.push_back(handle); });
   return vec;
 }