diff --git a/CMakeLists.txt b/CMakeLists.txt index 53d901f4cb..076eca2232 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -315,6 +315,7 @@ endif() if (WITH_WPILIB) set(WPILIBC_DEP_REPLACE ${WPILIBC_DEP_REPLACE_IMPL}) + add_subdirectory(apriltag) add_subdirectory(wpilibj) add_subdirectory(wpilibc) add_subdirectory(wpilibNewCommands) diff --git a/apriltag/CMakeLists.txt b/apriltag/CMakeLists.txt new file mode 100644 index 0000000000..d71ae35ccc --- /dev/null +++ b/apriltag/CMakeLists.txt @@ -0,0 +1,41 @@ +project(fieldImages) + +include(CompileWarnings) +include(GenResources) + +if (WITH_JAVA) + find_package(Java REQUIRED) + include(UseJava) + + file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar") + + set(CMAKE_JAVA_INCLUDE_PATH apriltags.jar ${JACKSON_JARS}) + + file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java) + file(GLOB_RECURSE JAVA_RESOURCES src/main/native/resources/*.json) + add_jar(apriltags_jar SOURCES ${JAVA_SOURCES} RESOURCES NAMESPACE "edu/wpi/first/apriltag" ${JAVA_RESOURCES} INCLUDE_JARS wpimath_jar OUTPUT_NAME apriltag) +endif() + + +GENERATE_RESOURCES(src/main/native/resources/edu/wpi/first/apriltag generated/main/cpp APRILTAG frc apriltags_resources_src) + +file(GLOB_RECURSE apriltags_native_src src/main/native/cpp/*.cpp) + +add_library(apriltags ${apriltags_native_src} ${apriltags_resources_src}) +set_target_properties(apriltags PROPERTIES DEBUG_POSTFIX "d") + +set_property(TARGET apriltags PROPERTY FOLDER "libraries") +target_compile_features(apriltags PUBLIC cxx_std_20) +wpilib_target_warnings(apriltags) +target_link_libraries(apriltags wpimath) + +target_include_directories(apriltags PUBLIC + $ + $) + + +if (WITH_TESTS) + wpilib_add_test(apriltags src/test/native/cpp) + target_include_directories(apriltags_test PRIVATE src/test/native/include) + target_link_libraries(apriltags_test apriltags gmock_main) +endif() diff --git a/apriltag/build.gradle b/apriltag/build.gradle new file mode 100644 index 0000000000..2422918f7a --- /dev/null +++ b/apriltag/build.gradle @@ -0,0 +1,77 @@ +apply from: "${rootDir}/shared/resources.gradle" + +ext { + nativeName = 'apriltags' + devMain = 'edu.wpi.first.apriltag.DevMain' + + def generateTask = createGenerateResourcesTask('main', 'APRILTAGS', 'frc', project) + + tasks.withType(CppCompile) { + dependsOn generateTask + } + extraSetup = { + it.sources { + resourcesCpp(CppSourceSet) { + source { + srcDirs "$buildDir/generated/main/cpp", "$rootDir/shared/singlelib" + include '*.cpp' + } + } + } + } +} + +evaluationDependsOn(':wpimath') + +apply from: "${rootDir}/shared/javacpp/setupBuild.gradle" + +dependencies { + implementation project(':wpimath') +} + +sourceSets { + main { + resources { + srcDirs 'src/main/native/resources' + } + } +} + + +model { + components {} + binaries { + all { + if (!it.buildable || !(it instanceof NativeBinarySpec)) { + return + } + it.cppCompiler.define 'WPILIB_EXPORTS' + + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' + lib project: ':wpimath', library: 'wpimath', linkage: 'shared' + + if ((it instanceof NativeExecutableBinarySpec || it instanceof GoogleTestTestSuiteBinarySpec) && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { + nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries') + } + } + } + tasks { + def c = $.components + def found = false + def systemArch = getCurrentArch() + c.each { + if (it in NativeExecutableSpec && it.name == "${nativeName}Dev") { + it.binaries.each { + if (!found) { + def arch = it.targetPlatform.name + if (arch == systemArch) { + def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib' + + found = true + } + } + } + } + } + } +} diff --git a/apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.java b/apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.java new file mode 100644 index 0000000000..35204b5b80 --- /dev/null +++ b/apriltag/src/dev/java/edu/wpi/first/apriltag/DevMain.java @@ -0,0 +1,14 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.apriltag; + +public final class DevMain { + /** Main entry point. */ + public static void main(String[] args) { + System.out.println("Hello World!"); + } + + private DevMain() {} +} diff --git a/apriltag/src/dev/native/cpp/main.cpp b/apriltag/src/dev/native/cpp/main.cpp new file mode 100644 index 0000000000..a3e363efca --- /dev/null +++ b/apriltag/src/dev/native/cpp/main.cpp @@ -0,0 +1,5 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +int main() {} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java similarity index 96% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java rename to apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java index 29e3e74ce4..8e455f6dc9 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTag.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package edu.wpi.first.wpilibj.apriltag; +package edu.wpi.first.apriltag; import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonProperty; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java similarity index 67% rename from wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java rename to apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java index 22f7d2765d..aed1cb7376 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java @@ -2,18 +2,20 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package edu.wpi.first.wpilibj.apriltag; +package edu.wpi.first.apriltag; import com.fasterxml.jackson.annotation.JsonAutoDetect; import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnore; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.DriverStation; import java.io.IOException; +import java.io.InputStream; +import java.io.InputStreamReader; import java.nio.file.Path; import java.util.ArrayList; import java.util.HashMap; @@ -31,21 +33,26 @@ import java.util.Optional; * meters with "width" and "length" values. This is to account for arbitrary field sizes when * mirroring the poses. * - *

