[wpical] Add WPIcal: Field Calibration Tool (#6915)

Co-authored-by: Gold856 <117957790+Gold856@users.noreply.github.com>
Co-authored-by: Jade <spacey-sooty@proton.me>
Co-authored-by: Matthew Morley <matthew.morley.ca@gmail.com>

wpical/src/main/native/include/fieldmap.h

@@ -0,0 +1,59 @@
+// 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 <cmath>
+#include <vector>
+
+#include <tagpose.h>
+#include <wpi/json.h>
+
+class Fieldmap {
+ public:
+  Fieldmap() = default;
+  explicit Fieldmap(const wpi::json& json) {
+    double field_length_meters =
+        static_cast<double>(json.at("field").at("length"));
+    double field_width_meters =
+        static_cast<double>(json.at("field").at("width"));
+    for (const auto& tag : json.at("tags").items()) {
+      double tagXPos =
+          static_cast<double>(tag.value().at("pose").at("translation").at("x"));
+      double tagYPos =
+          static_cast<double>(tag.value().at("pose").at("translation").at("y"));
+      double tagZPos =
+          static_cast<double>(tag.value().at("pose").at("translation").at("z"));
+      double tagWQuat = static_cast<double>(
+          tag.value().at("pose").at("rotation").at("quaternion").at("W"));
+      double tagXQuat = static_cast<double>(
+          tag.value().at("pose").at("rotation").at("quaternion").at("X"));
+      double tagYQuat = static_cast<double>(
+          tag.value().at("pose").at("rotation").at("quaternion").at("Y"));
+      double tagZQuat = static_cast<double>(
+          tag.value().at("pose").at("rotation").at("quaternion").at("Z"));
+
+      tagVec.emplace_back(tagXPos, tagYPos, tagZPos, tagWQuat, tagXQuat,
+                          tagYQuat, tagZQuat, field_length_meters,
+                          field_width_meters);
+    }
+  }
+
+  const tag::Pose& getTag(size_t tag) const { return tagVec[tag - 1]; }
+
+  int getNumTags() const { return tagVec.size(); }
+
+  static double minimizeAngle(double angle) {
+    angle = std::fmod(angle, 360);
+    if (angle > 180) {
+      return angle - 360;
+    } else if (angle < -180) {
+      return angle + 360;
+    }
+    return angle;
+  }
+
+ private:
+  std::vector<tag::Pose> tagVec;
+};