[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>

wpimath/src/main/native/cpp/geometry/Pose3d.cpp

@@ -6,6 +6,8 @@
 
 #include <cmath>
 
+#include <wpi/json.h>
+
 using namespace frc;
 
 namespace {
@@ -149,3 +151,13 @@ Twist3d Pose3d::Log(const Pose3d& end) const {
 Pose2d Pose3d::ToPose2d() const {
   return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
 }
+
+void frc::to_json(wpi::json& json, const Pose3d& pose) {
+  json = wpi::json{{"translation", pose.Translation()},
+                   {"rotation", pose.Rotation()}};
+}
+
+void frc::from_json(const wpi::json& json, Pose3d& pose) {
+  pose = Pose3d{json.at("translation").get<Translation3d>(),
+                json.at("rotation").get<Rotation3d>()};
+}

wpimath/src/main/native/cpp/geometry/Quaternion.cpp

@@ -4,6 +4,8 @@
 
 #include "frc/geometry/Quaternion.h"
 
+#include <wpi/json.h>
+
 using namespace frc;
 
 Quaternion::Quaternion(double w, double x, double y, double z)
@@ -81,3 +83,16 @@ Eigen::Vector3d Quaternion::ToRotationVector() const {
     }
   }
 }
+
+void frc::to_json(wpi::json& json, const Quaternion& quaternion) {
+  json = wpi::json{{"W", quaternion.W()},
+                   {"X", quaternion.X()},
+                   {"Y", quaternion.Y()},
+                   {"Z", quaternion.Z()}};
+}
+
+void frc::from_json(const wpi::json& json, Quaternion& quaternion) {
+  quaternion =
+      Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
+                 json.at("Y").get<double>(), json.at("Z").get<double>()};
+}

wpimath/src/main/native/cpp/geometry/Rotation3d.cpp

@@ -7,6 +7,8 @@
 #include <cmath>
 #include <numbers>
 
+#include <wpi/json.h>
+
 #include "Eigen/Core"
 #include "Eigen/LU"
 #include "Eigen/QR"
@@ -238,3 +240,11 @@ units::radian_t Rotation3d::Angle() const {
 Rotation2d Rotation3d::ToRotation2d() const {
   return Rotation2d{Z()};
 }
+
+void frc::to_json(wpi::json& json, const Rotation3d& rotation) {
+  json = wpi::json{{"quaternion", rotation.GetQuaternion()}};
+}
+
+void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
+  rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
+}

wpimath/src/main/native/cpp/geometry/Translation3d.cpp

@@ -4,6 +4,11 @@
 
 #include "frc/geometry/Translation3d.h"
 
+#include <wpi/json.h>
+
+#include "units/length.h"
+#include "units/math.h"
+
 using namespace frc;
 
 Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
@@ -39,3 +44,15 @@ bool Translation3d::operator==(const Translation3d& other) const {
 bool Translation3d::operator!=(const Translation3d& other) const {
   return !operator==(other);
 }
+
+void frc::to_json(wpi::json& json, const Translation3d& translation) {
+  json = wpi::json{{"x", translation.X().value()},
+                   {"y", translation.Y().value()},
+                   {"z", translation.Z().value()}};
+}
+
+void frc::from_json(const wpi::json& json, Translation3d& translation) {
+  translation = Translation3d{units::meter_t{json.at("x").get<double>()},
+                              units::meter_t{json.at("y").get<double>()},
+                              units::meter_t{json.at("z").get<double>()}};
+}

wpimath/src/main/native/include/frc/geometry/Pose3d.h

@@ -11,6 +11,10 @@
 #include "Translation3d.h"
 #include "Twist3d.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 /**
@@ -202,4 +206,10 @@ class WPILIB_DLLEXPORT Pose3d {
   Rotation3d m_rotation;
 };
 
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Pose3d& pose);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Pose3d& pose);
+
 }  // namespace frc

wpimath/src/main/native/include/frc/geometry/Quaternion.h

@@ -8,6 +8,10 @@
 
 #include "frc/EigenCore.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 class WPILIB_DLLEXPORT Quaternion {
@@ -92,4 +96,10 @@ class WPILIB_DLLEXPORT Quaternion {
   Eigen::Vector3d m_v{0.0, 0.0, 0.0};
 };
 
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Quaternion& quaternion);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Quaternion& quaternion);
+
 }  // namespace frc

wpimath/src/main/native/include/frc/geometry/Rotation3d.h

@@ -11,6 +11,10 @@
 #include "frc/EigenCore.h"
 #include "units/angle.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 /**
@@ -180,4 +184,10 @@ class WPILIB_DLLEXPORT Rotation3d {
   Quaternion m_q;
 };
 
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Rotation3d& rotation);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Rotation3d& rotation);
+
 }  // namespace frc

wpimath/src/main/native/include/frc/geometry/Translation3d.h

@@ -10,6 +10,10 @@
 #include "Translation2d.h"
 #include "units/length.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 /**
@@ -182,6 +186,12 @@ class WPILIB_DLLEXPORT Translation3d {
   units::meter_t m_z = 0_m;
 };
 
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Translation3d& state);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Translation3d& state);
+
 }  // namespace frc
 
 #include "Translation3d.inc"