[apriltag] Add AprilTagFieldLayout.loadField() (wpilibsuite#6377)

KangarooKoala · Feb 18, 2024 · 0ad6b3a · 0ad6b3a
1 parent 02aed35
commit 0ad6b3a
Show file tree

Hide file tree

Showing 10 changed files with 80 additions and 59 deletions.
diff --git 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
@@ -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
@@ -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<double>()};
 }
+
+// Use namespace declaration for forward declaration
+namespace frc {
+
+// C++ generated from resource files
+std::string_view GetResource_2022_rapidreact_json();
+std::string_view GetResource_2023_chargedup_json();
+std::string_view GetResource_2024_crescendo_json();
+
+}  // namespace frc
+
+AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) {
+  std::string_view fieldString;
+  switch (field) {
+    case AprilTagField::k2022RapidReact:
+      fieldString = GetResource_2022_rapidreact_json();
+      break;
+    case AprilTagField::k2023ChargedUp:
+      fieldString = GetResource_2023_chargedup_json();
+      break;
+    case AprilTagField::k2024Crescendo:
+      fieldString = GetResource_2024_crescendo_json();
+      break;
+    case AprilTagField::kNumFields:
+      throw std::invalid_argument("Invalid Field");
+  }
+
+  wpi::json json = wpi::json::parse(fieldString);
+  return json.get<AprilTagFieldLayout>();
+}
+
+AprilTagFieldLayout frc::LoadAprilTagLayoutField(AprilTagField field) {
+  return AprilTagFieldLayout::LoadField(field);
+}
diff --git a/apriltag/src/main/native/cpp/AprilTagFields.cpp b/apriltag/src/main/native/cpp/AprilTagFields.cpp
diff --git 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 <wpi/json_fwd.h>
 
 #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
@@ -8,8 +8,6 @@
 
 #include <wpi/SymbolExports.h>
 
-#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
@@ -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
@@ -4,6 +4,7 @@
 
 #include <gtest/gtest.h>
 
+#include "frc/apriltag/AprilTagFieldLayout.h"
 #include "frc/apriltag/AprilTagFields.h"
 
 namespace frc {
@@ -20,7 +21,7 @@ std::vector<AprilTagField> 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<AprilTagField> {};
 
 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
@@ -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::k2024Crescendo)};
   frc::Pose3d m_objectInField{m_aprilTagFieldLayout.GetTagPose(0).value()};
 
   frc::PWMSparkMax m_leftLeader{1};

diff --git a/...c/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/...c/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.k2024Crescendo).getTagPose(0).get();
 
     SmartDashboard.putData("Field", m_fieldSim);
     SmartDashboard.putData("FieldEstimation", m_fieldApproximation);