Pose3ds are assumed to be measured from the bottom-left corner of the field, when the blue - * alliance is at the left. Pose3ds will automatically be returned as passed in when calling {@link - * AprilTagFieldLayout#getTagPose(int)}. Setting an alliance color via {@link - * AprilTagFieldLayout#setAlliance(DriverStation.Alliance)} will mirror the poses returned from - * {@link AprilTagFieldLayout#getTagPose(int)} to be correct relative to the other alliance. + *

Pose3ds are assumed to be measured from the blue alliance right side of the field, when the + * blue alliance is at the left. Pose3ds will automatically be returned as passed in when calling + * {@link AprilTagFieldLayout#getTagPose(int)}. You can call {@link #setOrigin(OriginPosition)} to + * mirror the poses returned from {@link AprilTagFieldLayout#getTagPose(int)} to be correct relative + * to a custom frame. */ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class AprilTagFieldLayout { + public enum OriginPosition { + kBlueAllianceWallRightSide, + kRedAllianceWallRightSide, + } + private final Map m_apriltags = new HashMap<>(); @JsonProperty(value = "field") private FieldDimensions m_fieldDimensions; - private boolean m_mirror; + private Pose3d m_origin; /** * Construct a new AprilTagFieldLayout with values imported from a JSON file. @@ -68,6 +75,7 @@ public class AprilTagFieldLayout { new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class); m_apriltags.putAll(layout.m_apriltags); m_fieldDimensions = layout.m_fieldDimensions; + setOrigin(OriginPosition.kBlueAllianceWallRightSide); } /** @@ -90,6 +98,7 @@ public class AprilTagFieldLayout { m_apriltags.put(tag.ID, tag); } m_fieldDimensions = fieldDimensions; + setOrigin(OriginPosition.kBlueAllianceWallRightSide); } @JsonProperty("tags") @@ -98,15 +107,42 @@ public class AprilTagFieldLayout { } /** - * Set the alliance that your team is on. + * Sets the origin based on a pre-known enumeration of positions. The position is calculated from + * values in the configuration file. * *

This changes the {@link #getTagPose(int)} method to return the correct pose for your * alliance. * - * @param alliance The alliance to mirror poses for. + * @param position The predefined position */ - public void setAlliance(DriverStation.Alliance alliance) { - m_mirror = alliance == DriverStation.Alliance.Red; + @JsonIgnore + public void setOrigin(OriginPosition position) { + switch (position) { + case kBlueAllianceWallRightSide: + setOrigin(new Pose3d()); + break; + case kRedAllianceWallRightSide: + setOrigin( + new Pose3d( + new Translation3d(m_fieldDimensions.fieldWidth, m_fieldDimensions.fieldLength, 0), + new Rotation3d(0, 0, Math.PI))); + break; + default: + throw new IllegalArgumentException("Unsupported enum value"); + } + } + + /** + * Sets the origin for tag pose transformation. + * + *

This changes the {@link #getTagPose(int)} method to return the correct pose for your + * alliance. + * + * @param origin The new origin for tag transformations + */ + @JsonIgnore + public void setOrigin(Pose3d origin) { + m_origin = origin; } /** @@ -122,16 +158,7 @@ public class AprilTagFieldLayout { if (tag == null) { return Optional.empty(); } - Pose3d pose = tag.pose; - if (m_mirror) { - pose = - pose.relativeTo( - new Pose3d( - new Translation3d( - m_fieldDimensions.fieldWidth, m_fieldDimensions.fieldLength, 0.0), - new Rotation3d(0.0, 0.0, Math.PI))); - } - return Optional.of(pose); + return Optional.of(tag.pose.relativeTo(m_origin)); } /** @@ -154,18 +181,32 @@ public class AprilTagFieldLayout { new ObjectMapper().writeValue(path.toFile(), this); } + /** + * Deserializes a field layout from a resource within a jar file. + * + * @param resourcePath The absolute path of the resource + * @return The deserialized layout + * @throws IOException If the resource could not be loaded + */ + public static AprilTagFieldLayout loadFromResource(String resourcePath) throws IOException { + try (InputStream stream = AprilTagFieldLayout.class.getResourceAsStream(resourcePath); + InputStreamReader reader = new InputStreamReader(stream)) { + return new ObjectMapper().readerFor(AprilTagFieldLayout.class).readValue(reader); + } + } + @Override public boolean equals(Object obj) { if (obj instanceof AprilTagFieldLayout) { var other = (AprilTagFieldLayout) obj; - return m_apriltags.equals(other.m_apriltags) && m_mirror == other.m_mirror; + return m_apriltags.equals(other.m_apriltags) && m_origin.equals(other.m_origin); } return false; } @Override public int hashCode() { - return Objects.hash(m_apriltags, m_mirror); + return Objects.hash(m_apriltags, m_origin); } @JsonIgnoreProperties(ignoreUnknown = true) diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java new file mode 100644 index 0000000000..28eeb58de9 --- /dev/null +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.apriltag; + +public enum AprilTagFields { + k2022RapidReact("2022-rapidreact.json"); + + public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/"; + + /** Alias to the current game. */ + public static final AprilTagFields kDefaultField = k2022RapidReact; + + public final String m_resourceFile; + + AprilTagFields(String resourceFile) { + m_resourceFile = kBaseResourceDir + resourceFile; + } +} diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTag.cpp b/apriltag/src/main/native/cpp/AprilTag.cpp similarity index 100% rename from wpilibc/src/main/native/cpp/apriltag/AprilTag.cpp rename to apriltag/src/main/native/cpp/AprilTag.cpp diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp similarity index 82% rename from wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp rename to apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp index 3af133157a..6ed49c63b0 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -42,8 +42,22 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::vector apriltags, } } -void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { - m_mirror = alliance == DriverStation::Alliance::kRed; +void AprilTagFieldLayout::SetOrigin(OriginPosition position) { + switch (position) { + case OriginPosition::kBlueAllianceWallRightSide: + SetOrigin(Pose3d{}); + break; + case OriginPosition::kRedAllianceWallRightSide: + SetOrigin(Pose3d{Translation3d{m_fieldLength, m_fieldWidth, 0_m}, + Rotation3d{0_deg, 0_deg, 180_deg}}); + break; + default: + throw std::invalid_argument("Invalid origin"); + } +} + +void AprilTagFieldLayout::SetOrigin(const Pose3d& origin) { + m_origin = origin; } std::optional AprilTagFieldLayout::GetTagPose(int ID) const { @@ -51,12 +65,7 @@ std::optional AprilTagFieldLayout::GetTagPose(int ID) const { if (it == m_apriltags.end()) { return std::nullopt; } - Pose3d returnPose = it->second.pose; - if (m_mirror) { - returnPose = returnPose.RelativeTo(Pose3d{ - m_fieldLength, m_fieldWidth, 0_m, Rotation3d{0_deg, 0_deg, 180_deg}}); - } - return returnPose; + return it->second.pose.RelativeTo(m_origin); } void AprilTagFieldLayout::Serialize(std::string_view path) { @@ -73,7 +82,7 @@ void AprilTagFieldLayout::Serialize(std::string_view path) { } bool AprilTagFieldLayout::operator==(const AprilTagFieldLayout& other) const { - return m_apriltags == other.m_apriltags && m_mirror == other.m_mirror && + return m_apriltags == other.m_apriltags && m_origin == other.m_origin && m_fieldLength == other.m_fieldLength && m_fieldWidth == other.m_fieldWidth; } diff --git a/apriltag/src/main/native/cpp/AprilTagFields.cpp b/apriltag/src/main/native/cpp/AprilTagFields.cpp new file mode 100644 index 0000000000..3d7b1ed63e --- /dev/null +++ b/apriltag/src/main/native/cpp/AprilTagFields.cpp @@ -0,0 +1,28 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/apriltag/AprilTagFields.h" + +#include + +namespace frc { + +// C++ generated from resource files +std::string_view GetResource_2022_rapidreact_json(); + +AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) { + std::string_view fieldString; + switch (field) { + case AprilTagField::k2022RapidReact: + fieldString = GetResource_2022_rapidreact_json(); + break; + case AprilTagField::kNumFields: + throw std::invalid_argument("Invalid Field"); + } + + wpi::json json = wpi::json::parse(fieldString); + return json.get(); +} + +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h b/apriltag/src/main/native/include/frc/apriltag/AprilTag.h similarity index 100% rename from wpilibc/src/main/native/include/frc/apriltag/AprilTag.h rename to apriltag/src/main/native/include/frc/apriltag/AprilTag.h diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h similarity index 86% rename from wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h rename to apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index ec2217be1b..aab33443ec 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -12,7 +12,6 @@ #include #include -#include "frc/DriverStation.h" #include "frc/apriltag/AprilTag.h" #include "frc/geometry/Pose3d.h" @@ -40,6 +39,11 @@ namespace frc { */ class WPILIB_DLLEXPORT AprilTagFieldLayout { public: + enum class OriginPosition { + kBlueAllianceWallRightSide, + kRedAllianceWallRightSide, + }; + AprilTagFieldLayout() = default; /** @@ -60,14 +64,25 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { units::meter_t fieldLength, units::meter_t fieldWidth); /** - * Set the alliance that your team is on. + * Sets the origin based on a pre-known enumeration of positions. The position + * is calculated from values in the configuration file. * * This changes the GetTagPose(int) method to return the correct pose for your * alliance. * * @param alliance The alliance to mirror poses for. */ - void SetAlliance(DriverStation::Alliance alliance); + void SetOrigin(OriginPosition position); + + /** + * Sets the origin for tag pose transformation. + * + * This changes the GetTagPose(int) method to return the correct pose for your + * alliance. + * + * @param alliance The alliance to mirror poses for. + */ + void SetOrigin(const Pose3d& origin); /** * Gets an AprilTag pose by its ID. @@ -105,7 +120,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { std::unordered_map m_apriltags; units::meter_t m_fieldLength; units::meter_t m_fieldWidth; - bool m_mirror = false; + Pose3d m_origin; friend WPILIB_DLLEXPORT void to_json(wpi::json& json, const AprilTagFieldLayout& layout); diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h new file mode 100644 index 0000000000..2810a1aac0 --- /dev/null +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h @@ -0,0 +1,31 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include + +#include "frc/apriltag/AprilTagFieldLayout.h" + +namespace frc { + +enum class AprilTagField { + k2022RapidReact, + + // This is a placeholder for denoting the last supported field. This should + // always be the last entry in the enum and should not be used by users + kNumFields, +}; + +/** + * Loads an AprilTagFieldLayout from a predefined field + * + * @param field The predefined field + */ +WPILIB_DLLEXPORT AprilTagFieldLayout +LoadAprilTagLayoutField(AprilTagField field); + +} // namespace frc diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2022-rapidreact.json b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2022-rapidreact.json new file mode 100644 index 0000000000..b77715188e --- /dev/null +++ b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2022-rapidreact.json @@ -0,0 +1,415 @@ +{ + "tags" : [ { + "ID" : 0, + "pose" : { + "translation" : { + "x" : -0.0035306, + "y" : 7.578928199999999, + "z" : 0.8858503999999999 + }, + "rotation" : { + "quaternion" : { + "W" : 1.0, + "X" : 0.0, + "Y" : 0.0, + "Z" : 0.0 + } + } + } + }, { + "ID" : 1, + "pose" : { + "translation" : { + "x" : 3.2327088, + "y" : 5.486654, + "z" : 1.7254728 + }, + "rotation" : { + "quaternion" : { + "W" : 1.0, + "X" : 0.0, + "Y" : 0.0, + "Z" : 0.0 + } + } + } + }, { + "ID" : 2, + "pose" : { + "translation" : { + "x" : 3.067812, + "y" : 5.3305202, + "z" : 1.3762228 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7071067811865476, + "X" : 0.0, + "Y" : 0.0, + "Z" : -0.7071067811865475 + } + } + } + }, { + "ID" : 3, + "pose" : { + "translation" : { + "x" : 0.0039878, + "y" : 5.058536999999999, + "z" : 0.80645 + }, + "rotation" : { + "quaternion" : { + "W" : 1.0, + "X" : 0.0, + "Y" : 0.0, + "Z" : 0.0 + } + } + } + }, { + "ID" : 4, + "pose" : { + "translation" : { + "x" : 0.0039878, + "y" : 3.5124898, + "z" : 0.80645 + }, + "rotation" : { + "quaternion" : { + "W" : 1.0, + "X" : 0.0, + "Y" : 0.0, + "Z" : 0.0 + } + } + } + }, { + "ID" : 5, + "pose" : { + "translation" : { + "x" : 0.12110719999999998, + "y" : 1.7178274, + "z" : 0.8906002000000001 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9196502204050923, + "X" : 0.0, + "Y" : 0.0, + "Z" : 0.39273842708457407 + } + } + } + }, { + "ID" : 6, + "pose" : { + "translation" : { + "x" : 0.8733027999999999, + "y" : 0.9412985999999999, + "z" : 0.8906002000000001 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9196502204050923, + "X" : 0.0, + "Y" : 0.0, + "Z" : 0.39273842708457407 + } + } + } + }, { + "ID" : 7, + "pose" : { + "translation" : { + "x" : 1.6150844, + "y" : 0.15725139999999999, + "z" : 0.8906002000000001 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9196502204050923, + "X" : 0.0, + "Y" : 0.0, + "Z" : 0.39273842708457407 + } + } + } + }, { + "ID" : 10, + "pose" : { + "translation" : { + "x" : 16.4627306, + "y" : 0.6506718, + "z" : 0.8858503999999999 + }, + "rotation" : { + "quaternion" : { + "W" : 6.123233995736766E-17, + "X" : 0.0, + "Y" : 0.0, + "Z" : 1.0 + } + } + } + }, { + "ID" : 11, + "pose" : { + "translation" : { + "x" : 13.2350002, + "y" : 2.743454, + "z" : 1.7254728 + }, + "rotation" : { + "quaternion" : { + "W" : 6.123233995736766E-17, + "X" : 0.0, + "Y" : 0.0, + "Z" : 1.0 + } + } + } + }, { + "ID" : 12, + "pose" : { + "translation" : { + "x" : 13.391388000000001, + "y" : 2.8998418, + "z" : 1.3762228 + }, + "rotation" : { + "quaternion" : { + "W" : 0.7071067811865476, + "X" : 0.0, + "Y" : 0.0, + "Z" : 0.7071067811865475 + } + } + } + }, { + "ID" : 13, + "pose" : { + "translation" : { + "x" : 16.4552122, + "y" : 3.1755079999999998, + "z" : 0.80645 + }, + "rotation" : { + "quaternion" : { + "W" : 6.123233995736766E-17, + "X" : 0.0, + "Y" : 0.0, + "Z" : 1.0 + } + } + } + }, { + "ID" : 14, + "pose" : { + "translation" : { + "x" : 16.4552122, + "y" : 4.7171356, + "z" : 0.80645 + }, + "rotation" : { + "quaternion" : { + "W" : 6.123233995736766E-17, + "X" : 0.0, + "Y" : 0.0, + "Z" : 1.0 + } + } + } + }, { + "ID" : 15, + "pose" : { + "translation" : { + "x" : 16.3350194, + "y" : 6.5149729999999995, + "z" : 0.8937752 + }, + "rotation" : { + "quaternion" : { + "W" : -0.37298778257580906, + "X" : -0.0, + "Y" : 0.0, + "Z" : 0.9278362538989199 + } + } + } + }, { + "ID" : 16, + "pose" : { + "translation" : { + "x" : 15.5904946, + "y" : 7.292695599999999, + "z" : 0.8906002000000001 + }, + "rotation" : { + "quaternion" : { + "W" : -0.37298778257580906, + "X" : -0.0, + "Y" : 0.0, + "Z" : 0.9278362538989199 + } + } + } + }, { + "ID" : 17, + "pose" : { + "translation" : { + "x" : 14.847188999999998, + "y" : 8.0691228, + "z" : 0.8906002000000001 + }, + "rotation" : { + "quaternion" : { + "W" : -0.37298778257580906, + "X" : -0.0, + "Y" : 0.0, + "Z" : 0.9278362538989199 + } + } + } + }, { + "ID" : 40, + "pose" : { + "translation" : { + "x" : 7.874127, + "y" : 4.9131728, + "z" : 0.7032752 + }, + "rotation" : { + "quaternion" : { + "W" : 0.5446390350150271, + "X" : 0.0, + "Y" : 0.0, + "Z" : 0.838670567945424 + } + } + } + }, { + "ID" : 41, + "pose" : { + "translation" : { + "x" : 7.4312271999999995, + "y" : 3.759327, + "z" : 0.7032752 + }, + "rotation" : { + "quaternion" : { + "W" : -0.20791169081775934, + "X" : -0.0, + "Y" : 0.0, + "Z" : 0.9781476007338057 + } + } + } + }, { + "ID" : 42, + "pose" : { + "translation" : { + "x" : 8.585073, + "y" : 3.3164272, + "z" : 0.7032752 + }, + "rotation" : { + "quaternion" : { + "W" : 0.838670567945424, + "X" : 0.0, + "Y" : 0.0, + "Z" : -0.5446390350150271 + } + } + } + }, { + "ID" : 43, + "pose" : { + "translation" : { + "x" : 9.0279728, + "y" : 4.470273, + "z" : 0.7032752 + }, + "rotation" : { + "quaternion" : { + "W" : 0.9781476007338057, + "X" : 0.0, + "Y" : 0.0, + "Z" : 0.20791169081775934 + } + } + } + }, { + "ID" : 50, + "pose" : { + "translation" : { + "x" : 7.6790296, + "y" : 4.3261534, + "z" : 2.4177244 + }, + "rotation" : { + "quaternion" : { + "W" : 0.17729273396782605, + "X" : -0.22744989571511945, + "Y" : 0.04215534644161733, + "Z" : 0.9565859910053995 + } + } + } + }, { + "ID" : 51, + "pose" : { + "translation" : { + "x" : 8.0182466, + "y" : 3.5642296, + "z" : 2.4177244 + }, + "rotation" : { + "quaternion" : { + "W" : -0.5510435465842192, + "X" : -0.19063969497246985, + "Y" : -0.13102303230819815, + "Z" : 0.8017733354717242 + } + } + } + }, { + "ID" : 52, + "pose" : { + "translation" : { + "x" : 8.7801704, + "y" : 3.9034466, + "z" : 2.4177244 + }, + "rotation" : { + "quaternion" : { + "W" : -0.9565859910053994, + "X" : -0.04215534644161739, + "Y" : -0.22744989571511942, + "Z" : 0.17729273396782633 + } + } + } + }, { + "ID" : 53, + "pose" : { + "translation" : { + "x" : 8.4409534, + "y" : 4.6653704, + "z" : 2.4177244 + }, + "rotation" : { + "quaternion" : { + "W" : 0.8017733354717241, + "X" : -0.1310230323081982, + "Y" : 0.19063969497246983, + "Z" : 0.5510435465842194 + } + } + } + } ], + "field" : { + "width" : 16.4592, + "length" : 8.2296 + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagPoseMirroringTest.java similarity index 92% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java rename to apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagPoseMirroringTest.java index 169b2493c9..e40a81fcfe 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java +++ b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagPoseMirroringTest.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package edu.wpi.first.wpilibj.apriltag; +package edu.wpi.first.apriltag; import static org.junit.jupiter.api.Assertions.assertEquals; @@ -10,7 +10,6 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; import java.util.List; import org.junit.jupiter.api.Test; @@ -29,7 +28,7 @@ class AprilTagPoseMirroringTest { new Rotation3d(0, 0, Units.degreesToRadians(180))))), Units.feetToMeters(54.0), Units.feetToMeters(27.0)); - layout.setAlliance(DriverStation.Alliance.Red); + layout.setOrigin(AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide); assertEquals( new Pose3d( diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagSerializationTest.java similarity index 96% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java rename to apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagSerializationTest.java index ac47cdcf25..5f5d1fde66 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java +++ b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagSerializationTest.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package edu.wpi.first.wpilibj.apriltag; +package edu.wpi.first.apriltag; import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java new file mode 100644 index 0000000000..ab15994657 --- /dev/null +++ b/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java @@ -0,0 +1,74 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.apriltag; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import java.io.IOException; +import java.util.Optional; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.EnumSource; + +class LoadConfigTest { + @ParameterizedTest + @EnumSource(AprilTagFields.class) + void testLoad(AprilTagFields field) { + AprilTagFieldLayout layout = + Assertions.assertDoesNotThrow( + () -> AprilTagFieldLayout.loadFromResource(field.m_resourceFile)); + assertNotNull(layout); + } + + @Test + void test2022RapidReact() throws IOException { + AprilTagFieldLayout layout = + AprilTagFieldLayout.loadFromResource(AprilTagFields.k2022RapidReact.m_resourceFile); + + // Blue Hangar Truss - Hub + Pose3d expectedPose = + new Pose3d( + Units.inchesToMeters(127.272), + Units.inchesToMeters(216.01), + Units.inchesToMeters(67.932), + new Rotation3d(0, 0, 0)); + Optional 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/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp b/apriltag/src/test/native/cpp/AprilTagJsonTest.cpp similarity index 100% rename from wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp rename to apriltag/src/test/native/cpp/AprilTagJsonTest.cpp diff --git a/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp b/apriltag/src/test/native/cpp/AprilTagPoseMirroringTest.cpp similarity index 91% rename from wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp rename to apriltag/src/test/native/cpp/AprilTagPoseMirroringTest.cpp index 2c3a42bb32..6efde2b317 100644 --- a/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp +++ b/apriltag/src/test/native/cpp/AprilTagPoseMirroringTest.cpp @@ -22,7 +22,8 @@ TEST(AprilTagPoseMirroringTest, MirroringMatches) { 2, Pose3d{4_ft, 4_ft, 4_ft, Rotation3d{0_deg, 0_deg, 180_deg}}}}, 54_ft, 27_ft}; - layout.SetAlliance(DriverStation::Alliance::kRed); + layout.SetOrigin( + AprilTagFieldLayout::OriginPosition::kRedAllianceWallRightSide); auto mirrorPose = Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}}; diff --git a/apriltag/src/test/native/cpp/LoadConfigTest.cpp b/apriltag/src/test/native/cpp/LoadConfigTest.cpp new file mode 100644 index 0000000000..9db61e950f --- /dev/null +++ b/apriltag/src/test/native/cpp/LoadConfigTest.cpp @@ -0,0 +1,61 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/apriltag/AprilTagFields.h" +#include "gtest/gtest.h" + +namespace frc { + +std::vector GetAllFields() { + std::vector output; + + for (int i = 0; i < static_cast(AprilTagField::kNumFields); ++i) { + output.push_back(static_cast(i)); + } + + return output; +} + +TEST(AprilTagFieldsTest, TestLoad2022RapidReact) { + AprilTagFieldLayout layout = + LoadAprilTagLayoutField(AprilTagField::k2022RapidReact); + + // Blue Hangar Truss - Hub + auto expectedPose = + Pose3d{127.272_in, 216.01_in, 67.932_in, Rotation3d{0_deg, 0_deg, 0_deg}}; + auto maybePose = layout.GetTagPose(1); + EXPECT_TRUE(maybePose); + EXPECT_EQ(expectedPose, *maybePose); + + // Blue Terminal Near Station + expectedPose = Pose3d{4.768_in, 67.631_in, 35.063_in, + Rotation3d{0_deg, 0_deg, 46.25_deg}}; + maybePose = layout.GetTagPose(5); + EXPECT_TRUE(maybePose); + EXPECT_EQ(expectedPose, *maybePose); + + // Upper Hub Blue-Near + expectedPose = Pose3d{332.321_in, 183.676_in, 95.186_in, + Rotation3d{0_deg, 26.75_deg, 69_deg}}; + maybePose = layout.GetTagPose(53); + EXPECT_TRUE(maybePose); + EXPECT_EQ(expectedPose, *maybePose); + + // Doesn't exist + maybePose = layout.GetTagPose(54); + EXPECT_FALSE(maybePose); +} + +// Test all of the fields in the enum +class AllFieldsFixtureTest : public ::testing::TestWithParam {}; + +TEST_P(AllFieldsFixtureTest, CheckEntireEnum) { + AprilTagField field = GetParam(); + EXPECT_NO_THROW(LoadAprilTagLayoutField(field)); +} + +INSTANTIATE_TEST_SUITE_P(ValuesEnumTestInstTests, AllFieldsFixtureTest, + ::testing::ValuesIn(GetAllFields())); + +} // namespace frc diff --git a/apriltag/src/test/native/cpp/main.cpp b/apriltag/src/test/native/cpp/main.cpp new file mode 100644 index 0000000000..09072ee347 --- /dev/null +++ b/apriltag/src/test/native/cpp/main.cpp @@ -0,0 +1,11 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "gtest/gtest.h" + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/settings.gradle b/settings.gradle index 816f6ccddc..52dd20dbc8 100644 --- a/settings.gradle +++ b/settings.gradle @@ -52,6 +52,7 @@ include 'myRobot' include 'docs' include 'msvcruntime' include 'ntcoreffi' +include 'apriltag' buildCache { def cred = {