diff --git a/.bazelignore b/.bazelignore index 792570baee3..02c49272f04 100644 --- a/.bazelignore +++ b/.bazelignore @@ -6,12 +6,12 @@ apriltag/bin cameraserver/bin cameraserver/multiCameraServer/bin cscore/bin -fieldImages/bin +fields/bin hal/bin developerRobot/bin ntcore/bin romiVendordep/bin -wpilibNewCommands/bin +commandsv2/bin wpilibj/bin wpimath/bin wpinet/bin diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 3413dae59f4..54a219d5e1e 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -27,7 +27,7 @@ /simulation/ @wpilibsuite/simulation -/wpilibNewCommands/ @wpilibsuite/commandbased +/commandsv2/ @wpilibsuite/commandbased /wpilibcExamples/ @wpilibsuite/wpilib @wpilibsuite/documentation diff --git a/.github/actions/pregen/action.yml b/.github/actions/pregen/action.yml index e05e19e1ba6..a67e2a380f0 100644 --- a/.github/actions/pregen/action.yml +++ b/.github/actions/pregen/action.yml @@ -33,7 +33,7 @@ runs: run: | ./wpilibc/generate_hids.py ./wpilibj/generate_hids.py - ./wpilibNewCommands/generate_hids.py + ./commandsv2/generate_hids.py shell: bash - name: Regenerate PWM motor controllers @@ -43,7 +43,7 @@ runs: shell: bash - name: Regenerate mrcal minimath - run: ./wpical/generate_mrcal.py + run: ./tools/wpical/generate_mrcal.py shell: bash - name: Regenerate wpimath diff --git a/.github/labeler.yml b/.github/labeler.yml index 9a1881e4d32..e96a2660edb 100644 --- a/.github/labeler.yml +++ b/.github/labeler.yml @@ -5,7 +5,7 @@ - any-glob-to-any-file: apriltag/** 'component: command-based': - changed-files: - - any-glob-to-any-file: wpilibNewCommands/** + - any-glob-to-any-file: commandsv2/** 'component: cscore': - changed-files: - any-glob-to-any-file: cscore/** diff --git a/.github/workflows/command-robotpy-pr.yml b/.github/workflows/command-robotpy-pr.yml index 06864f5cbf1..08e97d0055c 100644 --- a/.github/workflows/command-robotpy-pr.yml +++ b/.github/workflows/command-robotpy-pr.yml @@ -5,7 +5,7 @@ on: types: - opened paths: - - 'wpilibNewCommands/src/**/*.java' + - 'commandsv2/src/**/*.java' jobs: comment: diff --git a/.github/workflows/sanitizers.yml b/.github/workflows/sanitizers.yml index f4e5f00b104..d9b726a6dac 100644 --- a/.github/workflows/sanitizers.yml +++ b/.github/workflows/sanitizers.yml @@ -23,7 +23,7 @@ jobs: - name: tsan cmake-flags: "-DCMAKE_BUILD_TYPE=Tsan" ctest-env: "TSAN_OPTIONS=second_deadlock_stack=1" - ctest-flags: "-E 'cscore|cameraserver|wpilibc|wpilibNewCommands'" + ctest-flags: "-E 'cscore|cameraserver|wpilibc|commandsv2'" - name: ubsan cmake-flags: "-DCMAKE_BUILD_TYPE=Ubsan" ctest-env: "" diff --git a/.styleguide b/.styleguide index 5349f31ae78..71becc3ccf5 100644 --- a/.styleguide +++ b/.styleguide @@ -20,7 +20,7 @@ modifiableFileExclude { generatedFileExclude { FRCNetComm\.java$ simulation/gz_msgs/src/include/simulation/gz_msgs/msgs\.h$ - fieldImages/src/main/native/resources/ + fields/src/main/native/resources/ apriltag/src/test/resources/ wpilibc/src/generated/ @@ -43,20 +43,13 @@ includeOtherLibs { ^fmt/ ^glass/ ^gtest/ - ^hal/ ^imgui ^implot ^mockdata/ - ^networktables/ - ^ntcore ^opencv2/ ^support/ ^units/ ^unsupported/ ^upb/ ^vision/ - ^wpi/ - ^wpigui - ^wpimath/ - ^wpinet/ } diff --git a/BUILD.bazel b/BUILD.bazel index 157474099b5..11b2d3942a3 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -50,7 +50,7 @@ write_source_files( "//wpilibcExamples:write_example_project_list", "//wpilibj:write_wpilibj", "//wpilibjExamples:write_example_project_list", - "//wpilibNewCommands:write_wpilib_new_commands", + "//commandsv2:write_wpilib_new_commands", "//commandsv3:write_commandsv3", "//wpimath:write_wpimath", "//wpiunits:write_wpiunits", @@ -70,12 +70,12 @@ publish_all( "//cscore:cscore-java_publish.publish", "//datalog:datalog-cpp_publish.publish", "//datalog:datalog-java_publish.publish", - "//datalogtool:datalogtool_publish.publish", + "//tools/datalogtool:datalogtool_publish.publish", "//docs:wpilibj_publish.publish", "//epilogue-processor:processor-java_publish.publish", "//epilogue-runtime:epilogue-java_publish.publish", - "//fieldImages:fieldImages-cpp_publish.publish", - "//fieldImages:fieldImages-java_publish.publish", + "//fields:fields-cpp_publish.publish", + "//fields:fields-java_publish.publish", "//glass:glass-cpp_publish.publish", "//glass:glassapp_publish.publish", "//glass:glassnt-cpp_publish.publish", @@ -84,8 +84,8 @@ publish_all( "//ntcore:ntcore-cpp_publish.publish", "//ntcore:ntcore-java_publish.publish", "//ntcoreffi:ntcoreffi-cpp_publish.publish", - "//outlineviewer:outlineviewer_publish.publish", - "//processstarter:processstarter_publish.publish", + "//tools/outlineviewer:outlineviewer_publish.publish", + "//tools/processstarter:processstarter_publish.publish", "//romiVendordep:romiVendordep-cpp_publish.publish", "//romiVendordep:romiVendordep-java_publish.publish", "//simulation/halsim_ds_socket:halsim_ds_socket-cpp_publish.publish", @@ -94,13 +94,13 @@ publish_all( "//simulation/halsim_ws_core:halsim_ws_core-cpp_publish.publish", "//simulation/halsim_ws_server:halsim_ws_server-cpp_publish.publish", "//simulation/halsim_xrp:halsim_xrp-cpp_publish.publish", - "//sysid:sysid_publish.publish", + "//tools/sysid:sysid_publish.publish", "//thirdparty/googletest:googletest-cpp_publish.publish", "//thirdparty/imgui_suite:imguiSuite-cpp_publish.publish", - "//wpical:wpical_publish.publish", + "//tools/wpical:wpical_publish.publish", "//wpigui:wpigui-cpp_publish.publish", - "//wpilibNewCommands:wpilibNewCommands-cpp_publish.publish", - "//wpilibNewCommands:wpilibNewCommands-java_publish.publish", + "//commandsv2:commandsv2-cpp_publish.publish", + "//commandsv2:commandsv2-java_publish.publish", "//commandsv3:commandsv3-java_publish.publish", "//wpilibc:wpilibc-cpp_publish.publish", "//wpilibcExamples:commands_publish.publish", diff --git a/CMakeLists.txt b/CMakeLists.txt index 6d6d4c80c8a..e9118a6fe13 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -317,17 +317,17 @@ if(WITH_WPIUNITS AND NOT WITH_WPIMATH) endif() if(WITH_GUI) - add_subdirectory(fieldImages) + add_subdirectory(fields) add_subdirectory(thirdparty/imgui_suite) add_subdirectory(wpigui) add_subdirectory(glass) - add_subdirectory(outlineviewer) - add_subdirectory(sysid) + add_subdirectory(tools/outlineviewer) + add_subdirectory(tools/sysid) if(WITH_WPICAL) - add_subdirectory(wpical) + add_subdirectory(tools/wpical) endif() if(LIBSSH_FOUND) - add_subdirectory(datalogtool) + add_subdirectory(tools/datalogtool) endif() endif() @@ -349,12 +349,12 @@ if(WITH_WPILIB) set(COMMANDSV3_DEP_REPLACE "find_dependency(commandsv3)") set(WPILIBC_DEP_REPLACE "find_dependency(wpilibc)") set(WPILIBJ_DEP_REPLACE "find_dependency(wpilibj)") - set(WPILIBNEWCOMMANDS_DEP_REPLACE "find_dependency(wpilibNewCommands)") + set(COMMAND_DEP_REPLACE "find_dependency(commandsv2)") add_subdirectory(apriltag) add_subdirectory(wpilibj) add_subdirectory(wpilibc) add_subdirectory(commandsv3) # must be after wpilibj - add_subdirectory(wpilibNewCommands) + add_subdirectory(commandsv2) add_subdirectory(romiVendordep) add_subdirectory(xrpVendordep) if(WITH_EXAMPLES) diff --git a/GeneratedFiles.md b/GeneratedFiles.md index 3bf6838080f..4a2b78861d3 100644 --- a/GeneratedFiles.md +++ b/GeneratedFiles.md @@ -6,9 +6,9 @@ The Python script used to generate a subproject's files will always be located i The templates will be located under `subproject/src/generate/main`, and generated files will be located under `subproject/src/generated/main`. -If the generated file is for C++, the hierarchy should be symmetrical, so if a generated header is located under `subproject/src/generated/main/native/include/frc/header.h`, the template to generate it should be located under `subproject/src/generate/main/native/include/frc/template.h.jinja`. You should pretend like `subproject/src/generate/main` is just like `subproject/src/main`, in that the file hierarchy must make sense if the files weren't generated, e.g, headers that would go in `subproject/src/main/native/include/blah` should be in `subproject/src/generated/main/native/include/blah`. +If the generated file is for C++, the hierarchy should be symmetrical, so if a generated header is located under `subproject/src/generated/main/native/include/wpi/header.h`, the template to generate it should be located under `subproject/src/generate/main/native/include/wpi/template.h.jinja`. You should pretend like `subproject/src/generate/main` is just like `subproject/src/main`, in that the file hierarchy must make sense if the files weren't generated, e.g, headers that would go in `subproject/src/main/native/include/blah` should be in `subproject/src/generated/main/native/include/blah`. -If the generated file is for Java, templates should be located under `subproject/src/generate/main/java`, and the hierarchy for output files should reflect the declared package of the output Java files. For example, a Jinja template at `subproject/src/main/java/template.java.jinja` with the package `edu.wpi.first.wpilibj` would be used to generate Java files located at `subproject/src/generated/main/java/edu/wpi/first/wpilibj` +If the generated file is for Java, templates should be located under `subproject/src/generate/main/java`, and the hierarchy for output files should reflect the declared package of the output Java files. For example, a Jinja template at `subproject/src/main/java/template.java.jinja` with the package `org.wpilib.wpilibj` would be used to generate Java files located at `subproject/src/generated/main/java/org/wpilib/wpilibj` The JSON files live under `subproject/src/generate` since they apply to both languages. One unique case is JSON files that are used by multiple subprojects, currently only JSON files shared by wpilibc and wpilibj. In that specific case, the JSON files will always be located in wpilibj since Java is the most used language. diff --git a/MavenArtifacts.md b/MavenArtifacts.md index aaceb9a0e85..ae955e1b008 100644 --- a/MavenArtifacts.md +++ b/MavenArtifacts.md @@ -108,7 +108,7 @@ All artifacts are based at `edu.wpi.first.artifactname` in the repository. * wpinet * wpiutil -* wpilibNewCommands +* commandsv2 * wpilibc * hal * cameraserver diff --git a/README-CMake.md b/README-CMake.md index a91885f19ac..b986f699c46 100644 --- a/README-CMake.md +++ b/README-CMake.md @@ -7,14 +7,14 @@ WPILib is normally built with Gradle, however for some systems, such as Linux ba * cameraserver * commandsv3 * cscore -* fieldImages +* fields * hal (simulation HAL only) * ntcore * romiVendordep * simulation extensions * wpigui * wpilib (wpilibc, wpilibj, and myRobot) -* wpilibNewCommands +* commandsv2 * wpimath * wpinet * wpiunits @@ -162,7 +162,7 @@ file(GLOB_RECURSE JAVA_SOURCES *.java) # If you want Gradle compatibility or you are using one of the templates/examples, comment out the above line and uncomment this line instead: # file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) add_jar(robot ${JAVA_SOURCES} - INCLUDE_JARS apriltag_jar cscore_jar hal_jar ntcore_jar wpilibNewCommands_jar wpimath_jar wpinet_jar wpiutil_jar wpiunits_jar wpilibj_jar ${opencvJar}) + INCLUDE_JARS apriltag_jar cscore_jar hal_jar ntcore_jar commandsv2_jar wpimath_jar wpinet_jar wpiutil_jar wpiunits_jar wpilibj_jar ${opencvJar}) export_jars(TARGETS robot FILE robot.jar) ``` This includes all the built JARs except for the vendordeps. If you are not using a JAR/library, you may remove it. @@ -176,7 +176,7 @@ After that, run `cmake --build .` to create your JAR file. To execute the JAR fi ## Using vendordeps -Vendordeps are not included as part of the `wpilib` CMake package. However, if you want to use a vendordep, you need to use `find_package(VENDORDEP)`, where `VENDORDEP` is the name of the vendordep (case-sensitive), like `xrpVendordep` or `romiVendordep`. Note that wpilibNewCommands, while a vendordep in normal robot projects, is not built as a vendordep in CMake, and is instead included as part of the `wpilib` CMake package. After you used `find_package`, you can reference the vendordep library like normal, either by using `target_link_libraries` for C++ or `add_jar` for Java. +Vendordeps are not included as part of the `wpilib` CMake package. However, if you want to use a vendordep, you need to use `find_package(VENDORDEP)`, where `VENDORDEP` is the name of the vendordep (case-sensitive), like `xrpVendordep` or `romiVendordep`. Note that commandsv2, while a vendordep in normal robot projects, is not built as a vendordep in CMake, and is instead included as part of the `wpilib` CMake package. After you used `find_package`, you can reference the vendordep library like normal, either by using `target_link_libraries` for C++ or `add_jar` for Java. ## Troubleshooting Below are some common issues that are run into when building. diff --git a/README.md b/README.md index 58ffb7fb6d2..be812467fe9 100644 --- a/README.md +++ b/README.md @@ -105,7 +105,7 @@ If opening from a fresh clone, generated java dependencies will not exist. Most - `cscore` - `hal` - `ntcore` -- `wpilibNewCommands` +- `commandsv2` - `wpimath` - `wpinet` - `wpiunits` diff --git a/apriltag/BUILD.bazel b/apriltag/BUILD.bazel index c8bc17c6157..01f771c83fa 100644 --- a/apriltag/BUILD.bazel +++ b/apriltag/BUILD.bazel @@ -55,7 +55,7 @@ cc_library( generate_resources( name = "generate-resources", - namespace = "frc", + namespace = "wpi::apriltag", prefix = "APRILTAG", resource_files = glob(["src/main/native/resources/**"]), ) @@ -133,7 +133,7 @@ wpilib_jni_java_library( srcs = glob(["src/main/java/**/*.java"]), extra_source_pkgs = ["resources"], maven_artifact_name = "apriltag-java", - maven_group_id = "edu.wpi.first.apriltag", + maven_group_id = "org.wpilib.apriltag", native_libs = [":apriltagjni"], resource_strip_prefix = "apriltag/src/main/native/resources", resources = glob(["src/main/native/resources/**"]), @@ -191,8 +191,8 @@ cc_binary( java_binary( name = "DevMain-Java", - srcs = ["src/dev/java/edu/wpi/first/apriltag/DevMain.java"], - main_class = "edu.wpi.first.apriltag.DevMain", + srcs = ["src/dev/java/org/wpilib/vision/apriltag/DevMain.java"], + main_class = "org.wpilib.vision.apriltag.DevMain", deps = [ ":apriltag-java", ], @@ -210,5 +210,5 @@ py_binary( package_minimal_jni_project( name = "apriltag", maven_artifact_name = "apriltag-cpp", - maven_group_id = "edu.wpi.first.apriltag", + maven_group_id = "org.wpilib.apriltag", ) diff --git a/apriltag/CMakeLists.txt b/apriltag/CMakeLists.txt index f2ea5dff008..223d055bb1f 100644 --- a/apriltag/CMakeLists.txt +++ b/apriltag/CMakeLists.txt @@ -62,7 +62,7 @@ if(WITH_JAVA) apriltag_jar SOURCES ${JAVA_SOURCES} RESOURCES - NAMESPACE "edu/wpi/first/apriltag" ${JAVA_RESOURCES} + NAMESPACE "org/wpilib/vision/apriltag" ${JAVA_RESOURCES} INCLUDE_JARS wpimath_jar wpiunits_jar @@ -107,10 +107,10 @@ if(WITH_JAVA_SOURCE) endif() generate_resources( - src/main/native/resources/edu/wpi/first/apriltag + src/main/native/resources/org/wpilib/vision/apriltag generated/main/cpp APRILTAG - frc + wpi::apriltag apriltag_resources_src ) diff --git a/apriltag/build.gradle b/apriltag/build.gradle index 311779f8423..909bf1df74e 100644 --- a/apriltag/build.gradle +++ b/apriltag/build.gradle @@ -4,7 +4,7 @@ apply plugin: 'c' ext { nativeName = 'apriltag' - devMain = 'edu.wpi.first.apriltag.DevMain' + devMain = 'org.wpilib.vision.apriltag.DevMain' useJava = true useCpp = true sharedCvConfigs = [ @@ -12,7 +12,7 @@ ext { apriltagTest: []] staticCvConfigs = [] - def generateTask = createGenerateResourcesTask('main', 'APRILTAG', 'frc', project) + def generateTask = createGenerateResourcesTask('main', 'APRILTAG', 'wpi::apriltag', project) tasks.withType(CppCompile) { dependsOn generateTask diff --git a/apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.java b/apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.java deleted file mode 100644 index 1047088044e..00000000000 --- a/apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.java +++ /dev/null @@ -1,20 +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; - -public final class DevMain { - /** Main entry point. */ - public static void main(String[] args) { - System.out.println("Hello World!"); - AprilTagDetector detector = new AprilTagDetector(); - detector.addFamily("tag16h5"); - AprilTagDetector.Config config = new AprilTagDetector.Config(); - config.refineEdges = false; - detector.setConfig(config); - detector.close(); - } - - private DevMain() {} -} diff --git a/apriltag/src/dev/java/org/wpilib/vision/apriltag/DevMain.java b/apriltag/src/dev/java/org/wpilib/vision/apriltag/DevMain.java new file mode 100644 index 00000000000..841cecd653c --- /dev/null +++ b/apriltag/src/dev/java/org/wpilib/vision/apriltag/DevMain.java @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.vision.apriltag; + +public final class DevMain { + /** Main entry point. */ + public static void main(String[] args) { + System.out.println("Hello World!"); + AprilTagDetector detector = new AprilTagDetector(); + detector.addFamily("tag16h5"); + AprilTagDetector.Config config = new AprilTagDetector.Config(); + config.refineEdges = false; + detector.setConfig(config); + detector.close(); + } + + private DevMain() {} +} diff --git a/apriltag/src/dev/native/cpp/main.cpp b/apriltag/src/dev/native/cpp/main.cpp index 515202edf01..10aad9b24a8 100644 --- a/apriltag/src/dev/native/cpp/main.cpp +++ b/apriltag/src/dev/native/cpp/main.cpp @@ -2,10 +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. -#include "frc/apriltag/AprilTagDetector.h" +#include "wpi/apriltag/AprilTagDetector.hpp" int main() { - frc::AprilTagDetector detector; + wpi::apriltag::AprilTagDetector detector; detector.AddFamily("tag16h5"); detector.SetConfig({.refineEdges = false}); } diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTag.java similarity index 92% rename from apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java rename to apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTag.java index e145cbc0e5e..05f8fab1818 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java +++ b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTag.java @@ -2,14 +2,14 @@ // 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; +package org.wpilib.vision.apriltag; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonProperty; -import edu.wpi.first.apriltag.jni.AprilTagJNI; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.util.RawFrame; import java.util.Objects; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.util.RawFrame; +import org.wpilib.vision.apriltag.jni.AprilTagJNI; /** Represents an AprilTag's metadata. */ @SuppressWarnings("MemberName") diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagDetection.java similarity index 96% rename from apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java rename to apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagDetection.java index 954cb8635da..919356f90cb 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetection.java +++ b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagDetection.java @@ -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. -package edu.wpi.first.apriltag; +package org.wpilib.vision.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; +import org.wpilib.math.linalg.MatBuilder; +import org.wpilib.math.linalg.Matrix; +import org.wpilib.math.numbers.N3; +import org.wpilib.math.util.Nat; /** A detection of an AprilTag tag. */ public class AprilTagDetection { diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagDetector.java similarity index 99% rename from apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java rename to apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagDetector.java index 23473f54123..6bba9cc671e 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagDetector.java +++ b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagDetector.java @@ -2,10 +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. -package edu.wpi.first.apriltag; +package org.wpilib.vision.apriltag; -import edu.wpi.first.apriltag.jni.AprilTagJNI; import org.opencv.core.Mat; +import org.wpilib.vision.apriltag.jni.AprilTagJNI; /** * An AprilTag detector engine. This is expensive to set up and tear down, so most use cases should diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagFieldLayout.java similarity index 98% rename from apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java rename to apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagFieldLayout.java index 3a85ded7c03..0c24a739622 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java +++ b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagFieldLayout.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package edu.wpi.first.apriltag; +package org.wpilib.vision.apriltag; import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; @@ -10,9 +10,6 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; import java.io.IOException; import java.io.InputStream; import java.io.InputStreamReader; @@ -25,6 +22,9 @@ import java.util.Map; import java.util.Objects; import java.util.Optional; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Translation3d; /** * Class for representing a layout of AprilTags on a field and reading them from a JSON format. diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagFields.java similarity index 92% rename from apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java rename to apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagFields.java index 2050ed3e087..d2fb0199cc1 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java +++ b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagFields.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package edu.wpi.first.apriltag; +package org.wpilib.vision.apriltag; import java.io.UncheckedIOException; @@ -20,7 +20,7 @@ public enum AprilTagFields { k2025ReefscapeAndyMark("2025-reefscape-andymark.json"); /** Base resource directory. */ - public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/"; + public static final String kBaseResourceDir = "/org/wpilib/vision/apriltag/"; /** Alias to the current game. */ public static final AprilTagFields kDefaultField = k2025ReefscapeWelded; diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimate.java b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagPoseEstimate.java similarity index 94% rename from apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimate.java rename to apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagPoseEstimate.java index 7bf079a0f71..54727e1a4f8 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimate.java +++ b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagPoseEstimate.java @@ -2,9 +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. -package edu.wpi.first.apriltag; +package org.wpilib.vision.apriltag; -import edu.wpi.first.math.geometry.Transform3d; +import org.wpilib.math.geometry.Transform3d; /** A pair of AprilTag pose estimates. */ @SuppressWarnings("MemberName") diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimator.java b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagPoseEstimator.java similarity index 97% rename from apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimator.java rename to apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagPoseEstimator.java index d0d8865a71e..224b78f3ef8 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagPoseEstimator.java +++ b/apriltag/src/main/java/org/wpilib/vision/apriltag/AprilTagPoseEstimator.java @@ -2,10 +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. -package edu.wpi.first.apriltag; +package org.wpilib.vision.apriltag; -import edu.wpi.first.apriltag.jni.AprilTagJNI; -import edu.wpi.first.math.geometry.Transform3d; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.vision.apriltag.jni.AprilTagJNI; /** Pose estimators for AprilTag tags. */ public class AprilTagPoseEstimator { diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java b/apriltag/src/main/java/org/wpilib/vision/apriltag/jni/AprilTagJNI.java similarity index 95% rename from apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java rename to apriltag/src/main/java/org/wpilib/vision/apriltag/jni/AprilTagJNI.java index 87c61e8279c..0c013f4a8f7 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/jni/AprilTagJNI.java +++ b/apriltag/src/main/java/org/wpilib/vision/apriltag/jni/AprilTagJNI.java @@ -2,16 +2,16 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package edu.wpi.first.apriltag.jni; - -import edu.wpi.first.apriltag.AprilTagDetection; -import edu.wpi.first.apriltag.AprilTagDetector; -import edu.wpi.first.apriltag.AprilTagPoseEstimate; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.util.RawFrame; -import edu.wpi.first.util.RuntimeLoader; +package org.wpilib.vision.apriltag.jni; + import java.io.IOException; import java.util.concurrent.atomic.AtomicBoolean; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.util.RawFrame; +import org.wpilib.util.runtime.RuntimeLoader; +import org.wpilib.vision.apriltag.AprilTagDetection; +import org.wpilib.vision.apriltag.AprilTagDetector; +import org.wpilib.vision.apriltag.AprilTagPoseEstimate; /** AprilTag JNI. */ public class AprilTagJNI { diff --git a/apriltag/src/main/native/cpp/AprilTag.cpp b/apriltag/src/main/native/cpp/AprilTag.cpp index 99ac81b49cf..adcbe68fb5f 100644 --- a/apriltag/src/main/native/cpp/AprilTag.cpp +++ b/apriltag/src/main/native/cpp/AprilTag.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 "frc/apriltag/AprilTag.h" +#include "wpi/apriltag/AprilTag.hpp" #include -#include +#include "wpi/util/json.hpp" #ifdef _WIN32 #pragma warning(disable : 4200) @@ -20,9 +20,9 @@ #include "tag16h5.h" #include "tag36h11.h" -using namespace frc; +using namespace wpi::apriltag; -static bool FamilyToImage(wpi::RawFrame* frame, apriltag_family_t* family, +static bool FamilyToImage(wpi::util::RawFrame* frame, apriltag_family_t* family, int id) { image_u8_t* image = apriltag_to_image(family, id); size_t totalDataSize = image->height * image->stride; @@ -37,25 +37,25 @@ static bool FamilyToImage(wpi::RawFrame* frame, apriltag_family_t* family, return rv; } -bool AprilTag::Generate36h11AprilTagImage(wpi::RawFrame* frame, int id) { +bool AprilTag::Generate36h11AprilTagImage(wpi::util::RawFrame* frame, int id) { apriltag_family_t* tagFamily = tag36h11_create(); bool rv = FamilyToImage(frame, tagFamily, id); tag36h11_destroy(tagFamily); return rv; } -bool AprilTag::Generate16h5AprilTagImage(wpi::RawFrame* frame, int id) { +bool AprilTag::Generate16h5AprilTagImage(wpi::util::RawFrame* frame, int id) { apriltag_family_t* tagFamily = tag16h5_create(); bool rv = FamilyToImage(frame, tagFamily, id); tag16h5_destroy(tagFamily); return rv; } -void frc::to_json(wpi::json& json, const AprilTag& apriltag) { - json = wpi::json{{"ID", apriltag.ID}, {"pose", apriltag.pose}}; +void wpi::apriltag::to_json(wpi::util::json& json, const AprilTag& apriltag) { + json = wpi::util::json{{"ID", apriltag.ID}, {"pose", apriltag.pose}}; } -void frc::from_json(const wpi::json& json, AprilTag& apriltag) { +void wpi::apriltag::from_json(const wpi::util::json& json, AprilTag& apriltag) { apriltag.ID = json.at("ID").get(); - apriltag.pose = json.at("pose").get(); + apriltag.pose = json.at("pose").get(); } diff --git a/apriltag/src/main/native/cpp/AprilTagDetection.cpp b/apriltag/src/main/native/cpp/AprilTagDetection.cpp index aaa80be8c6a..61001b5cb80 100644 --- a/apriltag/src/main/native/cpp/AprilTagDetection.cpp +++ b/apriltag/src/main/native/cpp/AprilTagDetection.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 "frc/apriltag/AprilTagDetection.h" +#include "wpi/apriltag/AprilTagDetection.hpp" #include @@ -16,7 +16,7 @@ #include "apriltag.h" -using namespace frc; +using namespace wpi::apriltag; static_assert(sizeof(AprilTagDetection) == sizeof(apriltag_detection_t), "structure sizes don't match"); diff --git a/apriltag/src/main/native/cpp/AprilTagDetector.cpp b/apriltag/src/main/native/cpp/AprilTagDetector.cpp index 458aa39798f..d3c85f1001c 100644 --- a/apriltag/src/main/native/cpp/AprilTagDetector.cpp +++ b/apriltag/src/main/native/cpp/AprilTagDetector.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 "frc/apriltag/AprilTagDetector.h" +#include "wpi/apriltag/AprilTagDetector.hpp" #include #include @@ -19,7 +19,7 @@ #include "tag16h5.h" #include "tag36h11.h" -using namespace frc; +using namespace wpi::apriltag; AprilTagDetector::Results::Results(void* impl, const private_init&) : span{reinterpret_cast( diff --git a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp index 8388210b255..255321198a2 100644 --- a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -2,38 +2,41 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include "frc/apriltag/AprilTagFieldLayout.h" +#include "wpi/apriltag/AprilTagFieldLayout.hpp" #include #include #include -#include -#include -#include -#include -#include +#include "wpi/units/angle.hpp" +#include "wpi/units/length.hpp" +#include "wpi/util/MemoryBuffer.hpp" +#include "wpi/util/json.hpp" +#include "wpi/util/raw_ostream.hpp" -using namespace frc; +using namespace wpi::apriltag; AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) { - auto fileBuffer = wpi::MemoryBuffer::GetFile(path); + auto fileBuffer = wpi::util::MemoryBuffer::GetFile(path); if (!fileBuffer) { throw std::runtime_error(fmt::format("Cannot open file: {}", path)); } - wpi::json json = wpi::json::parse(fileBuffer.value()->GetCharBuffer()); + wpi::util::json json = + wpi::util::json::parse(fileBuffer.value()->GetCharBuffer()); for (const auto& tag : json.at("tags").get>()) { m_apriltags[tag.ID] = tag; } - m_fieldWidth = units::meter_t{json.at("field").at("width").get()}; - m_fieldLength = units::meter_t{json.at("field").at("length").get()}; + m_fieldWidth = + wpi::units::meter_t{json.at("field").at("width").get()}; + m_fieldLength = + wpi::units::meter_t{json.at("field").at("length").get()}; } AprilTagFieldLayout::AprilTagFieldLayout(std::vector apriltags, - units::meter_t fieldLength, - units::meter_t fieldWidth) + wpi::units::meter_t fieldLength, + wpi::units::meter_t fieldWidth) : m_fieldLength(std::move(fieldLength)), m_fieldWidth(std::move(fieldWidth)) { for (const auto& tag : apriltags) { @@ -41,11 +44,11 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::vector apriltags, } } -units::meter_t AprilTagFieldLayout::GetFieldLength() const { +wpi::units::meter_t AprilTagFieldLayout::GetFieldLength() const { return m_fieldLength; } -units::meter_t AprilTagFieldLayout::GetFieldWidth() const { +wpi::units::meter_t AprilTagFieldLayout::GetFieldWidth() const { return m_fieldWidth; } @@ -61,26 +64,27 @@ std::vector AprilTagFieldLayout::GetTags() const { void AprilTagFieldLayout::SetOrigin(OriginPosition origin) { switch (origin) { case OriginPosition::kBlueAllianceWallRightSide: - SetOrigin(Pose3d{}); + SetOrigin(wpi::math::Pose3d{}); break; case OriginPosition::kRedAllianceWallRightSide: - SetOrigin(Pose3d{Translation3d{m_fieldLength, m_fieldWidth, 0_m}, - Rotation3d{0_deg, 0_deg, 180_deg}}); + SetOrigin(wpi::math::Pose3d{ + wpi::math::Translation3d{m_fieldLength, m_fieldWidth, 0_m}, + wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}}); break; default: throw std::invalid_argument("Invalid origin"); } } -void AprilTagFieldLayout::SetOrigin(const Pose3d& origin) { +void AprilTagFieldLayout::SetOrigin(const wpi::math::Pose3d& origin) { m_origin = origin; } -Pose3d AprilTagFieldLayout::GetOrigin() const { +wpi::math::Pose3d AprilTagFieldLayout::GetOrigin() const { return m_origin; } -std::optional AprilTagFieldLayout::GetTagPose(int ID) const { +std::optional AprilTagFieldLayout::GetTagPose(int ID) const { const auto& it = m_apriltags.find(ID); if (it == m_apriltags.end()) { return std::nullopt; @@ -91,43 +95,45 @@ std::optional AprilTagFieldLayout::GetTagPose(int ID) const { void AprilTagFieldLayout::Serialize(std::string_view path) { std::error_code error_code; - wpi::raw_fd_ostream output{path, error_code}; + wpi::util::raw_fd_ostream output{path, error_code}; if (error_code) { throw std::runtime_error(fmt::format("Cannot open file: {}", path)); } - wpi::json json = *this; + wpi::util::json json = *this; output << json; output.flush(); } -void frc::to_json(wpi::json& json, const AprilTagFieldLayout& layout) { +void wpi::apriltag::to_json(wpi::util::json& json, + const AprilTagFieldLayout& layout) { std::vector tagVector; tagVector.reserve(layout.m_apriltags.size()); for (const auto& pair : layout.m_apriltags) { tagVector.push_back(pair.second); } - json = wpi::json{{"field", - {{"length", layout.m_fieldLength.value()}, - {"width", layout.m_fieldWidth.value()}}}, - {"tags", tagVector}}; + json = wpi::util::json{{"field", + {{"length", layout.m_fieldLength.value()}, + {"width", layout.m_fieldWidth.value()}}}, + {"tags", tagVector}}; } -void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) { +void wpi::apriltag::from_json(const wpi::util::json& json, + AprilTagFieldLayout& layout) { layout.m_apriltags.clear(); for (const auto& tag : json.at("tags").get>()) { layout.m_apriltags[tag.ID] = tag; } layout.m_fieldLength = - units::meter_t{json.at("field").at("length").get()}; + wpi::units::meter_t{json.at("field").at("length").get()}; layout.m_fieldWidth = - units::meter_t{json.at("field").at("width").get()}; + wpi::units::meter_t{json.at("field").at("width").get()}; } // Use namespace declaration for forward declaration -namespace frc { +namespace wpi::apriltag { // C++ generated from resource files std::string_view GetResource_2022_rapidreact_json(); @@ -136,7 +142,7 @@ std::string_view GetResource_2024_crescendo_json(); std::string_view GetResource_2025_reefscape_welded_json(); std::string_view GetResource_2025_reefscape_andymark_json(); -} // namespace frc +} // namespace wpi::apriltag AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) { std::string_view fieldString; @@ -160,10 +166,11 @@ AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) { throw std::invalid_argument("Invalid Field"); } - wpi::json json = wpi::json::parse(fieldString); + wpi::util::json json = wpi::util::json::parse(fieldString); return json.get(); } -AprilTagFieldLayout frc::LoadAprilTagLayoutField(AprilTagField field) { +AprilTagFieldLayout wpi::apriltag::LoadAprilTagLayoutField( + AprilTagField field) { return AprilTagFieldLayout::LoadField(field); } diff --git a/apriltag/src/main/native/cpp/AprilTagPoseEstimate.cpp b/apriltag/src/main/native/cpp/AprilTagPoseEstimate.cpp index f755fd539cd..343642e6ede 100644 --- a/apriltag/src/main/native/cpp/AprilTagPoseEstimate.cpp +++ b/apriltag/src/main/native/cpp/AprilTagPoseEstimate.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 "frc/apriltag/AprilTagPoseEstimate.h" +#include "wpi/apriltag/AprilTagPoseEstimate.hpp" #include -using namespace frc; +using namespace wpi::apriltag; double AprilTagPoseEstimate::GetAmbiguity() const { auto min = (std::min)(error1, error2); diff --git a/apriltag/src/main/native/cpp/AprilTagPoseEstimator.cpp b/apriltag/src/main/native/cpp/AprilTagPoseEstimator.cpp index 29a0802ded5..ec92e4bdb00 100644 --- a/apriltag/src/main/native/cpp/AprilTagPoseEstimator.cpp +++ b/apriltag/src/main/native/cpp/AprilTagPoseEstimator.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 "frc/apriltag/AprilTagPoseEstimator.h" +#include "wpi/apriltag/AprilTagPoseEstimator.hpp" #include -#include "frc/apriltag/AprilTagDetection.h" +#include "wpi/apriltag/AprilTagDetection.hpp" #ifdef _WIN32 #pragma warning(disable : 4200) @@ -19,7 +19,7 @@ #include "apriltag.h" #include "apriltag_pose.h" -using namespace frc; +using namespace wpi::apriltag; static Eigen::Matrix3d OrthogonalizeRotationMatrix( const Eigen::Matrix3d& input) { @@ -42,14 +42,14 @@ static Eigen::Matrix3d OrthogonalizeRotationMatrix( return Q; } -static Transform3d MakePose(const apriltag_pose_t& pose) { +static wpi::math::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( + return {wpi::math::Translation3d{wpi::units::meter_t{pose.t->data[0]}, + wpi::units::meter_t{pose.t->data[1]}, + wpi::units::meter_t{pose.t->data[2]}}, + wpi::math::Rotation3d{OrthogonalizeRotationMatrix( Eigen::Map>{ pose.R->data})}}; } @@ -80,7 +80,7 @@ static apriltag_detection_t MakeBasicDet( return detection; } -static Transform3d DoEstimateHomography( +static wpi::math::Transform3d DoEstimateHomography( const apriltag_detection_t* detection, const AprilTagPoseEstimator::Config& config) { auto info = MakeDetectionInfo(detection, config); @@ -89,13 +89,13 @@ static Transform3d DoEstimateHomography( return MakePose(pose); } -Transform3d AprilTagPoseEstimator::EstimateHomography( +wpi::math::Transform3d AprilTagPoseEstimator::EstimateHomography( const AprilTagDetection& detection) const { return DoEstimateHomography( reinterpret_cast(&detection), m_config); } -Transform3d AprilTagPoseEstimator::EstimateHomography( +wpi::math::Transform3d AprilTagPoseEstimator::EstimateHomography( std::span homography) const { auto detection = MakeBasicDet(homography, nullptr); auto rv = DoEstimateHomography(&detection, m_config); @@ -130,21 +130,22 @@ AprilTagPoseEstimate AprilTagPoseEstimator::EstimateOrthogonalIteration( return rv; } -static Transform3d DoEstimate(const apriltag_detection_t* detection, - const AprilTagPoseEstimator::Config& config) { +static wpi::math::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( +wpi::math::Transform3d AprilTagPoseEstimator::Estimate( const AprilTagDetection& detection) const { return DoEstimate(reinterpret_cast(&detection), m_config); } -Transform3d AprilTagPoseEstimator::Estimate( +wpi::math::Transform3d AprilTagPoseEstimator::Estimate( std::span homography, std::span corners) const { auto detection = MakeBasicDet(homography, &corners); diff --git a/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp b/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp index b68b131087d..b47c9ef5d61 100644 --- a/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp +++ b/apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp @@ -8,16 +8,16 @@ #include #define WPI_RAWFRAME_JNI -#include -#include +#include "wpi/util/RawFrame.h" +#include "wpi/util/jni_util.hpp" -#include "edu_wpi_first_apriltag_jni_AprilTagJNI.h" -#include "frc/apriltag/AprilTag.h" -#include "frc/apriltag/AprilTagDetector.h" -#include "frc/apriltag/AprilTagPoseEstimator.h" +#include "org_wpilib_vision_apriltag_jni_AprilTagJNI.h" +#include "wpi/apriltag/AprilTag.hpp" +#include "wpi/apriltag/AprilTagDetector.hpp" +#include "wpi/apriltag/AprilTagPoseEstimator.hpp" -using namespace frc; -using namespace wpi::java; +using namespace wpi::apriltag; +using namespace wpi::util::java; static JavaVM* jvm = nullptr; @@ -34,16 +34,16 @@ 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", + {"org/wpilib/vision/apriltag/AprilTagDetection", &detectionCls}, + {"org/wpilib/vision/apriltag/AprilTagDetector$Config", &detectorConfigCls}, + {"org/wpilib/vision/apriltag/AprilTagDetector$QuadThresholdParameters", &detectorQTPCls}, - {"edu/wpi/first/apriltag/AprilTagPoseEstimate", &poseEstimateCls}, - {"edu/wpi/first/math/geometry/Quaternion", &quaternionCls}, - {"edu/wpi/first/math/geometry/Rotation3d", &rotation3dCls}, - {"edu/wpi/first/math/geometry/Transform3d", &transform3dCls}, - {"edu/wpi/first/math/geometry/Translation3d", &translation3dCls}, - {"edu/wpi/first/util/RawFrame", &rawFrameCls}}; + {"org/wpilib/vision/apriltag/AprilTagPoseEstimate", &poseEstimateCls}, + {"org/wpilib/math/geometry/Quaternion", &quaternionCls}, + {"org/wpilib/math/geometry/Rotation3d", &rotation3dCls}, + {"org/wpilib/math/geometry/Transform3d", &transform3dCls}, + {"org/wpilib/math/geometry/Translation3d", &translation3dCls}, + {"org/wpilib/util/RawFrame", &rawFrameCls}}; static const JExceptionInit exceptions[] = { {"java/lang/IllegalArgumentException", &illegalArgEx}, @@ -162,7 +162,7 @@ static AprilTagDetector::QuadThresholdParameters FromJavaDetectorQTP( return { FIELD(int, Int, minClusterPixels), FIELD(int, Int, maxNumMaxima), - .criticalAngle = units::radian_t{static_cast( + .criticalAngle = wpi::units::radian_t{static_cast( env->GetDoubleField(jparams, criticalAngleField))}, FIELD(float, Float, maxLineFitMSE), FIELD(int, Int, minWhiteBlackDiff), @@ -256,7 +256,7 @@ static jobject MakeJObject( static_cast(params.deglitch)); } -static jobject MakeJObject(JNIEnv* env, const Translation3d& xlate) { +static jobject MakeJObject(JNIEnv* env, const wpi::math::Translation3d& xlate) { static jmethodID constructor = env->GetMethodID(translation3dCls, "", "(DDD)V"); if (!constructor) { @@ -268,7 +268,7 @@ static jobject MakeJObject(JNIEnv* env, const Translation3d& xlate) { static_cast(xlate.Y()), static_cast(xlate.Z())); } -static jobject MakeJObject(JNIEnv* env, const Quaternion& q) { +static jobject MakeJObject(JNIEnv* env, const wpi::math::Quaternion& q) { static jmethodID constructor = env->GetMethodID(quaternionCls, "", "(DDDD)V"); if (!constructor) { @@ -281,9 +281,9 @@ static jobject MakeJObject(JNIEnv* env, const Quaternion& q) { static_cast(q.Z())); } -static jobject MakeJObject(JNIEnv* env, const Rotation3d& rot) { +static jobject MakeJObject(JNIEnv* env, const wpi::math::Rotation3d& rot) { static jmethodID constructor = env->GetMethodID( - rotation3dCls, "", "(Ledu/wpi/first/math/geometry/Quaternion;)V"); + rotation3dCls, "", "(Lorg/wpilib/math/geometry/Quaternion;)V"); if (!constructor) { return nullptr; } @@ -292,11 +292,11 @@ static jobject MakeJObject(JNIEnv* env, const Rotation3d& rot) { return env->NewObject(rotation3dCls, constructor, q.obj()); } -static jobject MakeJObject(JNIEnv* env, const Transform3d& xform) { +static jobject MakeJObject(JNIEnv* env, const wpi::math::Transform3d& xform) { static jmethodID constructor = env->GetMethodID(transform3dCls, "", - "(Ledu/wpi/first/math/geometry/Translation3d;" - "Ledu/wpi/first/math/geometry/Rotation3d;)V"); + "(Lorg/wpilib/math/geometry/Translation3d;" + "Lorg/wpilib/math/geometry/Rotation3d;)V"); if (!constructor) { return nullptr; } @@ -309,8 +309,8 @@ static jobject MakeJObject(JNIEnv* env, const Transform3d& xform) { static jobject MakeJObject(JNIEnv* env, const AprilTagPoseEstimate& est) { static jmethodID constructor = env->GetMethodID(poseEstimateCls, "", - "(Ledu/wpi/first/math/geometry/Transform3d;" - "Ledu/wpi/first/math/geometry/Transform3d;DD)V"); + "(Lorg/wpilib/math/geometry/Transform3d;" + "Lorg/wpilib/math/geometry/Transform3d;DD)V"); if (!constructor) { return nullptr; } @@ -325,36 +325,36 @@ static jobject MakeJObject(JNIEnv* env, const AprilTagPoseEstimate& est) { extern "C" { /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: createDetector * Signature: ()J */ JNIEXPORT jlong JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_createDetector +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_createDetector (JNIEnv* env, jclass) { return reinterpret_cast(new AprilTagDetector); } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: destroyDetector * Signature: (J)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_destroyDetector +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_destroyDetector (JNIEnv* env, jclass, jlong det) { delete reinterpret_cast(det); } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: setDetectorConfig * Signature: (JLjava/lang/Object;)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_setDetectorConfig +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_setDetectorConfig (JNIEnv* env, jclass, jlong det, jobject config) { if (det == 0) { @@ -366,12 +366,12 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_setDetectorConfig } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: getDetectorConfig * Signature: (J)Ljava/lang/Object; */ JNIEXPORT jobject JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_getDetectorConfig +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_getDetectorConfig (JNIEnv* env, jclass, jlong det) { if (det == 0) { @@ -383,12 +383,12 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_getDetectorConfig } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: setDetectorQTP * Signature: (JLjava/lang/Object;)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_setDetectorQTP +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_setDetectorQTP (JNIEnv* env, jclass, jlong det, jobject params) { if (det == 0) { @@ -400,12 +400,12 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_setDetectorQTP } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: getDetectorQTP * Signature: (J)Ljava/lang/Object; */ JNIEXPORT jobject JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_getDetectorQTP +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_getDetectorQTP (JNIEnv* env, jclass, jlong det) { if (det == 0) { @@ -418,12 +418,12 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_getDetectorQTP } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: addFamily * Signature: (JLjava/lang/String;I)Z */ JNIEXPORT jboolean JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_addFamily +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_addFamily (JNIEnv* env, jclass, jlong det, jstring fam, jint bitsCorrected) { if (det == 0) { @@ -439,12 +439,12 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_addFamily } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: removeFamily * Signature: (JLjava/lang/String;)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_removeFamily +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_removeFamily (JNIEnv* env, jclass, jlong det, jstring fam) { if (det == 0) { @@ -459,12 +459,12 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_removeFamily } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: clearFamilies * Signature: (J)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_clearFamilies +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_clearFamilies (JNIEnv* env, jclass, jlong det) { if (det == 0) { @@ -475,12 +475,12 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_clearFamilies } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: detect * Signature: (JIIIJ)[Ljava/lang/Object; */ JNIEXPORT jobjectArray JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_detect +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_detect (JNIEnv* env, jclass, jlong det, jint width, jint height, jint stride, jlong bufAddr) { @@ -498,12 +498,12 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_detect } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: estimatePoseHomography * Signature: ([DDDDDD)Ljava/lang/Object; */ JNIEXPORT jobject JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePoseHomography +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_estimatePoseHomography (JNIEnv* env, jclass, jdoubleArray homography, jdouble tagSize, jdouble fx, jdouble fy, jdouble cx, jdouble cy) { @@ -517,17 +517,18 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePoseHomography return nullptr; } - AprilTagPoseEstimator estimator({units::meter_t{tagSize}, fx, fy, cx, cy}); + AprilTagPoseEstimator estimator( + {wpi::units::meter_t{tagSize}, fx, fy, cx, cy}); return MakeJObject(env, estimator.EstimateHomography(harr)); } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: estimatePoseOrthogonalIteration * Signature: ([D[DDDDDDI)Ljava/lang/Object; */ JNIEXPORT jobject JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePoseOrthogonalIteration +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_estimatePoseOrthogonalIteration (JNIEnv* env, jclass, jdoubleArray homography, jdoubleArray corners, jdouble tagSize, jdouble fx, jdouble fy, jdouble cx, jdouble cy, jint nIters) { @@ -553,18 +554,19 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePoseOrthogonalIteration return nullptr; } - AprilTagPoseEstimator estimator({units::meter_t{tagSize}, fx, fy, cx, cy}); + AprilTagPoseEstimator estimator( + {wpi::units::meter_t{tagSize}, fx, fy, cx, cy}); return MakeJObject(env, estimator.EstimateOrthogonalIteration(harr, carr, nIters)); } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: estimatePose * Signature: ([D[DDDDDD)Ljava/lang/Object; */ JNIEXPORT jobject JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePose +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_estimatePose (JNIEnv* env, jclass, jdoubleArray homography, jdoubleArray corners, jdouble tagSize, jdouble fx, jdouble fy, jdouble cx, jdouble cy) { @@ -590,45 +592,46 @@ Java_edu_wpi_first_apriltag_jni_AprilTagJNI_estimatePose return nullptr; } - AprilTagPoseEstimator estimator({units::meter_t{tagSize}, fx, fy, cx, cy}); + AprilTagPoseEstimator estimator( + {wpi::units::meter_t{tagSize}, fx, fy, cx, cy}); return MakeJObject(env, estimator.Estimate(harr, carr)); } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: generate16h5AprilTagImage * Signature: (Ljava/lang/Object;JI)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_generate16h5AprilTagImage +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_generate16h5AprilTagImage (JNIEnv* env, jclass, jobject frameObj, jlong framePtr, jint id) { - auto* frame = reinterpret_cast(framePtr); + auto* frame = reinterpret_cast(framePtr); if (!frame) { nullPointerEx.Throw(env, "frame is null"); return; } bool newData = AprilTag::Generate16h5AprilTagImage(frame, id); - wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, newData); + wpi::util::SetFrameData(env, rawFrameCls, frameObj, *frame, newData); } /* - * Class: edu_wpi_first_apriltag_jni_AprilTagJNI + * Class: org_wpilib_vision_apriltag_jni_AprilTagJNI * Method: generate36h11AprilTagImage * Signature: (Ljava/lang/Object;JI)V */ JNIEXPORT void JNICALL -Java_edu_wpi_first_apriltag_jni_AprilTagJNI_generate36h11AprilTagImage +Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_generate36h11AprilTagImage (JNIEnv* env, jclass, jobject frameObj, jlong framePtr, jint id) { - auto* frame = reinterpret_cast(framePtr); + auto* frame = reinterpret_cast(framePtr); if (!frame) { nullPointerEx.Throw(env, "frame is null"); return; } // function might reallocate bool newData = AprilTag::Generate36h11AprilTagImage(frame, id); - wpi::SetFrameData(env, rawFrameCls, frameObj, *frame, newData); + wpi::util::SetFrameData(env, rawFrameCls, frameObj, *frame, newData); } } // extern "C" diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTag.h b/apriltag/src/main/native/include/frc/apriltag/AprilTag.h deleted file mode 100644 index a9ef78cc458..00000000000 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTag.h +++ /dev/null @@ -1,37 +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. - -#pragma once - -#include -#include -#include - -#include "frc/geometry/Pose3d.h" - -namespace frc { - -/** - * Represents an AprilTag's metadata. - */ -struct WPILIB_DLLEXPORT AprilTag { - /// The tag's ID. - int ID; - - /// The tag's pose. - Pose3d pose; - - bool operator==(const AprilTag&) const = default; - - static bool Generate36h11AprilTagImage(wpi::RawFrame* frame, int id); - static bool Generate16h5AprilTagImage(wpi::RawFrame* frame, int id); -}; - -WPILIB_DLLEXPORT -void to_json(wpi::json& json, const AprilTag& apriltag); - -WPILIB_DLLEXPORT -void from_json(const wpi::json& json, AprilTag& apriltag); - -} // namespace frc diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagDetection.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagDetection.h deleted file mode 100644 index 74f8c64ceba..00000000000 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagDetection.h +++ /dev/null @@ -1,160 +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. - -#pragma once - -#include - -#include -#include - -#include -#include - -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 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(c); } - - /** - * Gets a corner of the tag in image pixel coordinates. These always - * wrap counter-clock wise around the tag. Index 0 is the bottom left corner. - * - * @param ndx Corner index (range is 0-3, inclusive) - * @return Corner point - */ - const Point& GetCorner(int ndx) const { - return *reinterpret_cast(p[ndx]); - } - - /** - * Gets the corners of the tag in image pixel coordinates. These always - * wrap counter-clock wise around the tag. The first set of corner coordinates - * are the coordinates for the bottom left corner. - * - * @param cornersBuf Corner point array (X and Y for each corner in order) - * @return Corner point array (copy of cornersBuf span) - */ - std::span GetCorners(std::span 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 diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector.h deleted file mode 100644 index 29e5b79d4f8..00000000000 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector.h +++ /dev/null @@ -1,262 +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. - -#pragma once - -#include - -#include -#include -#include -#include - -#include -#include -#include - -#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 300 - * pixels. - */ - int minClusterPixels = 300; - - /** - * 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 45 degrees. - */ - units::radian_t criticalAngle = 45_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 { - struct private_init {}; - friend class AprilTagDetector; - - public: - Results() = default; - Results(void* impl, const private_init&); - ~Results() { Destroy(); } - Results(const Results&) = delete; - Results& operator=(const Results&) = delete; - Results(Results&& rhs) : span{std::move(rhs)}, m_impl{rhs.m_impl} { - rhs.m_impl = nullptr; - } - Results& operator=(Results&& rhs); - - private: - void Destroy(); - void* m_impl = nullptr; - }; - - AprilTagDetector(); - ~AprilTagDetector() { Destroy(); } - AprilTagDetector(const AprilTagDetector&) = delete; - AprilTagDetector& operator=(const AprilTagDetector&) = delete; - AprilTagDetector(AprilTagDetector&& rhs) - : m_impl{rhs.m_impl}, - m_families{std::move(rhs.m_families)}, - m_qtpCriticalAngle{rhs.m_qtpCriticalAngle} { - rhs.m_impl = nullptr; - } - AprilTagDetector& operator=(AprilTagDetector&& rhs); - - /** - * @{ - * @name Configuration functions - */ - - /** - * Sets detector configuration. - * - * @param config Configuration - */ - void SetConfig(const Config& config); - - /** - * Gets detector configuration. - * - * @return Configuration - */ - Config GetConfig() const; - - /** - * Sets quad threshold parameters. - * - * @param params Parameters - */ - void SetQuadThresholdParameters(const QuadThresholdParameters& params); - - /** - * Gets quad threshold parameters. - * - * @return Parameters - */ - QuadThresholdParameters GetQuadThresholdParameters() const; - - /** @} */ - - /** - * @{ - * @name Tag family functions - */ - - /** - * Adds a family of tags to be detected. - * - * @param fam Family name, e.g. "tag16h5" - * @param bitsCorrected Maximum number of bits to correct - * @return False if family can't be found - */ - bool AddFamily(std::string_view fam, int bitsCorrected = 2); - - /** - * Removes a family of tags from the detector. - * - * @param fam Family name, e.g. "tag16h5" - */ - void RemoveFamily(std::string_view fam); - - /** - * Unregister all families. - */ - void ClearFamilies(); - - /** @} */ - - /** - * Detect tags from an 8-bit image. - * The image must be grayscale. - * - * @param width width of the image - * @param height height of the image - * @param stride number of bytes between image rows (often the same as width) - * @param buf image buffer - * @return Results (array of AprilTagDetection pointers) - */ - Results Detect(int width, int height, int stride, uint8_t* buf); - - /** - * Detect tags from an 8-bit image. - * The image must be grayscale. - * - * @param width width of the image - * @param height height of the image - * @param buf image buffer - * @return Results (array of AprilTagDetection pointers) - */ - Results Detect(int width, int height, uint8_t* buf) { - return Detect(width, height, width, buf); - } - - private: - void Destroy(); - void DestroyFamilies(); - void DestroyFamily(std::string_view name, void* data); - - void* m_impl; - wpi::StringMap m_families; - units::radian_t m_qtpCriticalAngle = 10_deg; -}; - -} // namespace frc diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector_cv.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector_cv.h deleted file mode 100644 index 7231430ed31..00000000000 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagDetector_cv.h +++ /dev/null @@ -1,18 +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. - -#pragma once - -#include - -#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 diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h deleted file mode 100644 index c2e73c78c4d..00000000000 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ /dev/null @@ -1,175 +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. - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include - -#include "frc/apriltag/AprilTag.h" -#include "frc/apriltag/AprilTagFields.h" -#include "frc/geometry/Pose3d.h" - -namespace frc { -/** - * Class for representing a layout of AprilTags on a field and reading them from - * a JSON format. - * - * The JSON format contains two top-level objects, "tags" and "field". - * The "tags" object is a list of all AprilTags contained within a layout. Each - * AprilTag serializes to a JSON object containing an ID and a Pose3d. The - * "field" object is a descriptor of the size of the field in meters with - * "width" and "length" values. This is to account for arbitrary field sizes - * when transforming the poses. - * - * Pose3ds in the JSON are measured using the normal FRC coordinate system, NWU - * with the origin at the bottom-right corner of the blue alliance wall. - * SetOrigin(OriginPosition) can be used to change the poses returned from - * GetTagPose(int) to be from the perspective of a specific alliance. - * - * Tag poses represent the center of the tag, with a zero rotation representing - * a tag that is upright and facing away from the (blue) alliance wall (that is, - * towards the opposing alliance). */ -class WPILIB_DLLEXPORT AprilTagFieldLayout { - public: - /** - * Common origin positions for the AprilTag coordinate system. - */ - enum class OriginPosition { - /// Blue alliance wall, right side. - kBlueAllianceWallRightSide, - /// Red alliance wall, right side. - kRedAllianceWallRightSide, - }; - - /** - * Loads an AprilTagFieldLayout from a predefined field - * - * @param field The predefined field - * @return AprilTagFieldLayout of the field - */ - static AprilTagFieldLayout LoadField(AprilTagField field); - - AprilTagFieldLayout() = default; - - /** - * Construct a new AprilTagFieldLayout with values imported from a JSON file. - * - * @param path Path of the JSON file to import from. - */ - explicit AprilTagFieldLayout(std::string_view path); - - /** - * Construct a new AprilTagFieldLayout from a vector of AprilTag objects. - * - * @param apriltags Vector of AprilTags. - * @param fieldLength Length of field the layout is representing. - * @param fieldWidth Width of field the layout is representing. - */ - AprilTagFieldLayout(std::vector apriltags, - units::meter_t fieldLength, units::meter_t fieldWidth); - - /** - * Returns the length of the field the layout is representing. - * @return length - */ - units::meter_t GetFieldLength() const; - - /** - * Returns the length of the field the layout is representing. - * @return width - */ - units::meter_t GetFieldWidth() const; - - /** - * Returns a vector of all the april tags used in this layout. - * @return list of tags - */ - std::vector GetTags() const; - - /** - * Sets the origin based on a predefined enumeration of coordinate frame - * origins. The origins are calculated from the field dimensions. - * - * This transforms the Pose3ds returned by GetTagPose(int) to return the - * correct pose relative to a predefined coordinate frame. - * - * @param origin The predefined origin - */ - void SetOrigin(OriginPosition origin); - - /** - * Sets the origin for tag pose transformation. - * - * This transforms the Pose3ds returned by GetTagPose(int) to return the - * correct pose relative to the provided origin. - * - * @param origin The new origin for tag transformations - */ - void SetOrigin(const Pose3d& origin); - - /** - * Returns the origin used for tag pose transformation. - * @return the origin - */ - Pose3d GetOrigin() const; - - /** - * Gets an AprilTag pose by its ID. - * - * @param ID The ID of the tag. - * @return The pose corresponding to the ID that was passed in or an empty - * optional if a tag with that ID is not found. - */ - std::optional GetTagPose(int ID) const; - - /** - * Serializes an AprilTagFieldLayout to a JSON file. - * - * @param path The path to write the JSON file to. - */ - void Serialize(std::string_view path); - - /* - * Checks equality between this AprilTagFieldLayout and another object. - */ - bool operator==(const AprilTagFieldLayout&) const = default; - - private: - std::unordered_map m_apriltags; - units::meter_t m_fieldLength; - units::meter_t m_fieldWidth; - Pose3d m_origin; - - friend WPILIB_DLLEXPORT void to_json(wpi::json& json, - const AprilTagFieldLayout& layout); - - friend WPILIB_DLLEXPORT void from_json(const wpi::json& json, - AprilTagFieldLayout& layout); -}; - -WPILIB_DLLEXPORT -void to_json(wpi::json& json, const AprilTagFieldLayout& layout); - -WPILIB_DLLEXPORT -void from_json(const wpi::json& json, AprilTagFieldLayout& layout); - -/** - * Loads an AprilTagFieldLayout from a predefined field - * - * @param field The predefined field - * @return AprilTagFieldLayout of the field - * @deprecated Use AprilTagFieldLayout::LoadField() instead - */ -[[deprecated("Use AprilTagFieldLayout::LoadField() instead")]] -WPILIB_DLLEXPORT AprilTagFieldLayout -LoadAprilTagLayoutField(AprilTagField field); - -} // namespace frc diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h deleted file mode 100644 index 2e1f5cc795c..00000000000 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h +++ /dev/null @@ -1,35 +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. - -#pragma once - -#include - -#include - -namespace frc { - -/** - * Loadable AprilTag field layouts. - */ -enum class AprilTagField { - /// 2022 Rapid React. - k2022RapidReact, - /// 2023 Charged Up. - k2023ChargedUp, - /// 2024 Crescendo. - k2024Crescendo, - /// 2025 Reefscape AndyMark (see TU12). - k2025ReefscapeAndyMark, - /// 2025 Reefscape Welded (see TU12). - k2025ReefscapeWelded, - /// Alias to the current game. - kDefaultField = k2025ReefscapeWelded, - - // This is a placeholder for denoting the last supported field. This should - // always be the last entry in the enum and should not be used by users - kNumFields, -}; - -} // namespace frc diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimate.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimate.h deleted file mode 100644 index 28c7d51a6b6..00000000000 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimate.h +++ /dev/null @@ -1,36 +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. - -#pragma once - -#include - -#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 diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimator.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimator.h deleted file mode 100644 index 1e1e8524275..00000000000 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagPoseEstimator.h +++ /dev/null @@ -1,145 +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. - -#pragma once - -#include - -#include -#include - -#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 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 homography, std::span 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 homography, - std::span corners) const; - - private: - Config m_config; -}; - -} // namespace frc diff --git a/apriltag/src/main/native/include/wpi/apriltag/AprilTag.hpp b/apriltag/src/main/native/include/wpi/apriltag/AprilTag.hpp new file mode 100644 index 00000000000..5189c05be42 --- /dev/null +++ b/apriltag/src/main/native/include/wpi/apriltag/AprilTag.hpp @@ -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/math/geometry/Pose3d.hpp" +#include "wpi/util/RawFrame.h" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/json_fwd.hpp" + +namespace wpi::apriltag { + +/** + * Represents an AprilTag's metadata. + */ +struct WPILIB_DLLEXPORT AprilTag { + /// The tag's ID. + int ID; + + /// The tag's pose. + wpi::math::Pose3d pose; + + bool operator==(const AprilTag&) const = default; + + static bool Generate36h11AprilTagImage(wpi::util::RawFrame* frame, int id); + static bool Generate16h5AprilTagImage(wpi::util::RawFrame* frame, int id); +}; + +WPILIB_DLLEXPORT +void to_json(wpi::util::json& json, const AprilTag& apriltag); + +WPILIB_DLLEXPORT +void from_json(const wpi::util::json& json, AprilTag& apriltag); + +} // namespace wpi::apriltag diff --git a/apriltag/src/main/native/include/wpi/apriltag/AprilTagDetection.hpp b/apriltag/src/main/native/include/wpi/apriltag/AprilTagDetection.hpp new file mode 100644 index 00000000000..8b888686390 --- /dev/null +++ b/apriltag/src/main/native/include/wpi/apriltag/AprilTagDetection.hpp @@ -0,0 +1,161 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include + +#include + +#include "wpi/util/SymbolExports.hpp" + +namespace wpi::apriltag { + +/** + * 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 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(c); } + + /** + * Gets a corner of the tag in image pixel coordinates. These always + * wrap counter-clock wise around the tag. Index 0 is the bottom left corner. + * + * @param ndx Corner index (range is 0-3, inclusive) + * @return Corner point + */ + const Point& GetCorner(int ndx) const { + return *reinterpret_cast(p[ndx]); + } + + /** + * Gets the corners of the tag in image pixel coordinates. These always + * wrap counter-clock wise around the tag. The first set of corner coordinates + * are the coordinates for the bottom left corner. + * + * @param cornersBuf Corner point array (X and Y for each corner in order) + * @return Corner point array (copy of cornersBuf span) + */ + std::span GetCorners(std::span 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 wpi::apriltag diff --git a/apriltag/src/main/native/include/wpi/apriltag/AprilTagDetector.hpp b/apriltag/src/main/native/include/wpi/apriltag/AprilTagDetector.hpp new file mode 100644 index 00000000000..16410c77ab8 --- /dev/null +++ b/apriltag/src/main/native/include/wpi/apriltag/AprilTagDetector.hpp @@ -0,0 +1,261 @@ +// 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 + +#include +#include +#include +#include + +#include "wpi/apriltag/AprilTagDetection.hpp" +#include "wpi/units/angle.hpp" +#include "wpi/util/StringMap.hpp" +#include "wpi/util/SymbolExports.hpp" + +namespace wpi::apriltag { + +/** + * 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 300 + * pixels. + */ + int minClusterPixels = 300; + + /** + * 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 45 degrees. + */ + wpi::units::radian_t criticalAngle = 45_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 { + struct private_init {}; + friend class AprilTagDetector; + + public: + Results() = default; + Results(void* impl, const private_init&); + ~Results() { Destroy(); } + Results(const Results&) = delete; + Results& operator=(const Results&) = delete; + Results(Results&& rhs) : span{std::move(rhs)}, m_impl{rhs.m_impl} { + rhs.m_impl = nullptr; + } + Results& operator=(Results&& rhs); + + private: + void Destroy(); + void* m_impl = nullptr; + }; + + AprilTagDetector(); + ~AprilTagDetector() { Destroy(); } + AprilTagDetector(const AprilTagDetector&) = delete; + AprilTagDetector& operator=(const AprilTagDetector&) = delete; + AprilTagDetector(AprilTagDetector&& rhs) + : m_impl{rhs.m_impl}, + m_families{std::move(rhs.m_families)}, + m_qtpCriticalAngle{rhs.m_qtpCriticalAngle} { + rhs.m_impl = nullptr; + } + AprilTagDetector& operator=(AprilTagDetector&& rhs); + + /** + * @{ + * @name Configuration functions + */ + + /** + * Sets detector configuration. + * + * @param config Configuration + */ + void SetConfig(const Config& config); + + /** + * Gets detector configuration. + * + * @return Configuration + */ + Config GetConfig() const; + + /** + * Sets quad threshold parameters. + * + * @param params Parameters + */ + void SetQuadThresholdParameters(const QuadThresholdParameters& params); + + /** + * Gets quad threshold parameters. + * + * @return Parameters + */ + QuadThresholdParameters GetQuadThresholdParameters() const; + + /** @} */ + + /** + * @{ + * @name Tag family functions + */ + + /** + * Adds a family of tags to be detected. + * + * @param fam Family name, e.g. "tag16h5" + * @param bitsCorrected Maximum number of bits to correct + * @return False if family can't be found + */ + bool AddFamily(std::string_view fam, int bitsCorrected = 2); + + /** + * Removes a family of tags from the detector. + * + * @param fam Family name, e.g. "tag16h5" + */ + void RemoveFamily(std::string_view fam); + + /** + * Unregister all families. + */ + void ClearFamilies(); + + /** @} */ + + /** + * Detect tags from an 8-bit image. + * The image must be grayscale. + * + * @param width width of the image + * @param height height of the image + * @param stride number of bytes between image rows (often the same as width) + * @param buf image buffer + * @return Results (array of AprilTagDetection pointers) + */ + Results Detect(int width, int height, int stride, uint8_t* buf); + + /** + * Detect tags from an 8-bit image. + * The image must be grayscale. + * + * @param width width of the image + * @param height height of the image + * @param buf image buffer + * @return Results (array of AprilTagDetection pointers) + */ + Results Detect(int width, int height, uint8_t* buf) { + return Detect(width, height, width, buf); + } + + private: + void Destroy(); + void DestroyFamilies(); + void DestroyFamily(std::string_view name, void* data); + + void* m_impl; + wpi::util::StringMap m_families; + wpi::units::radian_t m_qtpCriticalAngle = 10_deg; +}; + +} // namespace wpi::apriltag diff --git a/apriltag/src/main/native/include/wpi/apriltag/AprilTagDetector_cv.hpp b/apriltag/src/main/native/include/wpi/apriltag/AprilTagDetector_cv.hpp new file mode 100644 index 00000000000..696f27eba6c --- /dev/null +++ b/apriltag/src/main/native/include/wpi/apriltag/AprilTagDetector_cv.hpp @@ -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 + +#include "wpi/apriltag/AprilTagDetector.hpp" + +namespace wpi::apriltag { + +inline AprilTagDetector::Results AprilTagDetect(AprilTagDetector& detector, + cv::Mat& image) { + return detector.Detect(image.cols, image.rows, image.data); +} + +} // namespace wpi::apriltag diff --git a/apriltag/src/main/native/include/wpi/apriltag/AprilTagFieldLayout.hpp b/apriltag/src/main/native/include/wpi/apriltag/AprilTagFieldLayout.hpp new file mode 100644 index 00000000000..a761d3782b2 --- /dev/null +++ b/apriltag/src/main/native/include/wpi/apriltag/AprilTagFieldLayout.hpp @@ -0,0 +1,175 @@ +// 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 +#include +#include +#include + +#include "wpi/apriltag/AprilTag.hpp" +#include "wpi/apriltag/AprilTagFields.hpp" +#include "wpi/math/geometry/Pose3d.hpp" +#include "wpi/units/length.hpp" +#include "wpi/util/SymbolExports.hpp" +#include "wpi/util/json_fwd.hpp" + +namespace wpi::apriltag { +/** + * Class for representing a layout of AprilTags on a field and reading them from + * a JSON format. + * + * The JSON format contains two top-level objects, "tags" and "field". + * The "tags" object is a list of all AprilTags contained within a layout. Each + * AprilTag serializes to a JSON object containing an ID and a + * wpi::math::Pose3d. The "field" object is a descriptor of the size of the + * field in meters with "width" and "length" values. This is to account for + * arbitrary field sizes when transforming the poses. + * + * Pose3ds in the JSON are measured using the normal FRC coordinate system, NWU + * with the origin at the bottom-right corner of the blue alliance wall. + * SetOrigin(OriginPosition) can be used to change the poses returned from + * GetTagPose(int) to be from the perspective of a specific alliance. + * + * Tag poses represent the center of the tag, with a zero rotation representing + * a tag that is upright and facing away from the (blue) alliance wall (that is, + * towards the opposing alliance). */ +class WPILIB_DLLEXPORT AprilTagFieldLayout { + public: + /** + * Common origin positions for the AprilTag coordinate system. + */ + enum class OriginPosition { + /// Blue alliance wall, right side. + kBlueAllianceWallRightSide, + /// Red alliance wall, right side. + kRedAllianceWallRightSide, + }; + + /** + * Loads an AprilTagFieldLayout from a predefined field + * + * @param field The predefined field + * @return AprilTagFieldLayout of the field + */ + static AprilTagFieldLayout LoadField(AprilTagField field); + + AprilTagFieldLayout() = default; + + /** + * Construct a new AprilTagFieldLayout with values imported from a JSON file. + * + * @param path Path of the JSON file to import from. + */ + explicit AprilTagFieldLayout(std::string_view path); + + /** + * Construct a new AprilTagFieldLayout from a vector of AprilTag objects. + * + * @param apriltags Vector of AprilTags. + * @param fieldLength Length of field the layout is representing. + * @param fieldWidth Width of field the layout is representing. + */ + AprilTagFieldLayout(std::vector apriltags, + wpi::units::meter_t fieldLength, + wpi::units::meter_t fieldWidth); + + /** + * Returns the length of the field the layout is representing. + * @return length + */ + wpi::units::meter_t GetFieldLength() const; + + /** + * Returns the length of the field the layout is representing. + * @return width + */ + wpi::units::meter_t GetFieldWidth() const; + + /** + * Returns a vector of all the april tags used in this layout. + * @return list of tags + */ + std::vector GetTags() const; + + /** + * Sets the origin based on a predefined enumeration of coordinate frame + * origins. The origins are calculated from the field dimensions. + * + * This transforms the Pose3ds returned by GetTagPose(int) to return the + * correct pose relative to a predefined coordinate frame. + * + * @param origin The predefined origin + */ + void SetOrigin(OriginPosition origin); + + /** + * Sets the origin for tag pose transformation. + * + * This transforms the Pose3ds returned by GetTagPose(int) to return the + * correct pose relative to the provided origin. + * + * @param origin The new origin for tag transformations + */ + void SetOrigin(const wpi::math::Pose3d& origin); + + /** + * Returns the origin used for tag pose transformation. + * @return the origin + */ + wpi::math::Pose3d GetOrigin() const; + + /** + * Gets an AprilTag pose by its ID. + * + * @param ID The ID of the tag. + * @return The pose corresponding to the ID that was passed in or an empty + * optional if a tag with that ID is not found. + */ + std::optional GetTagPose(int ID) const; + + /** + * Serializes an AprilTagFieldLayout to a JSON file. + * + * @param path The path to write the JSON file to. + */ + void Serialize(std::string_view path); + + /* + * Checks equality between this AprilTagFieldLayout and another object. + */ + bool operator==(const AprilTagFieldLayout&) const = default; + + private: + std::unordered_map m_apriltags; + wpi::units::meter_t m_fieldLength; + wpi::units::meter_t m_fieldWidth; + wpi::math::Pose3d m_origin; + + friend WPILIB_DLLEXPORT void to_json(wpi::util::json& json, + const AprilTagFieldLayout& layout); + + friend WPILIB_DLLEXPORT void from_json(const wpi::util::json& json, + AprilTagFieldLayout& layout); +}; + +WPILIB_DLLEXPORT +void to_json(wpi::util::json& json, const AprilTagFieldLayout& layout); + +WPILIB_DLLEXPORT +void from_json(const wpi::util::json& json, AprilTagFieldLayout& layout); + +/** + * Loads an AprilTagFieldLayout from a predefined field + * + * @param field The predefined field + * @return AprilTagFieldLayout of the field + * @deprecated Use AprilTagFieldLayout::LoadField() instead + */ +[[deprecated("Use AprilTagFieldLayout::LoadField() instead")]] +WPILIB_DLLEXPORT AprilTagFieldLayout +LoadAprilTagLayoutField(AprilTagField field); + +} // namespace wpi::apriltag diff --git a/apriltag/src/main/native/include/wpi/apriltag/AprilTagFields.hpp b/apriltag/src/main/native/include/wpi/apriltag/AprilTagFields.hpp new file mode 100644 index 00000000000..4d73d41de08 --- /dev/null +++ b/apriltag/src/main/native/include/wpi/apriltag/AprilTagFields.hpp @@ -0,0 +1,35 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "wpi/util/SymbolExports.hpp" + +namespace wpi::apriltag { + +/** + * Loadable AprilTag field layouts. + */ +enum class AprilTagField { + /// 2022 Rapid React. + k2022RapidReact, + /// 2023 Charged Up. + k2023ChargedUp, + /// 2024 Crescendo. + k2024Crescendo, + /// 2025 Reefscape AndyMark (see TU12). + k2025ReefscapeAndyMark, + /// 2025 Reefscape Welded (see TU12). + k2025ReefscapeWelded, + /// Alias to the current game. + kDefaultField = k2025ReefscapeWelded, + + // This is a placeholder for denoting the last supported field. This should + // always be the last entry in the enum and should not be used by users + kNumFields, +}; + +} // namespace wpi::apriltag diff --git a/apriltag/src/main/native/include/wpi/apriltag/AprilTagPoseEstimate.hpp b/apriltag/src/main/native/include/wpi/apriltag/AprilTagPoseEstimate.hpp new file mode 100644 index 00000000000..0d4037071e1 --- /dev/null +++ b/apriltag/src/main/native/include/wpi/apriltag/AprilTagPoseEstimate.hpp @@ -0,0 +1,35 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include "wpi/math/geometry/Transform3d.hpp" +#include "wpi/util/SymbolExports.hpp" + +namespace wpi::apriltag { + +/** A pair of AprilTag pose estimates. */ +struct WPILIB_DLLEXPORT AprilTagPoseEstimate { + /** Pose 1. */ + wpi::math::Transform3d pose1; + + /** Pose 2. */ + wpi::math::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 wpi::apriltag diff --git a/apriltag/src/main/native/include/wpi/apriltag/AprilTagPoseEstimator.hpp b/apriltag/src/main/native/include/wpi/apriltag/AprilTagPoseEstimator.hpp new file mode 100644 index 00000000000..7f4063e9321 --- /dev/null +++ b/apriltag/src/main/native/include/wpi/apriltag/AprilTagPoseEstimator.hpp @@ -0,0 +1,146 @@ +// 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 + +#include "wpi/apriltag/AprilTagPoseEstimate.hpp" +#include "wpi/math/geometry/Transform3d.hpp" +#include "wpi/units/length.hpp" +#include "wpi/util/SymbolExports.hpp" + +namespace wpi::apriltag { + +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. */ + wpi::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 + */ + wpi::math::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 + */ + wpi::math::Transform3d EstimateHomography( + std::span 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 homography, std::span 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 + */ + wpi::math::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 + */ + wpi::math::Transform3d Estimate(std::span homography, + std::span corners) const; + + private: + Config m_config; +}; + +} // namespace wpi::apriltag diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2022-rapidreact.json b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2022-rapidreact.json similarity index 100% rename from apriltag/src/main/native/resources/edu/wpi/first/apriltag/2022-rapidreact.json rename to apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2022-rapidreact.json diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2023-chargedup.json b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2023-chargedup.json similarity index 100% rename from apriltag/src/main/native/resources/edu/wpi/first/apriltag/2023-chargedup.json rename to apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2023-chargedup.json diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.json b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2024-crescendo.json similarity index 100% rename from apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.json rename to apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2024-crescendo.json diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-andymark.csv b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2025-reefscape-andymark.csv similarity index 100% rename from apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-andymark.csv rename to apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2025-reefscape-andymark.csv diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-andymark.json b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2025-reefscape-andymark.json similarity index 100% rename from apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-andymark.json rename to apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2025-reefscape-andymark.json diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-welded.csv b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2025-reefscape-welded.csv similarity index 100% rename from apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-welded.csv rename to apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2025-reefscape-welded.csv diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-welded.json b/apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2025-reefscape-welded.json similarity index 100% rename from apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape-welded.json rename to apriltag/src/main/native/resources/org/wpilib/vision/apriltag/2025-reefscape-welded.json diff --git a/apriltag/src/main/python/pyproject.toml b/apriltag/src/main/python/pyproject.toml index 5eed3fe5866..83e4728f68f 100644 --- a/apriltag/src/main/python/pyproject.toml +++ b/apriltag/src/main/python/pyproject.toml @@ -51,7 +51,7 @@ scan_headers_ignore = [ "apriltag_math.h", "apriltag_pose.h", - "frc/apriltag/AprilTagDetector_cv.h", + "wpi/apriltag/AprilTagDetector_cv.hpp", "tag16h5.h", "tag36h11.h", @@ -64,11 +64,11 @@ depends = ["wpiutil", "wpimath"] [tool.semiwrap.extension_modules."robotpy_apriltag._apriltag".headers] # frc/apriltag -AprilTag = "frc/apriltag/AprilTag.h" -AprilTagDetection = "frc/apriltag/AprilTagDetection.h" -AprilTagDetector = "frc/apriltag/AprilTagDetector.h" -# AprilTagDetector_cv = "frc/apriltag/AprilTagDetector_cv.h" -AprilTagFieldLayout = "frc/apriltag/AprilTagFieldLayout.h" -AprilTagFields = "frc/apriltag/AprilTagFields.h" -AprilTagPoseEstimate = "frc/apriltag/AprilTagPoseEstimate.h" -AprilTagPoseEstimator = "frc/apriltag/AprilTagPoseEstimator.h" +AprilTag = "wpi/apriltag/AprilTag.hpp" +AprilTagDetection = "wpi/apriltag/AprilTagDetection.hpp" +AprilTagDetector = "wpi/apriltag/AprilTagDetector.hpp" +# AprilTagDetector_cv = "wpi/apriltag/AprilTagDetector_cv.hpp" +AprilTagFieldLayout = "wpi/apriltag/AprilTagFieldLayout.hpp" +AprilTagFields = "wpi/apriltag/AprilTagFields.hpp" +AprilTagPoseEstimate = "wpi/apriltag/AprilTagPoseEstimate.hpp" +AprilTagPoseEstimator = "wpi/apriltag/AprilTagPoseEstimator.hpp" diff --git a/apriltag/src/main/python/semiwrap/AprilTag.yml b/apriltag/src/main/python/semiwrap/AprilTag.yml index 42ae4168c8b..5e8b2ebd8be 100644 --- a/apriltag/src/main/python/semiwrap/AprilTag.yml +++ b/apriltag/src/main/python/semiwrap/AprilTag.yml @@ -4,7 +4,7 @@ functions: from_json: ignore: true classes: - frc::AprilTag: + wpi::apriltag::AprilTag: attributes: ID: pose: diff --git a/apriltag/src/main/python/semiwrap/AprilTagDetection.yml b/apriltag/src/main/python/semiwrap/AprilTagDetection.yml index 23a35df5d2e..78332dad947 100644 --- a/apriltag/src/main/python/semiwrap/AprilTagDetection.yml +++ b/apriltag/src/main/python/semiwrap/AprilTagDetection.yml @@ -2,7 +2,7 @@ extra_includes: - pybind11/eigen.h classes: - frc::AprilTagDetection: + wpi::apriltag::AprilTagDetection: methods: GetFamily: GetId: @@ -18,7 +18,7 @@ classes: return py::str("") .format(self.GetFamily(), self.GetId(), self.GetHamming(), self.GetDecisionMargin(), self.GetCenter()); }) - frc::AprilTagDetection::Point: + wpi::apriltag::AprilTagDetection::Point: attributes: x: y: diff --git a/apriltag/src/main/python/semiwrap/AprilTagDetector.yml b/apriltag/src/main/python/semiwrap/AprilTagDetector.yml index 3385e5172ea..df9e1d970d8 100644 --- a/apriltag/src/main/python/semiwrap/AprilTagDetector.yml +++ b/apriltag/src/main/python/semiwrap/AprilTagDetector.yml @@ -2,7 +2,7 @@ extra_includes: - pybind11_typing.h classes: - frc::AprilTagDetector: + wpi::apriltag::AprilTagDetector: methods: AprilTagDetector: SetConfig: @@ -59,7 +59,7 @@ classes: :return: list of results )doc" ) - frc::AprilTagDetector::Config: + wpi::apriltag::AprilTagDetector::Config: attributes: numThreads: quadDecimate: @@ -69,7 +69,7 @@ classes: debug: methods: operator==: - frc::AprilTagDetector::QuadThresholdParameters: + wpi::apriltag::AprilTagDetector::QuadThresholdParameters: attributes: minClusterPixels: maxNumMaxima: @@ -79,7 +79,7 @@ classes: deglitch: methods: operator==: - frc::AprilTagDetector::Results: + wpi::apriltag::AprilTagDetector::Results: rename: _Results ignored_bases: - std::span diff --git a/apriltag/src/main/python/semiwrap/AprilTagFieldLayout.yml b/apriltag/src/main/python/semiwrap/AprilTagFieldLayout.yml index eba162f44cb..ac032718d92 100644 --- a/apriltag/src/main/python/semiwrap/AprilTagFieldLayout.yml +++ b/apriltag/src/main/python/semiwrap/AprilTagFieldLayout.yml @@ -6,7 +6,7 @@ functions: LoadAprilTagLayoutField: ignore: true classes: - frc::AprilTagFieldLayout: + wpi::apriltag::AprilTagFieldLayout: enums: OriginPosition: methods: @@ -14,7 +14,7 @@ classes: overloads: '': std::string_view: - std::vector, units::meter_t, units::meter_t: + std::vector, wpi::units::meter_t, wpi::units::meter_t: LoadField: GetFieldLength: GetFieldWidth: @@ -22,7 +22,7 @@ classes: SetOrigin: overloads: OriginPosition: - const Pose3d&: + const wpi::math::Pose3d&: GetOrigin: GetTagPose: Serialize: diff --git a/apriltag/src/main/python/semiwrap/AprilTagPoseEstimate.yml b/apriltag/src/main/python/semiwrap/AprilTagPoseEstimate.yml index 37faf79df41..2e31bb8f46e 100644 --- a/apriltag/src/main/python/semiwrap/AprilTagPoseEstimate.yml +++ b/apriltag/src/main/python/semiwrap/AprilTagPoseEstimate.yml @@ -1,5 +1,5 @@ classes: - frc::AprilTagPoseEstimate: + wpi::apriltag::AprilTagPoseEstimate: attributes: pose1: pose2: diff --git a/apriltag/src/main/python/semiwrap/AprilTagPoseEstimator.yml b/apriltag/src/main/python/semiwrap/AprilTagPoseEstimator.yml index d5f39edf3bc..d2cc7e8ea32 100644 --- a/apriltag/src/main/python/semiwrap/AprilTagPoseEstimator.yml +++ b/apriltag/src/main/python/semiwrap/AprilTagPoseEstimator.yml @@ -1,8 +1,8 @@ extra_includes: -- frc/apriltag/AprilTagDetection.h +- wpi/apriltag/AprilTagDetection.hpp classes: - frc::AprilTagPoseEstimator: + wpi::apriltag::AprilTagPoseEstimator: methods: AprilTagPoseEstimator: SetConfig: @@ -19,7 +19,7 @@ classes: overloads: const AprilTagDetection& [const]: std::span, std::span [const]: - frc::AprilTagPoseEstimator::Config: + wpi::apriltag::AprilTagPoseEstimator::Config: force_no_default_constructor: true attributes: tagSize: @@ -30,7 +30,7 @@ classes: methods: operator==: inline_code: | - .def(py::init([](units::meter_t tagSize, double fx, double fy, double cx, double cy) { + .def(py::init([](wpi::units::meter_t tagSize, double fx, double fy, double cx, double cy) { AprilTagPoseEstimator::Config cfg{tagSize, fx, fy, cx, cy}; return std::make_unique(std::move(cfg)); }), py::arg("tagSize"), py::arg("fx"), py::arg("fy"), py::arg("cx"), py::arg("cy")) diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java deleted file mode 100644 index 66923f4401c..00000000000 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java +++ /dev/null @@ -1,71 +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; - -import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertFalse; -import static org.junit.jupiter.api.Assertions.assertNotNull; -import static org.junit.jupiter.api.Assertions.assertTrue; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.util.Units; -import java.util.Optional; -import org.junit.jupiter.api.Assertions; -import org.junit.jupiter.api.Test; -import org.junit.jupiter.params.ParameterizedTest; -import org.junit.jupiter.params.provider.EnumSource; - -class LoadConfigTest { - @ParameterizedTest - @EnumSource(AprilTagFields.class) - void testLoad(AprilTagFields field) { - AprilTagFieldLayout layout = - Assertions.assertDoesNotThrow(() -> AprilTagFieldLayout.loadField(field)); - assertNotNull(layout); - } - - @Test - void test2022RapidReact() { - AprilTagFieldLayout layout = AprilTagFieldLayout.loadField(AprilTagFields.k2022RapidReact); - - // Blue Hangar Truss - Hub - Pose3d expectedPose = - new Pose3d( - Units.inchesToMeters(127.272), - Units.inchesToMeters(216.01), - Units.inchesToMeters(67.932), - Rotation3d.kZero); - Optional maybePose = layout.getTagPose(1); - assertTrue(maybePose.isPresent()); - assertEquals(expectedPose, maybePose.get()); - - // Blue Terminal Near Station - expectedPose = - new Pose3d( - Units.inchesToMeters(4.768), - Units.inchesToMeters(67.631), - Units.inchesToMeters(35.063), - new Rotation3d(0, 0, Math.toRadians(46.25))); - maybePose = layout.getTagPose(5); - assertTrue(maybePose.isPresent()); - assertEquals(expectedPose, maybePose.get()); - - // Upper Hub Blue-Near - expectedPose = - new Pose3d( - Units.inchesToMeters(332.321), - Units.inchesToMeters(183.676), - Units.inchesToMeters(95.186), - new Rotation3d(0, Math.toRadians(26.75), Math.toRadians(69))); - maybePose = layout.getTagPose(53); - assertTrue(maybePose.isPresent()); - assertEquals(expectedPose, maybePose.get()); - - // Doesn't exist - maybePose = layout.getTagPose(54); - assertFalse(maybePose.isPresent()); - } -} diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java b/apriltag/src/test/java/org/wpilib/vision/apriltag/AprilTagDetectorTest.java similarity index 98% rename from apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java rename to apriltag/src/test/java/org/wpilib/vision/apriltag/AprilTagDetectorTest.java index 4a83016b8c9..358bc487533 100644 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java +++ b/apriltag/src/test/java/org/wpilib/vision/apriltag/AprilTagDetectorTest.java @@ -2,15 +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. -package edu.wpi.first.apriltag; +package org.wpilib.vision.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; @@ -23,6 +20,9 @@ import org.opencv.core.Mat; import org.opencv.imgcodecs.Imgcodecs; import org.opencv.imgproc.Imgproc; +import org.wpilib.math.geometry.Transform3d; +import org.wpilib.math.util.Units; +import org.wpilib.util.runtime.RuntimeLoader; class AprilTagDetectorTest { @SuppressWarnings("MemberName") diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java b/apriltag/src/test/java/org/wpilib/vision/apriltag/AprilTagGenerationTest.java similarity index 94% rename from apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java rename to apriltag/src/test/java/org/wpilib/vision/apriltag/AprilTagGenerationTest.java index 3f3a9e65d98..328cae9dc6b 100644 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagGenerationTest.java +++ b/apriltag/src/test/java/org/wpilib/vision/apriltag/AprilTagGenerationTest.java @@ -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. -package edu.wpi.first.apriltag; +package org.wpilib.vision.apriltag; import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.util.PixelFormat; import org.junit.jupiter.api.Test; +import org.wpilib.util.PixelFormat; class AprilTagGenerationTest { @Test diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagPoseSetOriginTest.java b/apriltag/src/test/java/org/wpilib/vision/apriltag/AprilTagPoseSetOriginTest.java similarity index 87% rename from apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagPoseSetOriginTest.java rename to apriltag/src/test/java/org/wpilib/vision/apriltag/AprilTagPoseSetOriginTest.java index 8f373b363d2..68df151c37e 100644 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagPoseSetOriginTest.java +++ b/apriltag/src/test/java/org/wpilib/vision/apriltag/AprilTagPoseSetOriginTest.java @@ -2,16 +2,16 @@ // 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; +package org.wpilib.vision.apriltag; import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; import java.util.List; import org.junit.jupiter.api.Test; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.geometry.Translation3d; +import org.wpilib.math.util.Units; class AprilTagPoseSetOriginTest { @Test diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagSerializationTest.java b/apriltag/src/test/java/org/wpilib/vision/apriltag/AprilTagSerializationTest.java similarity index 87% rename from apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagSerializationTest.java rename to apriltag/src/test/java/org/wpilib/vision/apriltag/AprilTagSerializationTest.java index df734f3766e..8773f90bb22 100644 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagSerializationTest.java +++ b/apriltag/src/test/java/org/wpilib/vision/apriltag/AprilTagSerializationTest.java @@ -2,17 +2,17 @@ // 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; +package org.wpilib.vision.apriltag; import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.util.Units; import java.util.List; import org.junit.jupiter.api.Test; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.util.Units; class AprilTagSerializationTest { @Test diff --git a/apriltag/src/test/java/org/wpilib/vision/apriltag/LoadConfigTest.java b/apriltag/src/test/java/org/wpilib/vision/apriltag/LoadConfigTest.java new file mode 100644 index 00000000000..34f7b639c45 --- /dev/null +++ b/apriltag/src/test/java/org/wpilib/vision/apriltag/LoadConfigTest.java @@ -0,0 +1,71 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.wpilib.vision.apriltag; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.Optional; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.EnumSource; +import org.wpilib.math.geometry.Pose3d; +import org.wpilib.math.geometry.Rotation3d; +import org.wpilib.math.util.Units; + +class LoadConfigTest { + @ParameterizedTest + @EnumSource(AprilTagFields.class) + void testLoad(AprilTagFields field) { + AprilTagFieldLayout layout = + Assertions.assertDoesNotThrow(() -> AprilTagFieldLayout.loadField(field)); + assertNotNull(layout); + } + + @Test + void test2022RapidReact() { + AprilTagFieldLayout layout = AprilTagFieldLayout.loadField(AprilTagFields.k2022RapidReact); + + // Blue Hangar Truss - Hub + Pose3d expectedPose = + new Pose3d( + Units.inchesToMeters(127.272), + Units.inchesToMeters(216.01), + Units.inchesToMeters(67.932), + Rotation3d.kZero); + Optional maybePose = layout.getTagPose(1); + assertTrue(maybePose.isPresent()); + assertEquals(expectedPose, maybePose.get()); + + // Blue Terminal Near Station + expectedPose = + new Pose3d( + Units.inchesToMeters(4.768), + Units.inchesToMeters(67.631), + Units.inchesToMeters(35.063), + new Rotation3d(0, 0, Math.toRadians(46.25))); + maybePose = layout.getTagPose(5); + assertTrue(maybePose.isPresent()); + assertEquals(expectedPose, maybePose.get()); + + // Upper Hub Blue-Near + expectedPose = + new Pose3d( + Units.inchesToMeters(332.321), + Units.inchesToMeters(183.676), + Units.inchesToMeters(95.186), + new Rotation3d(0, Math.toRadians(26.75), Math.toRadians(69))); + maybePose = layout.getTagPose(53); + assertTrue(maybePose.isPresent()); + assertEquals(expectedPose, maybePose.get()); + + // Doesn't exist + maybePose = layout.getTagPose(54); + assertFalse(maybePose.isPresent()); + } +} diff --git a/apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp b/apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp index af7fc6ee527..365e72dc573 100644 --- a/apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp +++ b/apriltag/src/test/native/cpp/AprilTagDetectorTest.cpp @@ -4,9 +4,9 @@ #include -#include "frc/apriltag/AprilTagDetector.h" +#include "wpi/apriltag/AprilTagDetector.hpp" -using namespace frc; +using namespace wpi::apriltag; TEST(AprilTagDetectorTest, ConfigDefaults) { AprilTagDetector detector; diff --git a/apriltag/src/test/native/cpp/AprilTagJsonTest.cpp b/apriltag/src/test/native/cpp/AprilTagJsonTest.cpp index 4216479813d..9f787b605bc 100644 --- a/apriltag/src/test/native/cpp/AprilTagJsonTest.cpp +++ b/apriltag/src/test/native/cpp/AprilTagJsonTest.cpp @@ -5,23 +5,24 @@ #include #include -#include -#include "frc/apriltag/AprilTag.h" -#include "frc/apriltag/AprilTagFieldLayout.h" -#include "frc/geometry/Pose3d.h" +#include "wpi/apriltag/AprilTag.hpp" +#include "wpi/apriltag/AprilTagFieldLayout.hpp" +#include "wpi/math/geometry/Pose3d.hpp" +#include "wpi/util/json.hpp" -using namespace frc; +using namespace wpi::apriltag; TEST(AprilTagJsonTest, DeserializeMatches) { auto layout = AprilTagFieldLayout{ - std::vector{ - AprilTag{1, Pose3d{}}, - AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}}, + std::vector{AprilTag{1, wpi::math::Pose3d{}}, + AprilTag{3, wpi::math::Pose3d{0_m, 1_m, 0_m, + wpi::math::Rotation3d{ + 0_deg, 0_deg, 0_deg}}}}, 54_ft, 27_ft}; AprilTagFieldLayout deserialized; - wpi::json json = layout; + wpi::util::json json = layout; EXPECT_NO_THROW(deserialized = json.get()); EXPECT_EQ(layout, deserialized); } diff --git a/apriltag/src/test/native/cpp/AprilTagPoseSetOriginTest.cpp b/apriltag/src/test/native/cpp/AprilTagPoseSetOriginTest.cpp index 58fd873df39..c61cc1588da 100644 --- a/apriltag/src/test/native/cpp/AprilTagPoseSetOriginTest.cpp +++ b/apriltag/src/test/native/cpp/AprilTagPoseSetOriginTest.cpp @@ -5,29 +5,32 @@ #include #include -#include -#include "frc/apriltag/AprilTag.h" -#include "frc/apriltag/AprilTagFieldLayout.h" -#include "frc/geometry/Pose3d.h" +#include "wpi/apriltag/AprilTag.hpp" +#include "wpi/apriltag/AprilTagFieldLayout.hpp" +#include "wpi/math/geometry/Pose3d.hpp" +#include "wpi/util/json.hpp" -using namespace frc; +using namespace wpi::apriltag; TEST(AprilTagPoseSetOriginTest, TransformationMatches) { auto layout = AprilTagFieldLayout{ std::vector{ - AprilTag{1, - Pose3d{0_ft, 0_ft, 0_ft, Rotation3d{0_deg, 0_deg, 0_deg}}}, AprilTag{ - 2, Pose3d{4_ft, 4_ft, 4_ft, Rotation3d{0_deg, 0_deg, 180_deg}}}}, + 1, wpi::math::Pose3d{0_ft, 0_ft, 0_ft, + wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}}, + AprilTag{2, wpi::math::Pose3d{4_ft, 4_ft, 4_ft, + wpi::math::Rotation3d{0_deg, 0_deg, + 180_deg}}}}, 54_ft, 27_ft}; layout.SetOrigin( AprilTagFieldLayout::OriginPosition::kRedAllianceWallRightSide); - auto mirrorPose = - Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}}; + auto mirrorPose = wpi::math::Pose3d{ + 54_ft, 27_ft, 0_ft, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}}; EXPECT_EQ(mirrorPose, *layout.GetTagPose(1)); - mirrorPose = Pose3d{50_ft, 23_ft, 4_ft, Rotation3d{0_deg, 0_deg, 0_deg}}; + mirrorPose = wpi::math::Pose3d{50_ft, 23_ft, 4_ft, + wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}; EXPECT_EQ(mirrorPose, *layout.GetTagPose(2)); } diff --git a/apriltag/src/test/native/cpp/LoadConfigTest.cpp b/apriltag/src/test/native/cpp/LoadConfigTest.cpp index 9cf5b6fa105..c9243f60c4f 100644 --- a/apriltag/src/test/native/cpp/LoadConfigTest.cpp +++ b/apriltag/src/test/native/cpp/LoadConfigTest.cpp @@ -6,10 +6,10 @@ #include -#include "frc/apriltag/AprilTagFieldLayout.h" -#include "frc/apriltag/AprilTagFields.h" +#include "wpi/apriltag/AprilTagFieldLayout.hpp" +#include "wpi/apriltag/AprilTagFields.hpp" -namespace frc { +namespace wpi::apriltag { std::vector GetAllFields() { std::vector output; @@ -27,21 +27,24 @@ TEST(AprilTagFieldsTest, TestLoad2022RapidReact) { // Blue Hangar Truss - Hub auto expectedPose = - Pose3d{127.272_in, 216.01_in, 67.932_in, Rotation3d{0_deg, 0_deg, 0_deg}}; + wpi::math::Pose3d{127.272_in, 216.01_in, 67.932_in, + wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}; auto maybePose = layout.GetTagPose(1); EXPECT_TRUE(maybePose); EXPECT_EQ(expectedPose, *maybePose); // Blue Terminal Near Station - expectedPose = Pose3d{4.768_in, 67.631_in, 35.063_in, - Rotation3d{0_deg, 0_deg, 46.25_deg}}; + expectedPose = + wpi::math::Pose3d{4.768_in, 67.631_in, 35.063_in, + wpi::math::Rotation3d{0_deg, 0_deg, 46.25_deg}}; maybePose = layout.GetTagPose(5); EXPECT_TRUE(maybePose); EXPECT_EQ(expectedPose, *maybePose); // Upper Hub Blue-Near - expectedPose = Pose3d{332.321_in, 183.676_in, 95.186_in, - Rotation3d{0_deg, 26.75_deg, 69_deg}}; + expectedPose = + wpi::math::Pose3d{332.321_in, 183.676_in, 95.186_in, + wpi::math::Rotation3d{0_deg, 26.75_deg, 69_deg}}; maybePose = layout.GetTagPose(53); EXPECT_TRUE(maybePose); EXPECT_EQ(expectedPose, *maybePose); @@ -62,4 +65,4 @@ TEST_P(AllFieldsFixtureTest, CheckEntireEnum) { INSTANTIATE_TEST_SUITE_P(ValuesEnumTestInstTests, AllFieldsFixtureTest, ::testing::ValuesIn(GetAllFields())); -} // namespace frc +} // namespace wpi::apriltag diff --git a/apriltag/src/test/resources/edu/wpi/first/apriltag/tag1_640_480.jpg b/apriltag/src/test/resources/org/wpilib/vision/apriltag/tag1_640_480.jpg similarity index 100% rename from apriltag/src/test/resources/edu/wpi/first/apriltag/tag1_640_480.jpg rename to apriltag/src/test/resources/org/wpilib/vision/apriltag/tag1_640_480.jpg diff --git a/apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_16h5_straight.png b/apriltag/src/test/resources/org/wpilib/vision/apriltag/tag2_16h5_straight.png similarity index 100% rename from apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_16h5_straight.png rename to apriltag/src/test/resources/org/wpilib/vision/apriltag/tag2_16h5_straight.png diff --git a/apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_45deg_X.png b/apriltag/src/test/resources/org/wpilib/vision/apriltag/tag2_45deg_X.png similarity index 100% rename from apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_45deg_X.png rename to apriltag/src/test/resources/org/wpilib/vision/apriltag/tag2_45deg_X.png diff --git a/apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_45deg_y.png b/apriltag/src/test/resources/org/wpilib/vision/apriltag/tag2_45deg_y.png similarity index 100% rename from apriltag/src/test/resources/edu/wpi/first/apriltag/tag2_45deg_y.png rename to apriltag/src/test/resources/org/wpilib/vision/apriltag/tag2_45deg_y.png diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt index 1542955155c..dfadd80f4e2 100644 --- a/benchmark/CMakeLists.txt +++ b/benchmark/CMakeLists.txt @@ -15,7 +15,7 @@ target_link_libraries( PUBLIC $ $ - $ + $ $ $ ) diff --git a/benchmark/build.gradle b/benchmark/build.gradle index 7bc565d35c0..3540da64caf 100644 --- a/benchmark/build.gradle +++ b/benchmark/build.gradle @@ -51,7 +51,7 @@ dependencies { implementation project(':hal') implementation project(':ntcore') implementation project(':wpilibj') - implementation project(':wpilibNewCommands') + implementation project(':commandsv2') implementation project(':wpimath') implementation project(':wpinet') implementation project(':wpiunits') @@ -221,7 +221,7 @@ model { project(':ntcore').addNtcoreDependency(binary, 'shared') project(':ntcore').addNtcoreJniDependency(binary) lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared' - lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared' + lib project: ':commandsv2', library: 'commandsv2', linkage: 'shared' lib project: ':wpimath', library: 'wpimath', linkage: 'shared' lib project: ':wpimath', library: 'wpimathJNIShared', linkage: 'shared' lib project: ':wpinet', library: 'wpinet', linkage: 'shared' @@ -276,7 +276,7 @@ model { project(':hal').addHalDependency(binary, 'static') project(':ntcore').addNtcoreDependency(binary, 'static') lib project: ':wpilibc', library: 'wpilibc', linkage: 'static' - lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'static' + lib project: ':commandsv2', library: 'commandsv2', linkage: 'static' lib project: ':wpimath', library: 'wpimath', linkage: 'static' lib project: ':wpinet', library: 'wpinet', linkage: 'static' lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' diff --git a/benchmark/src/main/java/frc/robot/Main.java b/benchmark/src/main/java/frc/robot/Main.java index 862a2f58b7d..0a559a6a2e3 100644 --- a/benchmark/src/main/java/frc/robot/Main.java +++ b/benchmark/src/main/java/frc/robot/Main.java @@ -4,9 +4,6 @@ package frc.robot; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.path.TravelingSalesman; import java.util.concurrent.TimeUnit; import org.openjdk.jmh.annotations.Benchmark; import org.openjdk.jmh.annotations.BenchmarkMode; @@ -18,6 +15,9 @@ import org.openjdk.jmh.runner.options.Options; import org.openjdk.jmh.runner.options.OptionsBuilder; import org.openjdk.jmh.runner.options.TimeValue; +import org.wpilib.math.geometry.Pose2d; +import org.wpilib.math.geometry.Rotation2d; +import org.wpilib.math.path.TravelingSalesman; public class Main { private static final Pose2d[] poses = { diff --git a/benchmark/src/main/native/cpp/Main.cpp b/benchmark/src/main/native/cpp/Main.cpp index eb97dd941e6..406245c12e4 100644 --- a/benchmark/src/main/native/cpp/Main.cpp +++ b/benchmark/src/main/native/cpp/Main.cpp @@ -3,24 +3,24 @@ // the WPILib BSD license file in the root directory of this project. #include -#include -#include -#include -#include -#include +#include "wpi/math/geometry/Pose2d.hpp" +#include "wpi/math/path/TravelingSalesman.hpp" +#include "wpi/units/angle.hpp" +#include "wpi/units/length.hpp" +#include "wpi/util/array.hpp" -static constexpr wpi::array poses{ - frc::Pose2d{-1_m, 1_m, -90_deg}, frc::Pose2d{-1_m, 2_m, 90_deg}, - frc::Pose2d{0_m, 0_m, 0_deg}, frc::Pose2d{0_m, 3_m, -90_deg}, - frc::Pose2d{1_m, 1_m, 90_deg}, frc::Pose2d{1_m, 2_m, 90_deg}, +static constexpr wpi::util::array poses{ + wpi::math::Pose2d{-1_m, 1_m, -90_deg}, wpi::math::Pose2d{-1_m, 2_m, 90_deg}, + wpi::math::Pose2d{0_m, 0_m, 0_deg}, wpi::math::Pose2d{0_m, 3_m, -90_deg}, + wpi::math::Pose2d{1_m, 1_m, 90_deg}, wpi::math::Pose2d{1_m, 2_m, 90_deg}, }; static constexpr int iterations = 100; void BM_Transform(benchmark::State& state) { - frc::TravelingSalesman traveler{[](auto pose1, auto pose2) { + wpi::math::TravelingSalesman traveler{[](auto pose1, auto pose2) { auto transform = pose2 - pose1; - return units::math::hypot(transform.X(), transform.Y()).value(); + return wpi::units::math::hypot(transform.X(), transform.Y()).value(); }}; // NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores) for (auto _ : state) { @@ -30,9 +30,9 @@ void BM_Transform(benchmark::State& state) { BENCHMARK(BM_Transform); void BM_Twist(benchmark::State& state) { - frc::TravelingSalesman traveler{[](auto pose1, auto pose2) { + wpi::math::TravelingSalesman traveler{[](auto pose1, auto pose2) { auto twist = (pose2 - pose1).Log(); - return units::math::hypot(twist.dx, twist.dy).value(); + return wpi::units::math::hypot(twist.dx, twist.dy).value(); }}; // NOLINTNEXTLINE(clang-analyzer-deadcode.DeadStores) for (auto _ : state) { diff --git a/benchmark/src/main/native/thirdparty/benchmark/src/sysinfo.cpp b/benchmark/src/main/native/thirdparty/benchmark/src/sysinfo.cpp index 86922c0da63..93ad20b2f9e 100644 --- a/benchmark/src/main/native/thirdparty/benchmark/src/sysinfo.cpp +++ b/benchmark/src/main/native/thirdparty/benchmark/src/sysinfo.cpp @@ -441,7 +441,7 @@ std::vector GetCacheSizes() { return GetCacheSizesQNX(); #elif defined(BENCHMARK_OS_QURT) || defined(__EMSCRIPTEN__) return std::vector(); -#elif defined(__FRC_ROBORIO__) +#elif defined(__WPILIB_ROBORIO__) return std::vector(); #else return GetCacheSizesFromKVFS(); diff --git a/cameraserver/.styleguide b/cameraserver/.styleguide index 951f083e202..db5b6737b81 100644 --- a/cameraserver/.styleguide +++ b/cameraserver/.styleguide @@ -1,5 +1,6 @@ cppHeaderFileInclude { \.h$ + \.hpp$ \.inc$ } @@ -14,11 +15,8 @@ repoRootNameOverride { includeOtherLibs { ^fmt/ ^gtest/ - ^hal/ - ^networktables/ ^opencv2/ ^support/ - ^wpi/ } includeGuardRoots { diff --git a/cameraserver/BUILD.bazel b/cameraserver/BUILD.bazel index 1e3fe5ffbbf..50bedb393e9 100644 --- a/cameraserver/BUILD.bazel +++ b/cameraserver/BUILD.bazel @@ -54,7 +54,7 @@ wpilib_java_library( name = "cameraserver-java", srcs = glob(["src/main/java/**/*.java"]), maven_artifact_name = "cameraserver-java", - maven_group_id = "edu.wpi.first.cameraserver", + maven_group_id = "org.wpilib.cameraserver", visibility = ["//visibility:public"], deps = [ "//cscore:cscore-java", @@ -87,8 +87,8 @@ cc_binary( java_binary( name = "DevMain-Java", - srcs = ["src/dev/java/edu/wpi/first/cameraserver/DevMain.java"], - main_class = "edu.wpi.first.cameraserver.DevMain", + srcs = ["src/dev/java/org/wpilib/vision/stream/DevMain.java"], + main_class = "org.wpilib.vision.stream.DevMain", deps = [ ], ) @@ -96,5 +96,5 @@ java_binary( package_minimal_cc_project( name = "cameraserver", maven_artifact_name = "cameraserver-cpp", - maven_group_id = "edu.wpi.first.cameraserver", + maven_group_id = "org.wpilib.cameraserver", ) diff --git a/cameraserver/build.gradle b/cameraserver/build.gradle index 92925c65646..65439718444 100644 --- a/cameraserver/build.gradle +++ b/cameraserver/build.gradle @@ -1,6 +1,6 @@ ext { nativeName = 'cameraserver' - devMain = 'edu.wpi.first.cameraserver.DevMain' + devMain = 'org.wpilib.cameraserver.DevMain' } evaluationDependsOn(':ntcore') diff --git a/cameraserver/multiCameraServer/BUILD.bazel b/cameraserver/multiCameraServer/BUILD.bazel index 9716e1bb447..bcc2105da50 100644 --- a/cameraserver/multiCameraServer/BUILD.bazel +++ b/cameraserver/multiCameraServer/BUILD.bazel @@ -2,8 +2,8 @@ load("@rules_java//java:defs.bzl", "java_binary") java_binary( name = "multiCameraServer-java", - srcs = ["src/main/java/edu/wpi/Main.java"], - main_class = "edu.wpi.Main", + srcs = ["src/main/java/org/wpilib/Main.java"], + main_class = "org.wpilib.Main", deps = [ "//cameraserver:cameraserver-java", "//cscore:cscore-java", diff --git a/cameraserver/multiCameraServer/build.gradle b/cameraserver/multiCameraServer/build.gradle index 4173396371f..3412f0e964d 100644 --- a/cameraserver/multiCameraServer/build.gradle +++ b/cameraserver/multiCameraServer/build.gradle @@ -19,7 +19,7 @@ ext { apply from: "${rootDir}/shared/opencv.gradle" application { - mainClass = 'edu.wpi.Main' + mainClass = 'org.wpilib.Main' } apply plugin: 'com.gradleup.shadow' diff --git a/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java b/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java deleted file mode 100644 index fb85333b273..00000000000 --- a/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java +++ /dev/null @@ -1,196 +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; - -import com.google.gson.Gson; -import com.google.gson.GsonBuilder; -import com.google.gson.JsonArray; -import com.google.gson.JsonElement; -import com.google.gson.JsonObject; -import com.google.gson.JsonParser; -import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.cscore.VideoSource; -import edu.wpi.first.networktables.NetworkTableInstance; -import java.io.IOException; -import java.nio.file.Files; -import java.nio.file.Paths; -import java.util.ArrayList; -import java.util.List; - -/* - JSON format: - { - "team": , - "ntmode": <"client" or "server", "client" if unspecified> - "cameras": [ - { - "name": - "path": - "pixel format": <"MJPEG", "YUYV", etc> // optional - "width":