[wpilib] Add AprilTag and AprilTagFieldLayout (#4421)

This is an API for looking up a Pose3d from a tag id, and includes functionality to load that map from a JSON file.

This also adds JSON support to Pose3d, Rotation3d. Translation3d, and Quaternion.

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

wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java

@@ -0,0 +1,47 @@
+// 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.wpilibj.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 edu.wpi.first.wpilibj.DriverStation;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class AprilTagPoseMirroringTest {
+  @Test
+  void poseMirroring() {
+    var layout =
+        new AprilTagFieldLayout(
+            List.of(
+                new AprilTag(1, new Pose3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0))),
+                new AprilTag(
+                    2,
+                    new Pose3d(
+                        new Translation3d(
+                            Units.feetToMeters(4.0), Units.feetToMeters(4), Units.feetToMeters(4)),
+                        new Rotation3d(0, 0, Units.degreesToRadians(180))))),
+            Units.feetToMeters(54.0),
+            Units.feetToMeters(27.0));
+    layout.setAlliance(DriverStation.Alliance.Red);
+
+    assertEquals(
+        new Pose3d(
+            new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(180.0))),
+        layout.getTagPose(1).orElse(null));
+
+    assertEquals(
+        new Pose3d(
+            new Translation3d(
+                Units.feetToMeters(50.0), Units.feetToMeters(23.0), Units.feetToMeters(4)),
+            new Rotation3d(0.0, 0.0, 0)),
+        layout.getTagPose(2).orElse(null));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java

@@ -0,0 +1,38 @@
+// 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.wpilibj.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;
+
+class AprilTagSerializationTest {
+  @Test
+  void deserializeMatches() {
+    var layout =
+        new AprilTagFieldLayout(
+            List.of(
+                new AprilTag(1, new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0))),
+                new AprilTag(3, new Pose3d(0, 1, 0, new Rotation3d(0, 0, 0)))),
+            Units.feetToMeters(54.0),
+            Units.feetToMeters(27.0));
+
+    var objectMapper = new ObjectMapper();
+
+    var deserialized =
+        assertDoesNotThrow(
+            () ->
+                objectMapper.readValue(
+                    objectMapper.writeValueAsString(layout), AprilTagFieldLayout.class));
+
+    assertEquals(layout, deserialized);
+  }
+}