[apriltag] Split AprilTag functionality to a separate library (#4578)

Add AprilTagFieldLayout JSON file and move class to edu.wpi.first.apriltag.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

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)

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
+                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+                            $<INSTALL_INTERFACE:${include_dest}/apriltags>)
+
+
+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()

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

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() {}
+}

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

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;

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.
  *
- * <p>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.
+ * <p>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<Integer, AprilTag> 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.
    *
    * <p>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.
+   *
+   * <p>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)

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;
+  }
+}

apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp

@@ -42,8 +42,22 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::vector<AprilTag> 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<frc::Pose3d> AprilTagFieldLayout::GetTagPose(int ID) const {
@@ -51,12 +65,7 @@ std::optional<frc::Pose3d> 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;
 }

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 <wpi/json.h>
+
+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<AprilTagFieldLayout>();
+}
+
+}  // namespace frc

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

@@ -12,7 +12,6 @@
 #include <units/length.h>
 #include <wpi/SymbolExports.h>
 
-#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<int, AprilTag> 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);

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 <string_view>
+
+#include <wpi/SymbolExports.h>
+
+#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

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

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(

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;

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<Pose3d> 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());
+  }
+}

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

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<AprilTagField> GetAllFields() {
+  std::vector<AprilTagField> output;
+
+  for (int i = 0; i < static_cast<int>(AprilTagField::kNumFields); ++i) {
+    output.push_back(static_cast<AprilTagField>(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<AprilTagField> {};
+
+TEST_P(AllFieldsFixtureTest, CheckEntireEnum) {
+  AprilTagField field = GetParam();
+  EXPECT_NO_THROW(LoadAprilTagLayoutField(field));
+}
+
+INSTANTIATE_TEST_SUITE_P(ValuesEnumTestInstTests, AllFieldsFixtureTest,
+                         ::testing::ValuesIn(GetAllFields()));
+
+}  // namespace frc

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;
+}

settings.gradle

@@ -52,6 +52,7 @@ include 'myRobot'
 include 'docs'
 include 'msvcruntime'
 include 'ntcoreffi'
+include 'apriltag'
 
 buildCache {
     def cred = {