From 8639db3507c2ee592e237b4556b876b87efc259e Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Sat, 17 Feb 2024 08:35:28 -0800 Subject: [PATCH 1/2] Add AprilTagFieldLayout.loadField() and migrate to it --- .../first/apriltag/AprilTagFieldLayout.java | 17 +++++++++ .../wpi/first/apriltag/AprilTagFields.java | 8 +---- .../main/native/cpp/AprilTagFieldLayout.cpp | 34 ++++++++++++++++++ .../src/main/native/cpp/AprilTagFields.cpp | 36 ------------------- .../frc/apriltag/AprilTagFieldLayout.h | 18 ++++++++++ .../include/frc/apriltag/AprilTagFields.h | 10 ------ .../wpi/first/apriltag/LoadConfigTest.java | 5 +-- .../src/test/native/cpp/LoadConfigTest.cpp | 5 +-- .../include/Drivetrain.h | 2 +- .../Drivetrain.java | 4 ++- 10 files changed, 80 insertions(+), 59 deletions(-) delete mode 100644 apriltag/src/main/native/cpp/AprilTagFields.cpp diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java index 5d0e4248878..d2a5dd1cdfe 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java @@ -16,6 +16,7 @@ import java.io.IOException; import java.io.InputStream; import java.io.InputStreamReader; +import java.io.UncheckedIOException; import java.nio.file.Path; import java.util.ArrayList; import java.util.HashMap; @@ -221,6 +222,22 @@ public void serialize(Path path) throws IOException { new ObjectMapper().writeValue(path.toFile(), this); } + /** + * Get an official {@link AprilTagFieldLayout}. + * + * @param field The loadable AprilTag field layout. + * @return AprilTagFieldLayout of the field. + * @throws UncheckedIOException If the layout does not exist. + */ + public static AprilTagFieldLayout loadField(AprilTagFields field) { + try { + return loadFromResource(field.m_resourceFile); + } catch (IOException e) { + throw new UncheckedIOException( + "Could not load AprilTagFieldLayout from " + field.m_resourceFile, e); + } + } + /** * Deserializes a field layout from a resource within a internal jar file. * diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java index 653151e6266..776050c2749 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java @@ -4,7 +4,6 @@ package edu.wpi.first.apriltag; -import java.io.IOException; import java.io.UncheckedIOException; /** Loadable AprilTag field layouts. */ @@ -36,11 +35,6 @@ public enum AprilTagFields { * @throws UncheckedIOException If the layout does not exist */ public AprilTagFieldLayout loadAprilTagLayoutField() { - try { - return AprilTagFieldLayout.loadFromResource(m_resourceFile); - } catch (IOException e) { - throw new UncheckedIOException( - "Could not load AprilTagFieldLayout from " + m_resourceFile, e); - } + return AprilTagFieldLayout.loadField(this); } } diff --git a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp index 59b30ec30eb..c0b4ca233bb 100644 --- a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -125,3 +125,37 @@ void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) { layout.m_fieldWidth = units::meter_t{json.at("field").at("width").get()}; } + +// Use namespace declaration for forward declaration +namespace frc { + +// C++ generated from resource files +std::string_view GetResource_2022_rapidreact_json(); +std::string_view GetResource_2023_chargedup_json(); +std::string_view GetResource_2024_crescendo_json(); + +} // namespace frc + +AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) { + std::string_view fieldString; + switch (field) { + case AprilTagField::k2022RapidReact: + fieldString = GetResource_2022_rapidreact_json(); + break; + case AprilTagField::k2023ChargedUp: + fieldString = GetResource_2023_chargedup_json(); + break; + case AprilTagField::k2024Crescendo: + fieldString = GetResource_2024_crescendo_json(); + break; + case AprilTagField::kNumFields: + throw std::invalid_argument("Invalid Field"); + } + + wpi::json json = wpi::json::parse(fieldString); + return json.get(); +} + +AprilTagFieldLayout frc::LoadAprilTagLayoutField(AprilTagField field) { + return AprilTagFieldLayout::LoadField(field); +} diff --git a/apriltag/src/main/native/cpp/AprilTagFields.cpp b/apriltag/src/main/native/cpp/AprilTagFields.cpp deleted file mode 100644 index be512caa00b..00000000000 --- a/apriltag/src/main/native/cpp/AprilTagFields.cpp +++ /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. - -#include "frc/apriltag/AprilTagFields.h" - -#include - -namespace frc { - -// C++ generated from resource files -std::string_view GetResource_2022_rapidreact_json(); -std::string_view GetResource_2023_chargedup_json(); -std::string_view GetResource_2024_crescendo_json(); - -AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) { - std::string_view fieldString; - switch (field) { - case AprilTagField::k2022RapidReact: - fieldString = GetResource_2022_rapidreact_json(); - break; - case AprilTagField::k2023ChargedUp: - fieldString = GetResource_2023_chargedup_json(); - break; - case AprilTagField::k2024Crescendo: - fieldString = GetResource_2024_crescendo_json(); - break; - case AprilTagField::kNumFields: - throw std::invalid_argument("Invalid Field"); - } - - wpi::json json = wpi::json::parse(fieldString); - return json.get(); -} - -} // namespace frc diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 1f5397b25a2..6ba3824ed66 100644 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -14,6 +14,7 @@ #include #include "frc/apriltag/AprilTag.h" +#include "frc/apriltag/AprilTagFields.h" #include "frc/geometry/Pose3d.h" namespace frc { @@ -48,6 +49,14 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { 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; /** @@ -152,4 +161,13 @@ void to_json(wpi::json& json, const AprilTagFieldLayout& layout); WPILIB_DLLEXPORT void from_json(const wpi::json& json, AprilTagFieldLayout& layout); +/** + * Loads an AprilTagFieldLayout from a predefined field + * + * @param field The predefined field + * @return AprilTagFieldLayout of the field + */ +WPILIB_DLLEXPORT AprilTagFieldLayout +LoadAprilTagLayoutField(AprilTagField field); + } // namespace frc diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h index 933cf287ae3..6ef0930e9b6 100644 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h @@ -8,8 +8,6 @@ #include -#include "frc/apriltag/AprilTagFieldLayout.h" - namespace frc { /** @@ -28,12 +26,4 @@ enum class AprilTagField { 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/test/java/edu/wpi/first/apriltag/LoadConfigTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java index d5deb573af9..e4e4875a194 100644 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java +++ b/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java @@ -22,13 +22,14 @@ class LoadConfigTest { @ParameterizedTest @EnumSource(AprilTagFields.class) void testLoad(AprilTagFields field) { - AprilTagFieldLayout layout = Assertions.assertDoesNotThrow(field::loadAprilTagLayoutField); + AprilTagFieldLayout layout = + Assertions.assertDoesNotThrow(() -> AprilTagFieldLayout.loadField(field)); assertNotNull(layout); } @Test void test2022RapidReact() { - AprilTagFieldLayout layout = AprilTagFields.k2022RapidReact.loadAprilTagLayoutField(); + AprilTagFieldLayout layout = AprilTagFieldLayout.loadField(AprilTagFields.k2022RapidReact); // Blue Hangar Truss - Hub Pose3d expectedPose = diff --git a/apriltag/src/test/native/cpp/LoadConfigTest.cpp b/apriltag/src/test/native/cpp/LoadConfigTest.cpp index c3ff7c03c40..ba6fe81ff96 100644 --- a/apriltag/src/test/native/cpp/LoadConfigTest.cpp +++ b/apriltag/src/test/native/cpp/LoadConfigTest.cpp @@ -4,6 +4,7 @@ #include +#include "frc/apriltag/AprilTagFieldLayout.h" #include "frc/apriltag/AprilTagFields.h" namespace frc { @@ -20,7 +21,7 @@ std::vector GetAllFields() { TEST(AprilTagFieldsTest, TestLoad2022RapidReact) { AprilTagFieldLayout layout = - LoadAprilTagLayoutField(AprilTagField::k2022RapidReact); + AprilTagFieldLayout::LoadField(AprilTagField::k2022RapidReact); // Blue Hangar Truss - Hub auto expectedPose = @@ -53,7 +54,7 @@ class AllFieldsFixtureTest : public ::testing::TestWithParam {}; TEST_P(AllFieldsFixtureTest, CheckEntireEnum) { AprilTagField field = GetParam(); - EXPECT_NO_THROW(LoadAprilTagLayoutField(field)); + EXPECT_NO_THROW(AprilTagFieldLayout::LoadField(field)); } INSTANTIATE_TEST_SUITE_P(ValuesEnumTestInstTests, AllFieldsFixtureTest, diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h index 63a9aea0142..c7d723a1afe 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h @@ -131,7 +131,7 @@ class Drivetrain { nt::DoubleArrayEntry& m_cameraToObjectEntryRef = m_cameraToObjectEntry; frc::AprilTagFieldLayout m_aprilTagFieldLayout{ - frc::LoadAprilTagLayoutField(frc::AprilTagField::k2022RapidReact)}; + frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2022RapidReact)}; frc::Pose3d m_objectInField{m_aprilTagFieldLayout.GetTagPose(0).value()}; frc::PWMSparkMax m_leftLeader{1}; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java index 9b7ec75b5f8..009a2bc2aa8 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -4,6 +4,7 @@ package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator; +import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.ComputerVisionUtil; import edu.wpi.first.math.VecBuilder; @@ -125,7 +126,8 @@ public Drivetrain(DoubleArrayTopic cameraToObjectTopic) { m_cameraToObjectEntry = cameraToObjectTopic.getEntry(m_defaultVal); - m_objectInField = AprilTagFields.k2022RapidReact.loadAprilTagLayoutField().getTagPose(0).get(); + m_objectInField = + AprilTagFieldLayout.loadField(AprilTagFields.k2022RapidReact).getTagPose(0).get(); SmartDashboard.putData("Field", m_fieldSim); SmartDashboard.putData("FieldEstimation", m_fieldApproximation); From 31718cbf0d5cbbf7345e8aab8b2a116ee22062cf Mon Sep 17 00:00:00 2001 From: Joseph Eng Date: Sat, 17 Feb 2024 16:52:57 -0800 Subject: [PATCH 2/2] Update examples to 2024 --- .../DifferentialDrivePoseEstimator/include/Drivetrain.h | 2 +- .../examples/differentialdriveposeestimator/Drivetrain.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h index c7d723a1afe..633571ef173 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h @@ -131,7 +131,7 @@ class Drivetrain { nt::DoubleArrayEntry& m_cameraToObjectEntryRef = m_cameraToObjectEntry; frc::AprilTagFieldLayout m_aprilTagFieldLayout{ - frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2022RapidReact)}; + frc::AprilTagFieldLayout::LoadField(frc::AprilTagField::k2024Crescendo)}; frc::Pose3d m_objectInField{m_aprilTagFieldLayout.GetTagPose(0).value()}; frc::PWMSparkMax m_leftLeader{1}; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java index 009a2bc2aa8..d0c200250b0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -127,7 +127,7 @@ public Drivetrain(DoubleArrayTopic cameraToObjectTopic) { m_cameraToObjectEntry = cameraToObjectTopic.getEntry(m_defaultVal); m_objectInField = - AprilTagFieldLayout.loadField(AprilTagFields.k2022RapidReact).getTagPose(0).get(); + AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo).getTagPose(0).get(); SmartDashboard.putData("Field", m_fieldSim); SmartDashboard.putData("FieldEstimation", m_fieldApproximation);