From 28f447d9f6ac8547a852c5d9500220a2eb01f407 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Fri, 16 Sep 2022 22:18:19 -0400 Subject: [PATCH 01/73] First attempt at apriltag id-pose API --- .../java/edu/wpi/first/math/WPIMathJNI.java | 16 +++ .../math/apriltag/ApriltagFieldLayout.java | 34 ++++++ .../wpi/first/math/apriltag/ApriltagUtil.java | 83 ++++++++++++++ .../cpp/apriltag/ApriltagFieldLayout.cpp | 32 ++++++ .../main/native/cpp/apriltag/ApriltagUtil.cpp | 54 ++++++++++ .../src/main/native/cpp/jni/WPIMathJNI.cpp | 101 ++++++++++++++++++ .../frc/apriltag/ApriltagFieldLayout.h | 54 ++++++++++ .../include/frc/apriltag/ApriltagUtil.h | 54 ++++++++++ 8 files changed, 428 insertions(+) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagFieldLayout.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagUtil.java create mode 100644 wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp create mode 100644 wpimath/src/main/native/cpp/apriltag/ApriltagUtil.cpp create mode 100644 wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h create mode 100644 wpimath/src/main/native/include/frc/apriltag/ApriltagUtil.h diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index 54445d3600c..e8257daf01d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -125,6 +125,22 @@ public static native void discreteAlgebraicRiccatiEquation( */ public static native String serializeTrajectory(double[] elements); + /** + * Deserializes an Apriltag layout JSON into a double[] of elements. + * + * @param json The JSON containing the serialized trajectory. + * @return A double array with the trajectory states. + */ + public static native double[] deserializeApriltagLayout(String json); + + /** + * Serializes the Apriltag layout into a JSON string. + * + * @param elements The elements of the layout. + * @return A JSON containing the serialized layout. + */ + public static native String serializeApriltagLayout(double[] elements); + public static class Helper { private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true); diff --git a/wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagFieldLayout.java b/wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagFieldLayout.java new file mode 100644 index 00000000000..72872a708ee --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagFieldLayout.java @@ -0,0 +1,34 @@ +package edu.wpi.first.math.apriltag; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; + +import java.util.HashMap; +import java.util.Map; + +public class ApriltagFieldLayout { + private final Map m_tags = new HashMap<>(); + private boolean m_mirror = false; + + public ApriltagFieldLayout(Map tags) { + // To ensure the underlying semantics don't change with what kind of map is passed in + m_tags.putAll(tags); + } + + public void setShouldMirror(boolean mirror) { + m_mirror = mirror; + } + + public Map getTags() { + return m_tags; + } + + public Pose3d getTag(int id) { + Pose3d tag = m_tags.get(id); + if(m_mirror) { + tag = tag.relativeTo(new Pose3d(new Translation3d(16.4592, 8.2296, 0), new Rotation3d(0, 0, 180))); + } + return tag; + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagUtil.java b/wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagUtil.java new file mode 100644 index 00000000000..9f84f77d002 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagUtil.java @@ -0,0 +1,83 @@ +package edu.wpi.first.math.apriltag; + +import edu.wpi.first.math.WPIMathJNI; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; + +import java.util.HashMap; +import java.util.Map; + +public class ApriltagUtil { + /** + * Creates an Apriltag layout from a double[] of elements. + * + * @param elements A double[] containing the raw elements of the Apriltag layout. + * @return An Apriltag layout created from the elements. + */ + private static ApriltagFieldLayout createApriltagFieldLayoutFromElements(double[] elements) { + // Make sure that the elements have the correct length. + if (elements.length % 7 != 0) { + throw new ApriltagLayoutSerializationException( + "An error occurred when converting trajectory elements into a trajectory."); + } + + // Create a list of states from the elements. + Map apriltagLayout = new HashMap<>(); + for (int i = 0; i < elements.length; i += 7) { + apriltagLayout.put( + (int) elements[i], + new Pose3d( + elements[i + 1], + elements[i + 2], + elements[i + 3], + new Rotation3d(new Quaternion( + elements[i + 4], + elements[i + 5], + elements[i + 6], + elements[i + 7] + )) + ) + ); + } + return new ApriltagFieldLayout(apriltagLayout); + } + + /** + * Returns a double[] of elements from the given Apriltag layout. + * + * @param apriltagFieldLayout The Apriltag field layout to retrieve raw elements from. + * @return A double[] of elements from the given trajectory. + */ + private static double[] getElementsFromApriltagFieldLayout(ApriltagFieldLayout apriltagFieldLayout) { + // Create a double[] of elements and fill it from the trajectory states. + double[] elements = new double[apriltagFieldLayout.getTags().size() * 7]; + + for (int i = 0; i < apriltagFieldLayout.getTags().size() * 7; i += 7) { + var entry = apriltagFieldLayout.getTags().entrySet().stream().toList().get(i / 7); + elements[i] = entry.getKey(); + elements[i + 1] = entry.getValue().getX(); + elements[i + 2] = entry.getValue().getY(); + elements[i + 3] = entry.getValue().getZ(); + elements[i + 4] = entry.getValue().getRotation().getQuaternion().getW(); + elements[i + 5] = entry.getValue().getRotation().getQuaternion().getX(); + elements[i + 6] = entry.getValue().getRotation().getQuaternion().getY(); + elements[i + 7] = entry.getValue().getRotation().getQuaternion().getZ(); + } + return elements; + } + + public static ApriltagFieldLayout deserializeApriltagFieldLayout(String json) { + return createApriltagFieldLayoutFromElements(WPIMathJNI.deserializeApriltagLayout(json)); + } + + public static String serializeApriltagFieldLayout(ApriltagFieldLayout apriltagFieldLayout) { + return WPIMathJNI.serializeApriltagLayout(getElementsFromApriltagFieldLayout(apriltagFieldLayout)); + } + + public static class ApriltagLayoutSerializationException extends RuntimeException { + public ApriltagLayoutSerializationException(String message) { + super(message); + } + } +} diff --git a/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp b/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp new file mode 100644 index 00000000000..bb89cb7997c --- /dev/null +++ b/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp @@ -0,0 +1,32 @@ +// 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/ApriltagFieldLayout.h" + +#include +#include "frc/geometry/Pose3d.h" + +using namespace frc; + +frc::Pose3d ApriltagFieldLayout::GetTagPose(int id) const { + auto returnPose = Pose3d{}; + for(Apriltag tag : m_apriltags) { + if(tag.id == id) { + returnPose = tag.pose; + } + } + return returnPose; +} + +void frc::to_json(wpi::json& json, const ApriltagFieldLayout::Apriltag& apriltag) { + json = wpi::json{ + {"id", apriltag.id}, + {"pose", apriltag.pose} + }; +} + +void frc::from_json(const wpi::json& json, ApriltagFieldLayout::Apriltag& apriltag) { + apriltag.id = json.at("id").get(); + apriltag.pose = json.at("pose").get(); +} \ No newline at end of file diff --git a/wpimath/src/main/native/cpp/apriltag/ApriltagUtil.cpp b/wpimath/src/main/native/cpp/apriltag/ApriltagUtil.cpp new file mode 100644 index 00000000000..2a622ba0fe7 --- /dev/null +++ b/wpimath/src/main/native/cpp/apriltag/ApriltagUtil.cpp @@ -0,0 +1,54 @@ +// 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/ApriltagUtil.h" + +#include +#include "frc/apriltag/ApriltagFieldLayout.h" + +#include +#include +#include +#include +#include + +using namespace frc; + +void ApriltagUtil::ToJson(const ApriltagFieldLayout& apriltagFieldLayout, + std::string_view path) { + std::error_code error_code; + + wpi::raw_fd_ostream output{path, error_code}; + if (error_code) { + throw std::runtime_error(fmt::format("Cannot open file: {}", path)); + } + + wpi::json json = apriltagFieldLayout.GetTags(); + output << json; + output.flush(); +} + +ApriltagFieldLayout ApriltagUtil::FromJson(std::string_view path) { + std::error_code error_code; + + wpi::raw_fd_istream input{path, error_code}; + if (error_code) { + throw std::runtime_error(fmt::format("Cannot open file: {}", path)); + } + + wpi::json json; + input >> json; + + return ApriltagFieldLayout{json.get>()}; +} + +std::string ApriltagUtil::SerializeApriltagLayout(const ApriltagFieldLayout& apriltagFieldLayout) { + wpi::json json = apriltagFieldLayout.GetTags(); + return json.dump(); +} + +ApriltagFieldLayout ApriltagUtil::DeserializeApriltagLayout(std::string_view jsonStr) { + wpi::json json = wpi::json::parse(jsonStr); + return ApriltagFieldLayout{json.get>()}; +} diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index b036833fac4..5998d66230d 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -13,7 +13,13 @@ #include "Eigen/QR" #include "drake/math/discrete_algebraic_riccati_equation.h" #include "edu_wpi_first_math_WPIMathJNI.h" +#include "frc/apriltag/ApriltagFieldLayout.h" +#include "frc/apriltag/ApriltagUtil.h" +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Rotation3d.h" #include "frc/trajectory/TrajectoryUtil.h" +#include "units/angle.h" +#include "units/length.h" #include "unsupported/Eigen/MatrixFunctions" using namespace wpi::java; @@ -92,6 +98,51 @@ frc::Trajectory CreateTrajectoryFromElements(wpi::span elements) { return frc::Trajectory(states); } +std::vector GetElementsFromApriltagLayout( + const frc::ApriltagFieldLayout& apriltagFieldLayout) { + std::vector elements; + elements.reserve(apriltagFieldLayout.GetTags().size() * 8); + + for (auto&& apriltag : apriltagFieldLayout.GetTags()) { + elements.push_back(apriltag.id); + elements.push_back(apriltag.pose.X().value()); + elements.push_back(apriltag.pose.Y().value()); + elements.push_back(apriltag.pose.Z().value()); + elements.push_back(apriltag.pose.Rotation().GetQuaternion().W()); + elements.push_back(apriltag.pose.Rotation().GetQuaternion().X()); + elements.push_back(apriltag.pose.Rotation().GetQuaternion().Y()); + elements.push_back(apriltag.pose.Rotation().GetQuaternion().Z()); + } + + return elements; +} + +frc::ApriltagFieldLayout CreateApriltagLayoutFromElements(wpi::span elements) { + // Make sure that the elements have the correct length. + assert(elements.size() % 8 == 0); + + // Create a vector of states from the elements. + std::vector apriltags; + apriltags.reserve(elements.size() / 8); + + for (size_t i = 0; i < elements.size(); i += 8) { + apriltags.emplace_back(frc::ApriltagFieldLayout::Apriltag{ + static_cast(elements[0]), + frc::Pose3d{units::meter_t{elements[i + 1]}, + units::meter_t{elements[i + 2]}, + units::meter_t{elements[i + 3]}, + frc::Rotation3d{frc::Quaternion{ + elements[i + 4], + elements[i + 5], + elements[i + 6], + elements[i + 7]}}}}); + } + + return frc::ApriltagFieldLayout(apriltags); +} + + + extern "C" { /* @@ -307,4 +358,54 @@ Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory } } +/* + * Class: edu_wpi_first_math_WPIMathJNI + * Method: deserializeApriltagLayout + * Signature: (Ljava/lang/String;)[D + */ +JNIEXPORT jdoubleArray JNICALL +Java_edu_wpi_first_math_WPIMathJNI_deserializeApriltagLayout + (JNIEnv* env, jclass, jstring json) +{ + try { + auto apriltagFieldLayout = frc::ApriltagUtil::DeserializeApriltagLayout( + JStringRef{env, json}.c_str()); + std::vector elements = GetElementsFromApriltagLayout(apriltagFieldLayout); + return MakeJDoubleArray(env, elements); + } catch (std::exception& e) { + jclass cls = env->FindClass( + "edu/wpi/first/math/apriltag/ApriltagUtil$" + "ApriltagLayoutSerializationException"); + if (cls) { + env->ThrowNew(cls, e.what()); + } + return nullptr; + } +} + +/* + * Class: edu_wpi_first_math_WPIMathJNI + * Method: serializeApriltagLayout + * Signature: ([D)Ljava/lang/String; + */ +JNIEXPORT jstring JNICALL +Java_edu_wpi_first_math_WPIMathJNI_serializeApriltagLayout + (JNIEnv* env, jclass, jdoubleArray elements) +{ + try { + auto apriltagFieldLayout = + CreateApriltagLayoutFromElements(JDoubleArrayRef{env, elements}); + return MakeJString(env, + frc::ApriltagUtil::SerializeApriltagLayout(apriltagFieldLayout)); + } catch (std::exception& e) { + jclass cls = env->FindClass( + "edu/wpi/first/math/apriltag/ApriltagUtil$" + "ApriltagLayoutSerializationException"); + if (cls) { + env->ThrowNew(cls, e.what()); + } + return nullptr; + } +} + } // extern "C" diff --git a/wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h b/wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h new file mode 100644 index 00000000000..1edb3f0a797 --- /dev/null +++ b/wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h @@ -0,0 +1,54 @@ +// 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 +#include +#include + +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Transform2d.h" +#include "units/acceleration.h" +#include "units/curvature.h" +#include "units/time.h" +#include "units/velocity.h" + +namespace wpi { +class json; +} // namespace wpi + +namespace frc { +class WPILIB_DLLEXPORT ApriltagFieldLayout { + public: + struct WPILIB_DLLEXPORT Apriltag { + int id; + + Pose3d pose = Pose3d(); + }; + + ApriltagFieldLayout() = default; + + explicit ApriltagFieldLayout(const std::vector& apriltags); + + const std::vector& GetTags() const { return m_apriltags; }; + + Pose3d GetTagPose(int id) const; + + void setShouldMirror(bool mirror) { m_mirror = mirror; } + + private: + std::vector m_apriltags; + bool m_mirror; +}; + +WPILIB_DLLEXPORT +void to_json(wpi::json& json, const ApriltagFieldLayout::Apriltag& apriltag); + +WPILIB_DLLEXPORT +void from_json(const wpi::json& json, ApriltagFieldLayout::Apriltag& apriltag); +} \ No newline at end of file diff --git a/wpimath/src/main/native/include/frc/apriltag/ApriltagUtil.h b/wpimath/src/main/native/include/frc/apriltag/ApriltagUtil.h new file mode 100644 index 00000000000..6d6bec9562f --- /dev/null +++ b/wpimath/src/main/native/include/frc/apriltag/ApriltagUtil.h @@ -0,0 +1,54 @@ +// 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 +#include + +#include + +#include "frc/apriltag/ApriltagFieldLayout.h" + +namespace frc { +class WPILIB_DLLEXPORT ApriltagUtil { + public: + ApriltagUtil() = delete; + + /** + * Exports a Apriltag Layout JSON file. + * + * @param apriltag the Apriltag layout to export + * @param path the path of the file to export to + */ + static void ToJson(const ApriltagFieldLayout& apriltagFieldLayout, + std::string_view path); + /** + * Imports a Apriltag Layout JSON file. + * + * @param path The path of the json file to import from. + * + * @return The Apriltag layout represented by the file. + */ + static ApriltagFieldLayout FromJson(std::string_view path); + + /** + * Deserializes a Apriltag layout JSON. + * + * @param apriltag the Apriltag layout to export + * + * @return the string containing the serialized JSON + */ + static std::string SerializeApriltagLayout(const ApriltagFieldLayout& apriltagFieldLayout); + + /** + * Serializes a Apriltag layout JSON. + * + * @param jsonStr the string containing the serialized JSON + * + * @return the Apriltag layout represented by the JSON + */ + static ApriltagFieldLayout DeserializeApriltagLayout(std::string_view jsonStr); +}; +} // namespace frc From 442b7be27ec41ba051d42414f1cb2fb9417421da Mon Sep 17 00:00:00 2001 From: brennenputh Date: Fri, 16 Sep 2022 22:31:21 -0400 Subject: [PATCH 02/73] Implement pose mirroring on C++ side --- wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp b/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp index bb89cb7997c..567837bf58e 100644 --- a/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp +++ b/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp @@ -6,6 +6,8 @@ #include #include "frc/geometry/Pose3d.h" +#include "units/angle.h" +#include "units/length.h" using namespace frc; @@ -16,6 +18,9 @@ frc::Pose3d ApriltagFieldLayout::GetTagPose(int id) const { returnPose = tag.pose; } } + if(m_mirror) { + returnPose = returnPose.RelativeTo(Pose3d{units::inch_t{54}, units::inch_t{27}, returnPose.Z(), Rotation3d{units::radian_t{0}, units::radian_t{0}, units::radian_t{180}}}); + } return returnPose; } From 1b23a91ac006e86ebc31f745ca138b4742d0ded3 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sat, 17 Sep 2022 07:08:34 -0400 Subject: [PATCH 03/73] Fix longhand typing for some variables --- wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp | 4 ++-- .../main/native/include/frc/apriltag/ApriltagFieldLayout.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp b/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp index 567837bf58e..cce941e3d49 100644 --- a/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp +++ b/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp @@ -12,8 +12,8 @@ using namespace frc; frc::Pose3d ApriltagFieldLayout::GetTagPose(int id) const { - auto returnPose = Pose3d{}; - for(Apriltag tag : m_apriltags) { + Pose3d returnPose; + for(auto& tag : m_apriltags) { if(tag.id == id) { returnPose = tag.pose; } diff --git a/wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h b/wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h index 1edb3f0a797..138852ae209 100644 --- a/wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h +++ b/wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h @@ -28,7 +28,7 @@ class WPILIB_DLLEXPORT ApriltagFieldLayout { struct WPILIB_DLLEXPORT Apriltag { int id; - Pose3d pose = Pose3d(); + Pose3d pose; }; ApriltagFieldLayout() = default; From 908d086d517b7abd53e6ef8b42ae0fac3af0a76b Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sat, 17 Sep 2022 07:10:47 -0400 Subject: [PATCH 04/73] Fix RelativeTo call in GetTagPose --- wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp b/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp index cce941e3d49..a133211afa5 100644 --- a/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp +++ b/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp @@ -19,7 +19,7 @@ frc::Pose3d ApriltagFieldLayout::GetTagPose(int id) const { } } if(m_mirror) { - returnPose = returnPose.RelativeTo(Pose3d{units::inch_t{54}, units::inch_t{27}, returnPose.Z(), Rotation3d{units::radian_t{0}, units::radian_t{0}, units::radian_t{180}}}); + returnPose = returnPose.RelativeTo(Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}}); } return returnPose; } From cf3537dbd485a43762fa36365c7665cf3c8ae406 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sat, 17 Sep 2022 07:12:13 -0400 Subject: [PATCH 05/73] Fix SetShouldMirror function --- wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp | 4 ++++ .../main/native/include/frc/apriltag/ApriltagFieldLayout.h | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp b/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp index a133211afa5..a49de83ec86 100644 --- a/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp +++ b/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp @@ -24,6 +24,10 @@ frc::Pose3d ApriltagFieldLayout::GetTagPose(int id) const { return returnPose; } +void ApriltagFieldLayout::SetShouldMirror(bool mirror) { + m_mirror = mirror; +} + void frc::to_json(wpi::json& json, const ApriltagFieldLayout::Apriltag& apriltag) { json = wpi::json{ {"id", apriltag.id}, diff --git a/wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h b/wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h index 138852ae209..f0b63b29364 100644 --- a/wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h +++ b/wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h @@ -39,7 +39,7 @@ class WPILIB_DLLEXPORT ApriltagFieldLayout { Pose3d GetTagPose(int id) const; - void setShouldMirror(bool mirror) { m_mirror = mirror; } + void SetShouldMirror(bool mirror); private: std::vector m_apriltags; From ada36d158d8802baa999ec433de68185a9c90609 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sat, 17 Sep 2022 07:22:31 -0400 Subject: [PATCH 06/73] Change capitalization of Apriltag to AprilTag --- .../java/edu/wpi/first/math/WPIMathJNI.java | 10 ++--- ...ldLayout.java => AprilTagFieldLayout.java} | 4 +- .../{ApriltagUtil.java => AprilTagUtil.java} | 38 +++++++++--------- ...ieldLayout.cpp => AprilTagFieldLayout.cpp} | 10 ++--- .../{ApriltagUtil.cpp => AprilTagUtil.cpp} | 16 ++++---- .../src/main/native/cpp/jni/WPIMathJNI.cpp | 40 +++++++++---------- ...tagFieldLayout.h => AprilTagFieldLayout.h} | 16 ++++---- .../{ApriltagUtil.h => AprilTagUtil.h} | 30 +++++++------- 8 files changed, 82 insertions(+), 82 deletions(-) rename wpimath/src/main/java/edu/wpi/first/math/apriltag/{ApriltagFieldLayout.java => AprilTagFieldLayout.java} (90%) rename wpimath/src/main/java/edu/wpi/first/math/apriltag/{ApriltagUtil.java => AprilTagUtil.java} (64%) rename wpimath/src/main/native/cpp/apriltag/{ApriltagFieldLayout.cpp => AprilTagFieldLayout.cpp} (71%) rename wpimath/src/main/native/cpp/apriltag/{ApriltagUtil.cpp => AprilTagUtil.cpp} (68%) rename wpimath/src/main/native/include/frc/apriltag/{ApriltagFieldLayout.h => AprilTagFieldLayout.h} (66%) rename wpimath/src/main/native/include/frc/apriltag/{ApriltagUtil.h => AprilTagUtil.h} (50%) diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index 82be224815f..4b63eaf15f4 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -139,20 +139,20 @@ public static native void rankUpdate( double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular); /** - * Deserializes an Apriltag layout JSON into a double[] of elements. + * Deserializes an AprilTag layout JSON into a double[] of elements. * * @param json The JSON containing the serialized trajectory. - * @return A double array with the trajectory states. + * @return A double array with the Apriltag layout. */ - public static native double[] deserializeApriltagLayout(String json); + public static native double[] deserializeAprilTagLayout(String json); /** - * Serializes the Apriltag layout into a JSON string. + * Serializes the AprilTag layout into a JSON string. * * @param elements The elements of the layout. * @return A JSON containing the serialized layout. */ - public static native String serializeApriltagLayout(double[] elements); + public static native String serializeAprilTagLayout(double[] elements); public static class Helper { private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true); diff --git a/wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagFieldLayout.java b/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagFieldLayout.java similarity index 90% rename from wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagFieldLayout.java rename to wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagFieldLayout.java index 72872a708ee..8e10b697680 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagFieldLayout.java +++ b/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagFieldLayout.java @@ -7,11 +7,11 @@ import java.util.HashMap; import java.util.Map; -public class ApriltagFieldLayout { +public class AprilTagFieldLayout { private final Map m_tags = new HashMap<>(); private boolean m_mirror = false; - public ApriltagFieldLayout(Map tags) { + public AprilTagFieldLayout(Map tags) { // To ensure the underlying semantics don't change with what kind of map is passed in m_tags.putAll(tags); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagUtil.java b/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java similarity index 64% rename from wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagUtil.java rename to wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java index 9f84f77d002..e230a175032 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java @@ -8,17 +8,17 @@ import java.util.HashMap; import java.util.Map; -public class ApriltagUtil { +public class AprilTagUtil { /** - * Creates an Apriltag layout from a double[] of elements. + * Creates an AprilTag layout from a double[] of elements. * - * @param elements A double[] containing the raw elements of the Apriltag layout. - * @return An Apriltag layout created from the elements. + * @param elements A double[] containing the raw elements of the AprilTag layout. + * @return An AprilTag layout created from the elements. */ - private static ApriltagFieldLayout createApriltagFieldLayoutFromElements(double[] elements) { + private static AprilTagFieldLayout createAprilTagFieldLayoutFromElements(double[] elements) { // Make sure that the elements have the correct length. if (elements.length % 7 != 0) { - throw new ApriltagLayoutSerializationException( + throw new AprilTagLayoutSerializationException( "An error occurred when converting trajectory elements into a trajectory."); } @@ -40,21 +40,21 @@ private static ApriltagFieldLayout createApriltagFieldLayoutFromElements(double[ ) ); } - return new ApriltagFieldLayout(apriltagLayout); + return new AprilTagFieldLayout(apriltagLayout); } /** - * Returns a double[] of elements from the given Apriltag layout. + * Returns a double[] of elements from the given AprilTag layout. * - * @param apriltagFieldLayout The Apriltag field layout to retrieve raw elements from. + * @param aprilTagFieldLayout The AprilTag field layout to retrieve raw elements from. * @return A double[] of elements from the given trajectory. */ - private static double[] getElementsFromApriltagFieldLayout(ApriltagFieldLayout apriltagFieldLayout) { + private static double[] getElementsFromAprilTagFieldLayout(AprilTagFieldLayout aprilTagFieldLayout) { // Create a double[] of elements and fill it from the trajectory states. - double[] elements = new double[apriltagFieldLayout.getTags().size() * 7]; + double[] elements = new double[aprilTagFieldLayout.getTags().size() * 7]; - for (int i = 0; i < apriltagFieldLayout.getTags().size() * 7; i += 7) { - var entry = apriltagFieldLayout.getTags().entrySet().stream().toList().get(i / 7); + for (int i = 0; i < aprilTagFieldLayout.getTags().size() * 7; i += 7) { + var entry = aprilTagFieldLayout.getTags().entrySet().stream().toList().get(i / 7); elements[i] = entry.getKey(); elements[i + 1] = entry.getValue().getX(); elements[i + 2] = entry.getValue().getY(); @@ -67,16 +67,16 @@ private static double[] getElementsFromApriltagFieldLayout(ApriltagFieldLayout a return elements; } - public static ApriltagFieldLayout deserializeApriltagFieldLayout(String json) { - return createApriltagFieldLayoutFromElements(WPIMathJNI.deserializeApriltagLayout(json)); + public static AprilTagFieldLayout deserializeAprilTagFieldLayout(String json) { + return createAprilTagFieldLayoutFromElements(WPIMathJNI.deserializeAprilTagLayout(json)); } - public static String serializeApriltagFieldLayout(ApriltagFieldLayout apriltagFieldLayout) { - return WPIMathJNI.serializeApriltagLayout(getElementsFromApriltagFieldLayout(apriltagFieldLayout)); + public static String serializeAprilTagFieldLayout(AprilTagFieldLayout aprilTagFieldLayout) { + return WPIMathJNI.serializeAprilTagLayout(getElementsFromAprilTagFieldLayout(aprilTagFieldLayout)); } - public static class ApriltagLayoutSerializationException extends RuntimeException { - public ApriltagLayoutSerializationException(String message) { + public static class AprilTagLayoutSerializationException extends RuntimeException { + public AprilTagLayoutSerializationException(String message) { super(message); } } diff --git a/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp b/wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp similarity index 71% rename from wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp rename to wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index a49de83ec86..2010ce544ea 100644 --- a/wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp +++ b/wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -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. -#include "frc/apriltag/ApriltagFieldLayout.h" +#include "frc/apriltag/AprilTagFieldLayout.h" #include #include "frc/geometry/Pose3d.h" @@ -11,7 +11,7 @@ using namespace frc; -frc::Pose3d ApriltagFieldLayout::GetTagPose(int id) const { +frc::Pose3d AprilTagFieldLayout::GetTagPose(int id) const { Pose3d returnPose; for(auto& tag : m_apriltags) { if(tag.id == id) { @@ -24,18 +24,18 @@ frc::Pose3d ApriltagFieldLayout::GetTagPose(int id) const { return returnPose; } -void ApriltagFieldLayout::SetShouldMirror(bool mirror) { +void AprilTagFieldLayout::SetShouldMirror(bool mirror) { m_mirror = mirror; } -void frc::to_json(wpi::json& json, const ApriltagFieldLayout::Apriltag& apriltag) { +void frc::to_json(wpi::json& json, const AprilTagFieldLayout::AprilTag& apriltag) { json = wpi::json{ {"id", apriltag.id}, {"pose", apriltag.pose} }; } -void frc::from_json(const wpi::json& json, ApriltagFieldLayout::Apriltag& apriltag) { +void frc::from_json(const wpi::json& json, AprilTagFieldLayout::AprilTag& apriltag) { apriltag.id = json.at("id").get(); apriltag.pose = json.at("pose").get(); } \ No newline at end of file diff --git a/wpimath/src/main/native/cpp/apriltag/ApriltagUtil.cpp b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp similarity index 68% rename from wpimath/src/main/native/cpp/apriltag/ApriltagUtil.cpp rename to wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp index 2a622ba0fe7..26ec3f8cc68 100644 --- a/wpimath/src/main/native/cpp/apriltag/ApriltagUtil.cpp +++ b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp @@ -2,10 +2,10 @@ // 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/ApriltagUtil.h" +#include "frc/apriltag/AprilTagUtil.h" #include -#include "frc/apriltag/ApriltagFieldLayout.h" +#include "frc/apriltag/AprilTagFieldLayout.h" #include #include @@ -15,7 +15,7 @@ using namespace frc; -void ApriltagUtil::ToJson(const ApriltagFieldLayout& apriltagFieldLayout, +void AprilTagUtil::ToJson(const AprilTagFieldLayout& apriltagFieldLayout, std::string_view path) { std::error_code error_code; @@ -29,7 +29,7 @@ void ApriltagUtil::ToJson(const ApriltagFieldLayout& apriltagFieldLayout, output.flush(); } -ApriltagFieldLayout ApriltagUtil::FromJson(std::string_view path) { +AprilTagFieldLayout AprilTagUtil::FromJson(std::string_view path) { std::error_code error_code; wpi::raw_fd_istream input{path, error_code}; @@ -40,15 +40,15 @@ ApriltagFieldLayout ApriltagUtil::FromJson(std::string_view path) { wpi::json json; input >> json; - return ApriltagFieldLayout{json.get>()}; + return AprilTagFieldLayout{json.get>()}; } -std::string ApriltagUtil::SerializeApriltagLayout(const ApriltagFieldLayout& apriltagFieldLayout) { +std::string AprilTagUtil::SerializeAprilTagLayout(const AprilTagFieldLayout& apriltagFieldLayout) { wpi::json json = apriltagFieldLayout.GetTags(); return json.dump(); } -ApriltagFieldLayout ApriltagUtil::DeserializeApriltagLayout(std::string_view jsonStr) { +AprilTagFieldLayout AprilTagUtil::DeserializeAprilTagLayout(std::string_view jsonStr) { wpi::json json = wpi::json::parse(jsonStr); - return ApriltagFieldLayout{json.get>()}; + return AprilTagFieldLayout{json.get>()}; } diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index ddb2888b765..4feabeab708 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -14,8 +14,8 @@ #include "Eigen/QR" #include "drake/math/discrete_algebraic_riccati_equation.h" #include "edu_wpi_first_math_WPIMathJNI.h" -#include "frc/apriltag/ApriltagFieldLayout.h" -#include "frc/apriltag/ApriltagUtil.h" +#include "frc/apriltag/AprilTagFieldLayout.h" +#include "frc/apriltag/AprilTagUtil.h" #include "frc/geometry/Pose3d.h" #include "frc/geometry/Rotation3d.h" #include "frc/trajectory/TrajectoryUtil.h" @@ -99,8 +99,8 @@ frc::Trajectory CreateTrajectoryFromElements(wpi::span elements) { return frc::Trajectory(states); } -std::vector GetElementsFromApriltagLayout( - const frc::ApriltagFieldLayout& apriltagFieldLayout) { +std::vector GetElementsFromAprilTagLayout( + const frc::AprilTagFieldLayout& apriltagFieldLayout) { std::vector elements; elements.reserve(apriltagFieldLayout.GetTags().size() * 8); @@ -118,16 +118,16 @@ std::vector GetElementsFromApriltagLayout( return elements; } -frc::ApriltagFieldLayout CreateApriltagLayoutFromElements(wpi::span elements) { +frc::AprilTagFieldLayout CreateAprilTagLayoutFromElements(wpi::span elements) { // Make sure that the elements have the correct length. assert(elements.size() % 8 == 0); // Create a vector of states from the elements. - std::vector apriltags; + std::vector apriltags; apriltags.reserve(elements.size() / 8); for (size_t i = 0; i < elements.size(); i += 8) { - apriltags.emplace_back(frc::ApriltagFieldLayout::Apriltag{ + apriltags.emplace_back(frc::AprilTagFieldLayout::AprilTag{ static_cast(elements[0]), frc::Pose3d{units::meter_t{elements[i + 1]}, units::meter_t{elements[i + 2]}, @@ -139,7 +139,7 @@ frc::ApriltagFieldLayout CreateApriltagLayoutFromElements(wpi::span elements = GetElementsFromApriltagLayout(apriltagFieldLayout); + std::vector elements = GetElementsFromAprilTagLayout(apriltagFieldLayout); return MakeJDoubleArray(env, elements); } catch (std::exception& e) { jclass cls = env->FindClass( - "edu/wpi/first/math/apriltag/ApriltagUtil$" - "ApriltagLayoutSerializationException"); + "edu/wpi/first/math/apriltag/AprilTagUtil$" + "AprilTagLayoutSerializationException"); if (cls) { env->ThrowNew(cls, e.what()); } @@ -386,22 +386,22 @@ Java_edu_wpi_first_math_WPIMathJNI_deserializeApriltagLayout /* * Class: edu_wpi_first_math_WPIMathJNI - * Method: serializeApriltagLayout + * Method: serializeAprilTagLayout * Signature: ([D)Ljava/lang/String; */ JNIEXPORT jstring JNICALL -Java_edu_wpi_first_math_WPIMathJNI_serializeApriltagLayout +Java_edu_wpi_first_math_WPIMathJNI_serializeAprilTagLayout (JNIEnv* env, jclass, jdoubleArray elements) { try { auto apriltagFieldLayout = - CreateApriltagLayoutFromElements(JDoubleArrayRef{env, elements}); + CreateAprilTagLayoutFromElements(JDoubleArrayRef{env, elements}); return MakeJString(env, - frc::ApriltagUtil::SerializeApriltagLayout(apriltagFieldLayout)); + frc::AprilTagUtil::SerializeAprilTagLayout(apriltagFieldLayout)); } catch (std::exception& e) { jclass cls = env->FindClass( - "edu/wpi/first/math/apriltag/ApriltagUtil$" - "ApriltagLayoutSerializationException"); + "edu/wpi/first/math/apriltag/AprilTagUtil$" + "AprilTagLayoutSerializationException"); if (cls) { env->ThrowNew(cls, e.what()); } diff --git a/wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h b/wpimath/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h similarity index 66% rename from wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h rename to wpimath/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index f0b63b29364..b4ac44dcaf1 100644 --- a/wpimath/src/main/native/include/frc/apriltag/ApriltagFieldLayout.h +++ b/wpimath/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -23,32 +23,32 @@ class json; } // namespace wpi namespace frc { -class WPILIB_DLLEXPORT ApriltagFieldLayout { +class WPILIB_DLLEXPORT AprilTagFieldLayout { public: - struct WPILIB_DLLEXPORT Apriltag { + struct WPILIB_DLLEXPORT AprilTag { int id; Pose3d pose; }; - ApriltagFieldLayout() = default; + AprilTagFieldLayout() = default; - explicit ApriltagFieldLayout(const std::vector& apriltags); + explicit AprilTagFieldLayout(const std::vector& apriltags); - const std::vector& GetTags() const { return m_apriltags; }; + const std::vector& GetTags() const { return m_apriltags; }; Pose3d GetTagPose(int id) const; void SetShouldMirror(bool mirror); private: - std::vector m_apriltags; + std::vector m_apriltags; bool m_mirror; }; WPILIB_DLLEXPORT -void to_json(wpi::json& json, const ApriltagFieldLayout::Apriltag& apriltag); +void to_json(wpi::json& json, const AprilTagFieldLayout::AprilTag& apriltag); WPILIB_DLLEXPORT -void from_json(const wpi::json& json, ApriltagFieldLayout::Apriltag& apriltag); +void from_json(const wpi::json& json, AprilTagFieldLayout::AprilTag& apriltag); } \ No newline at end of file diff --git a/wpimath/src/main/native/include/frc/apriltag/ApriltagUtil.h b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h similarity index 50% rename from wpimath/src/main/native/include/frc/apriltag/ApriltagUtil.h rename to wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h index 6d6bec9562f..acca443edcf 100644 --- a/wpimath/src/main/native/include/frc/apriltag/ApriltagUtil.h +++ b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h @@ -9,46 +9,46 @@ #include -#include "frc/apriltag/ApriltagFieldLayout.h" +#include "frc/apriltag/AprilTagFieldLayout.h" namespace frc { -class WPILIB_DLLEXPORT ApriltagUtil { +class WPILIB_DLLEXPORT AprilTagUtil { public: - ApriltagUtil() = delete; + AprilTagUtil() = delete; /** - * Exports a Apriltag Layout JSON file. + * Exports a AprilTag Layout JSON file. * - * @param apriltag the Apriltag layout to export + * @param apriltag the AprilTag layout to export * @param path the path of the file to export to */ - static void ToJson(const ApriltagFieldLayout& apriltagFieldLayout, + static void ToJson(const AprilTagFieldLayout& apriltagFieldLayout, std::string_view path); /** - * Imports a Apriltag Layout JSON file. + * Imports a AprilTag Layout JSON file. * * @param path The path of the json file to import from. * - * @return The Apriltag layout represented by the file. + * @return The AprilTag layout represented by the file. */ - static ApriltagFieldLayout FromJson(std::string_view path); + static AprilTagFieldLayout FromJson(std::string_view path); /** - * Deserializes a Apriltag layout JSON. + * Deserializes a AprilTag layout JSON. * - * @param apriltag the Apriltag layout to export + * @param apriltag the AprilTag layout to export * * @return the string containing the serialized JSON */ - static std::string SerializeApriltagLayout(const ApriltagFieldLayout& apriltagFieldLayout); + static std::string SerializeAprilTagLayout(const AprilTagFieldLayout& apriltagFieldLayout); /** - * Serializes a Apriltag layout JSON. + * Serializes a AprilTag layout JSON. * * @param jsonStr the string containing the serialized JSON * - * @return the Apriltag layout represented by the JSON + * @return the AprilTag layout represented by the JSON */ - static ApriltagFieldLayout DeserializeApriltagLayout(std::string_view jsonStr); + static AprilTagFieldLayout DeserializeAprilTagLayout(std::string_view jsonStr); }; } // namespace frc From ad1c97c77fe5319cee3359b9fffcc9fc8aa256d5 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sat, 17 Sep 2022 11:13:48 -0400 Subject: [PATCH 07/73] Add test for serialization and deserialization of Apriltags --- .../cpp/apriltag/AprilTagFieldLayout.cpp | 10 ++++++++ .../src/main/native/cpp/geometry/Pose3d.cpp | 12 +++++++++ .../main/native/cpp/geometry/Rotation3d.cpp | 16 ++++++++++++ .../native/cpp/geometry/Translation3d.cpp | 13 ++++++++++ .../frc/apriltag/AprilTagFieldLayout.h | 17 +++++++++++++ .../main/native/include/frc/geometry/Pose3d.h | 6 +++++ .../native/include/frc/geometry/Rotation3d.h | 6 +++++ .../include/frc/geometry/Translation3d.h | 6 +++++ .../native/cpp/apriltag/AprilTagJsonTest.cpp | 25 +++++++++++++++++++ 9 files changed, 111 insertions(+) create mode 100644 wpimath/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp diff --git a/wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index 2010ce544ea..feb6940d9ff 100644 --- a/wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -11,6 +11,16 @@ using namespace frc; +bool AprilTagFieldLayout::AprilTag::operator==(const AprilTag &other) const { + return id == other.id && pose == other.pose; +} + +bool AprilTagFieldLayout::AprilTag::operator!=(const AprilTag &other) const { + return !operator==(other); +} + +AprilTagFieldLayout::AprilTagFieldLayout(const std::vector& apriltags): m_apriltags(apriltags) {} + frc::Pose3d AprilTagFieldLayout::GetTagPose(int id) const { Pose3d returnPose; for(auto& tag : m_apriltags) { diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index 422b6a61946..5603ee142c5 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -5,6 +5,7 @@ #include "frc/geometry/Pose3d.h" #include +#include "wpi/json.h" using namespace frc; @@ -137,3 +138,14 @@ 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(), + json.at("rotation").get()}; +} + diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp index 77edf2d0dd6..693cbb83ca0 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -10,7 +10,9 @@ #include "Eigen/Core" #include "Eigen/QR" +#include "units/angle.h" #include "units/math.h" +#include "wpi/json.h" using namespace frc; @@ -172,3 +174,17 @@ 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{ + {"yaw", rotation.X().value()}, + {"pitch", rotation.Y().value()}, + {"roll", rotation.Z().value()} + }; +} + +void frc::from_json(const wpi::json& json, Rotation3d& rotation) { + rotation = Rotation3d{units::degree_t{json.at("yaw").get()}, + units::degree_t{json.at("pitch").get()}, + units::degree_t{json.at("roll").get()}}; +} diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp index 9a252546524..e3b63ca3880 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp @@ -4,7 +4,9 @@ #include "frc/geometry/Translation3d.h" +#include "units/length.h" #include "units/math.h" +#include "wpi/json.h" using namespace frc; @@ -69,3 +71,14 @@ 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()}, + units::meter_t{json.at("y").get()}, + units::meter_t{json.at("z").get()}}; +} diff --git a/wpimath/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpimath/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index b4ac44dcaf1..1d47deb3b9f 100644 --- a/wpimath/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpimath/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -29,6 +29,23 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { int id; Pose3d pose; + + /** + * Checks equality between this State and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const AprilTag& other) const; + + /** + * Checks inequality between this State and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const AprilTag& other) const; + }; AprilTagFieldLayout() = default; diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 23c75d38a86..66b37502204 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -177,4 +177,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 diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index aada0e25f2d..b54a6fe50ad 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -162,4 +162,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 diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index 385a300c21b..1ac9c170c97 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -182,4 +182,10 @@ 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 diff --git a/wpimath/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp b/wpimath/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp new file mode 100644 index 00000000000..0df0185c8b6 --- /dev/null +++ b/wpimath/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp @@ -0,0 +1,25 @@ +// 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 +#include "frc/apriltag/AprilTagFieldLayout.h" +#include "frc/apriltag/AprilTagUtil.h" +#include "frc/geometry/Pose3d.h" +#include "frc/trajectory/TrajectoryConfig.h" +#include "frc/trajectory/TrajectoryUtil.h" +#include "gtest/gtest.h" + +using namespace frc; + +TEST(AprilTagJsonTest, DeserializeMatches) { + auto layout = AprilTagFieldLayout{std::vector{ + AprilTagFieldLayout::AprilTag{1, Pose3d{}}, + AprilTagFieldLayout::AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}} + }; + + AprilTagFieldLayout deserialized; + EXPECT_NO_THROW(deserialized = AprilTagUtil::DeserializeAprilTagLayout( + AprilTagUtil::SerializeAprilTagLayout(layout))); + EXPECT_EQ(layout.GetTags(), deserialized.GetTags()); +} From fd643623c71b4bbb894f8644fe87d6611bb84a15 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 10:53:34 -0400 Subject: [PATCH 08/73] Create JSON serialization test for Java --- .../wpi/first/math/apriltag/AprilTagUtil.java | 16 ++++++---- .../src/main/native/cpp/jni/WPIMathJNI.cpp | 6 ++-- .../first/math/apriltag/AprilTagJsonTest.java | 29 +++++++++++++++++++ 3 files changed, 41 insertions(+), 10 deletions(-) create mode 100644 wpimath/src/test/java/edu/wpi/first/math/apriltag/AprilTagJsonTest.java diff --git a/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java b/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java index e230a175032..34368420b43 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java @@ -5,6 +5,8 @@ import edu.wpi.first.math.geometry.Quaternion; import edu.wpi.first.math.geometry.Rotation3d; +import java.util.ArrayList; +import java.util.Arrays; import java.util.HashMap; import java.util.Map; @@ -17,14 +19,14 @@ public class AprilTagUtil { */ private static AprilTagFieldLayout createAprilTagFieldLayoutFromElements(double[] elements) { // Make sure that the elements have the correct length. - if (elements.length % 7 != 0) { + if (elements.length % 8 != 0) { throw new AprilTagLayoutSerializationException( - "An error occurred when converting trajectory elements into a trajectory."); + "An error occurred when converting AprilTag elements into a AprilTag layout."); } // Create a list of states from the elements. Map apriltagLayout = new HashMap<>(); - for (int i = 0; i < elements.length; i += 7) { + for (int i = 0; i < elements.length; i += 8) { apriltagLayout.put( (int) elements[i], new Pose3d( @@ -51,10 +53,11 @@ private static AprilTagFieldLayout createAprilTagFieldLayoutFromElements(double[ */ private static double[] getElementsFromAprilTagFieldLayout(AprilTagFieldLayout aprilTagFieldLayout) { // Create a double[] of elements and fill it from the trajectory states. - double[] elements = new double[aprilTagFieldLayout.getTags().size() * 7]; + double[] elements = new double[aprilTagFieldLayout.getTags().size() * 8]; - for (int i = 0; i < aprilTagFieldLayout.getTags().size() * 7; i += 7) { - var entry = aprilTagFieldLayout.getTags().entrySet().stream().toList().get(i / 7); + ArrayList> entries = new ArrayList<>(aprilTagFieldLayout.getTags().entrySet()); + for (int i = 0; i < aprilTagFieldLayout.getTags().size() * 8; i += 8) { + var entry = entries.get(i / 8); elements[i] = entry.getKey(); elements[i + 1] = entry.getValue().getX(); elements[i + 2] = entry.getValue().getY(); @@ -64,6 +67,7 @@ private static double[] getElementsFromAprilTagFieldLayout(AprilTagFieldLayout a elements[i + 6] = entry.getValue().getRotation().getQuaternion().getY(); elements[i + 7] = entry.getValue().getRotation().getQuaternion().getZ(); } + System.out.println(Arrays.toString(elements)); return elements; } diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index 4feabeab708..44c46241e4b 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -122,13 +122,13 @@ frc::AprilTagFieldLayout CreateAprilTagLayoutFromElements(wpi::span apriltags; apriltags.reserve(elements.size() / 8); for (size_t i = 0; i < elements.size(); i += 8) { apriltags.emplace_back(frc::AprilTagFieldLayout::AprilTag{ - static_cast(elements[0]), + static_cast(elements[i]), frc::Pose3d{units::meter_t{elements[i + 1]}, units::meter_t{elements[i + 2]}, units::meter_t{elements[i + 3]}, @@ -142,8 +142,6 @@ frc::AprilTagFieldLayout CreateAprilTagLayoutFromElements(wpi::span AprilTagUtil.deserializeAprilTagFieldLayout( + AprilTagUtil.serializeAprilTagFieldLayout(layout) + ) + ); + + assertEquals(layout.getTags(), deserialized.getTags()); + } +} From 1a6b8945462f34fd663be04b69f63ec060539fb1 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 11:23:49 -0400 Subject: [PATCH 09/73] Make Java side of AprilTagFieldLayout less interconnected --- .../math/apriltag/AprilTagFieldLayout.java | 16 ++++++++++++ .../wpi/first/math/apriltag/AprilTagUtil.java | 26 +++++++------------ .../first/math/apriltag/AprilTagJsonTest.java | 5 ++-- 3 files changed, 27 insertions(+), 20 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagFieldLayout.java b/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagFieldLayout.java index 8e10b697680..f49098f1b53 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagFieldLayout.java +++ b/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagFieldLayout.java @@ -1,9 +1,13 @@ package edu.wpi.first.math.apriltag; +import edu.wpi.first.math.WPIMathJNI; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; +import java.io.IOException; +import java.nio.file.Files; +import java.nio.file.Path; import java.util.HashMap; import java.util.Map; @@ -11,6 +15,14 @@ public class AprilTagFieldLayout { private final Map m_tags = new HashMap<>(); private boolean m_mirror = false; + public AprilTagFieldLayout(String path) throws IOException { + this(Path.of(path)); + } + + public AprilTagFieldLayout(Path path) throws IOException { + this(AprilTagUtil.createAprilTagFieldLayoutFromElements(WPIMathJNI.deserializeAprilTagLayout(Files.readString(path)))); + } + public AprilTagFieldLayout(Map tags) { // To ensure the underlying semantics don't change with what kind of map is passed in m_tags.putAll(tags); @@ -31,4 +43,8 @@ public Pose3d getTag(int id) { } return tag; } + + public String serialize() { + return WPIMathJNI.serializeAprilTagLayout(AprilTagUtil.getElementsFromAprilTagFieldLayout(m_tags)); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java b/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java index 34368420b43..276eaca694e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java @@ -4,6 +4,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Quaternion; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.util.datalog.IntegerLogEntry; import java.util.ArrayList; import java.util.Arrays; @@ -17,7 +18,7 @@ public class AprilTagUtil { * @param elements A double[] containing the raw elements of the AprilTag layout. * @return An AprilTag layout created from the elements. */ - private static AprilTagFieldLayout createAprilTagFieldLayoutFromElements(double[] elements) { + public static Map createAprilTagFieldLayoutFromElements(double[] elements) { // Make sure that the elements have the correct length. if (elements.length % 8 != 0) { throw new AprilTagLayoutSerializationException( @@ -42,21 +43,21 @@ private static AprilTagFieldLayout createAprilTagFieldLayoutFromElements(double[ ) ); } - return new AprilTagFieldLayout(apriltagLayout); + return apriltagLayout; } /** * Returns a double[] of elements from the given AprilTag layout. * - * @param aprilTagFieldLayout The AprilTag field layout to retrieve raw elements from. - * @return A double[] of elements from the given trajectory. + * @param aprilTagLayout The AprilTag layout to retrieve raw elements from. + * @return A double[] of elements from the given AprilTag layout. */ - private static double[] getElementsFromAprilTagFieldLayout(AprilTagFieldLayout aprilTagFieldLayout) { + public static double[] getElementsFromAprilTagFieldLayout(Map aprilTagLayout) { // Create a double[] of elements and fill it from the trajectory states. - double[] elements = new double[aprilTagFieldLayout.getTags().size() * 8]; + double[] elements = new double[aprilTagLayout.size() * 8]; - ArrayList> entries = new ArrayList<>(aprilTagFieldLayout.getTags().entrySet()); - for (int i = 0; i < aprilTagFieldLayout.getTags().size() * 8; i += 8) { + ArrayList> entries = new ArrayList<>(aprilTagLayout.entrySet()); + for (int i = 0; i < aprilTagLayout.size() * 8; i += 8) { var entry = entries.get(i / 8); elements[i] = entry.getKey(); elements[i + 1] = entry.getValue().getX(); @@ -67,18 +68,9 @@ private static double[] getElementsFromAprilTagFieldLayout(AprilTagFieldLayout a elements[i + 6] = entry.getValue().getRotation().getQuaternion().getY(); elements[i + 7] = entry.getValue().getRotation().getQuaternion().getZ(); } - System.out.println(Arrays.toString(elements)); return elements; } - public static AprilTagFieldLayout deserializeAprilTagFieldLayout(String json) { - return createAprilTagFieldLayoutFromElements(WPIMathJNI.deserializeAprilTagLayout(json)); - } - - public static String serializeAprilTagFieldLayout(AprilTagFieldLayout aprilTagFieldLayout) { - return WPIMathJNI.serializeAprilTagLayout(getElementsFromAprilTagFieldLayout(aprilTagFieldLayout)); - } - public static class AprilTagLayoutSerializationException extends RuntimeException { public AprilTagLayoutSerializationException(String message) { super(message); diff --git a/wpimath/src/test/java/edu/wpi/first/math/apriltag/AprilTagJsonTest.java b/wpimath/src/test/java/edu/wpi/first/math/apriltag/AprilTagJsonTest.java index acc98d10cf9..fdb4d2c0ea1 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/apriltag/AprilTagJsonTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/apriltag/AprilTagJsonTest.java @@ -3,6 +3,7 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.WPIMathJNI; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.trajectory.TrajectoryUtil; @@ -19,9 +20,7 @@ void deserializeMatches() { )); var deserialized = assertDoesNotThrow( - () -> AprilTagUtil.deserializeAprilTagFieldLayout( - AprilTagUtil.serializeAprilTagFieldLayout(layout) - ) + () -> new AprilTagFieldLayout(AprilTagUtil.createAprilTagFieldLayoutFromElements(WPIMathJNI.deserializeAprilTagLayout(layout.serialize()))) ); assertEquals(layout.getTags(), deserialized.getTags()); From b8b85c0bf12e7dc2f541df1cc0a1b3b875ac2ba5 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 11:31:36 -0400 Subject: [PATCH 10/73] Move AprilTagFieldLayout and AprilTagUtil java files to wpilibj --- .../wpilibj}/apriltag/AprilTagFieldLayout.java | 7 ++++--- .../wpi/first/wpilibj}/apriltag/AprilTagUtil.java | 5 +---- .../wpi/first/wpilibj/AprilTagFieldLayoutTest.java | 13 +++++++------ 3 files changed, 12 insertions(+), 13 deletions(-) rename {wpimath/src/main/java/edu/wpi/first/math => wpilibj/src/main/java/edu/wpi/first/wpilibj}/apriltag/AprilTagFieldLayout.java (87%) rename {wpimath/src/main/java/edu/wpi/first/math => wpilibj/src/main/java/edu/wpi/first/wpilibj}/apriltag/AprilTagUtil.java (95%) rename wpimath/src/test/java/edu/wpi/first/math/apriltag/AprilTagJsonTest.java => wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java (82%) diff --git a/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java similarity index 87% rename from wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagFieldLayout.java rename to wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index f49098f1b53..c384087bfda 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -1,9 +1,10 @@ -package edu.wpi.first.math.apriltag; +package edu.wpi.first.wpilibj.apriltag; import edu.wpi.first.math.WPIMathJNI; 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.nio.file.Files; @@ -28,8 +29,8 @@ public AprilTagFieldLayout(Map tags) { m_tags.putAll(tags); } - public void setShouldMirror(boolean mirror) { - m_mirror = mirror; + public void setAlliance(DriverStation.Alliance alliance) { + m_mirror = alliance == DriverStation.Alliance.Red; } public Map getTags() { diff --git a/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java similarity index 95% rename from wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java rename to wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java index 276eaca694e..87caaf35747 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/apriltag/AprilTagUtil.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java @@ -1,13 +1,10 @@ -package edu.wpi.first.math.apriltag; +package edu.wpi.first.wpilibj.apriltag; -import edu.wpi.first.math.WPIMathJNI; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Quaternion; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.util.datalog.IntegerLogEntry; import java.util.ArrayList; -import java.util.Arrays; import java.util.HashMap; import java.util.Map; diff --git a/wpimath/src/test/java/edu/wpi/first/math/apriltag/AprilTagJsonTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java similarity index 82% rename from wpimath/src/test/java/edu/wpi/first/math/apriltag/AprilTagJsonTest.java rename to wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java index fdb4d2c0ea1..13f5d80e65a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/apriltag/AprilTagJsonTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java @@ -1,17 +1,18 @@ -package edu.wpi.first.math.apriltag; - -import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; -import static org.junit.jupiter.api.Assertions.assertEquals; +package edu.wpi.first.wpilibj; import edu.wpi.first.math.WPIMathJNI; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.trajectory.TrajectoryUtil; +import edu.wpi.first.wpilibj.apriltag.AprilTagFieldLayout; +import edu.wpi.first.wpilibj.apriltag.AprilTagUtil; import org.junit.jupiter.api.Test; import java.util.Map; -public class AprilTagJsonTest { +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class AprilTagFieldLayoutTest { @Test void deserializeMatches() { var layout = new AprilTagFieldLayout(Map.of( From ca927e225575db2333110c40c1dbd8a475e4cc42 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 11:53:43 -0400 Subject: [PATCH 11/73] Decouple AprilTag serialization/deserialization from AprilTagFieldLayout C++ --- .../cpp/apriltag/AprilTagFieldLayout.cpp | 24 ++---------- .../main/native/cpp/apriltag/AprilTagUtil.cpp | 37 +++++++++++++++---- .../src/main/native/cpp/jni/WPIMathJNI.cpp | 14 +++---- .../frc/apriltag/AprilTagFieldLayout.h | 34 +++-------------- .../include/frc/apriltag/AprilTagUtil.h | 34 ++++++++++++++--- .../native/cpp/apriltag/AprilTagJsonTest.cpp | 8 ++-- 6 files changed, 78 insertions(+), 73 deletions(-) diff --git a/wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index feb6940d9ff..e3ae171e179 100644 --- a/wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -11,15 +11,7 @@ using namespace frc; -bool AprilTagFieldLayout::AprilTag::operator==(const AprilTag &other) const { - return id == other.id && pose == other.pose; -} - -bool AprilTagFieldLayout::AprilTag::operator!=(const AprilTag &other) const { - return !operator==(other); -} - -AprilTagFieldLayout::AprilTagFieldLayout(const std::vector& apriltags): m_apriltags(apriltags) {} +AprilTagFieldLayout::AprilTagFieldLayout(const std::vector& apriltags): m_apriltags(apriltags) {} frc::Pose3d AprilTagFieldLayout::GetTagPose(int id) const { Pose3d returnPose; @@ -38,14 +30,6 @@ void AprilTagFieldLayout::SetShouldMirror(bool mirror) { m_mirror = mirror; } -void frc::to_json(wpi::json& json, const AprilTagFieldLayout::AprilTag& apriltag) { - json = wpi::json{ - {"id", apriltag.id}, - {"pose", apriltag.pose} - }; -} - -void frc::from_json(const wpi::json& json, AprilTagFieldLayout::AprilTag& apriltag) { - apriltag.id = json.at("id").get(); - apriltag.pose = json.at("pose").get(); -} \ No newline at end of file +const std::vector& frc::AprilTagFieldLayout::GetTags() const { + return m_apriltags; +}; \ No newline at end of file diff --git a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp index 26ec3f8cc68..77c21746672 100644 --- a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp +++ b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp @@ -5,6 +5,7 @@ #include "frc/apriltag/AprilTagUtil.h" #include +#include #include "frc/apriltag/AprilTagFieldLayout.h" #include @@ -15,7 +16,15 @@ using namespace frc; -void AprilTagUtil::ToJson(const AprilTagFieldLayout& apriltagFieldLayout, +bool AprilTagUtil::AprilTag::operator==(const AprilTag &other) const { + return id == other.id && pose == other.pose; +} + +bool AprilTagUtil::AprilTag::operator!=(const AprilTag &other) const { + return !operator==(other); +} + +void AprilTagUtil::ToJson(const std::vector& apriltagFieldLayout, std::string_view path) { std::error_code error_code; @@ -24,12 +33,12 @@ void AprilTagUtil::ToJson(const AprilTagFieldLayout& apriltagFieldLayout, throw std::runtime_error(fmt::format("Cannot open file: {}", path)); } - wpi::json json = apriltagFieldLayout.GetTags(); + wpi::json json = apriltagFieldLayout; output << json; output.flush(); } -AprilTagFieldLayout AprilTagUtil::FromJson(std::string_view path) { +std::vector AprilTagUtil::FromJson(std::string_view path) { std::error_code error_code; wpi::raw_fd_istream input{path, error_code}; @@ -40,15 +49,27 @@ AprilTagFieldLayout AprilTagUtil::FromJson(std::string_view path) { wpi::json json; input >> json; - return AprilTagFieldLayout{json.get>()}; + return json.get>(); } -std::string AprilTagUtil::SerializeAprilTagLayout(const AprilTagFieldLayout& apriltagFieldLayout) { - wpi::json json = apriltagFieldLayout.GetTags(); +std::string AprilTagUtil::SerializeAprilTagLayout(const std::vector& apriltagLayout) { + wpi::json json = apriltagLayout; return json.dump(); } -AprilTagFieldLayout AprilTagUtil::DeserializeAprilTagLayout(std::string_view jsonStr) { +std::vector AprilTagUtil::DeserializeAprilTagLayout(std::string_view jsonStr) { wpi::json json = wpi::json::parse(jsonStr); - return AprilTagFieldLayout{json.get>()}; + return json.get>(); } + +void frc::to_json(wpi::json& json, const AprilTagUtil::AprilTag& apriltag) { + json = wpi::json{ + {"id", apriltag.id}, + {"pose", apriltag.pose} + }; +} + +void frc::from_json(const wpi::json& json, AprilTagUtil::AprilTag& apriltag) { + apriltag.id = json.at("id").get(); + apriltag.pose = json.at("pose").get(); +} \ No newline at end of file diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index 44c46241e4b..574de0e82c0 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -100,11 +100,11 @@ frc::Trajectory CreateTrajectoryFromElements(wpi::span elements) { } std::vector GetElementsFromAprilTagLayout( - const frc::AprilTagFieldLayout& apriltagFieldLayout) { + const std::vector& apriltagFieldLayout) { std::vector elements; - elements.reserve(apriltagFieldLayout.GetTags().size() * 8); + elements.reserve(apriltagFieldLayout.size() * 8); - for (auto&& apriltag : apriltagFieldLayout.GetTags()) { + for (auto&& apriltag : apriltagFieldLayout) { elements.push_back(apriltag.id); elements.push_back(apriltag.pose.X().value()); elements.push_back(apriltag.pose.Y().value()); @@ -118,16 +118,16 @@ std::vector GetElementsFromAprilTagLayout( return elements; } -frc::AprilTagFieldLayout CreateAprilTagLayoutFromElements(wpi::span elements) { +std::vector CreateAprilTagLayoutFromElements(wpi::span elements) { // Make sure that the elements have the correct length. assert(elements.size() % 8 == 0); // Create a vector of AprilTags from the elements. - std::vector apriltags; + std::vector apriltags; apriltags.reserve(elements.size() / 8); for (size_t i = 0; i < elements.size(); i += 8) { - apriltags.emplace_back(frc::AprilTagFieldLayout::AprilTag{ + apriltags.emplace_back(frc::AprilTagUtil::AprilTag{ static_cast(elements[i]), frc::Pose3d{units::meter_t{elements[i + 1]}, units::meter_t{elements[i + 2]}, @@ -139,7 +139,7 @@ frc::AprilTagFieldLayout CreateAprilTagLayoutFromElements(wpi::span& apriltags); + explicit AprilTagFieldLayout(const std::vector& apriltags); - const std::vector& GetTags() const { return m_apriltags; }; + const std::vector& GetTags() const; Pose3d GetTagPose(int id) const; void SetShouldMirror(bool mirror); private: - std::vector m_apriltags; + std::vector m_apriltags; bool m_mirror; }; WPILIB_DLLEXPORT -void to_json(wpi::json& json, const AprilTagFieldLayout::AprilTag& apriltag); +void to_json(wpi::json& json, const AprilTagUtil::AprilTag& apriltag); WPILIB_DLLEXPORT -void from_json(const wpi::json& json, AprilTagFieldLayout::AprilTag& apriltag); +void from_json(const wpi::json& json, AprilTagUtil::AprilTag& apriltag); } \ No newline at end of file diff --git a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h index acca443edcf..202566629ec 100644 --- a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h +++ b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h @@ -6,23 +6,45 @@ #include #include +#include +#include "frc/geometry/Pose3d.h" #include -#include "frc/apriltag/AprilTagFieldLayout.h" - namespace frc { class WPILIB_DLLEXPORT AprilTagUtil { public: AprilTagUtil() = delete; + struct WPILIB_DLLEXPORT AprilTag { + int id; + + Pose3d pose; + + /** + * Checks equality between this State and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const AprilTag& other) const; + + /** + * Checks inequality between this State and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const AprilTag& other) const; + }; + /** * Exports a AprilTag Layout JSON file. * * @param apriltag the AprilTag layout to export * @param path the path of the file to export to */ - static void ToJson(const AprilTagFieldLayout& apriltagFieldLayout, + static void ToJson(const std::vector& apriltagFieldLayout, std::string_view path); /** * Imports a AprilTag Layout JSON file. @@ -31,7 +53,7 @@ class WPILIB_DLLEXPORT AprilTagUtil { * * @return The AprilTag layout represented by the file. */ - static AprilTagFieldLayout FromJson(std::string_view path); + static std::vector FromJson(std::string_view path); /** * Deserializes a AprilTag layout JSON. @@ -40,7 +62,7 @@ class WPILIB_DLLEXPORT AprilTagUtil { * * @return the string containing the serialized JSON */ - static std::string SerializeAprilTagLayout(const AprilTagFieldLayout& apriltagFieldLayout); + static std::string SerializeAprilTagLayout(const std::vector& apriltagFieldLayout); /** * Serializes a AprilTag layout JSON. @@ -49,6 +71,6 @@ class WPILIB_DLLEXPORT AprilTagUtil { * * @return the AprilTag layout represented by the JSON */ - static AprilTagFieldLayout DeserializeAprilTagLayout(std::string_view jsonStr); + static std::vector DeserializeAprilTagLayout(std::string_view jsonStr); }; } // namespace frc diff --git a/wpimath/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp b/wpimath/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp index 0df0185c8b6..95963a35ce9 100644 --- a/wpimath/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp +++ b/wpimath/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp @@ -14,12 +14,12 @@ using namespace frc; TEST(AprilTagJsonTest, DeserializeMatches) { auto layout = AprilTagFieldLayout{std::vector{ - AprilTagFieldLayout::AprilTag{1, Pose3d{}}, - AprilTagFieldLayout::AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}} + AprilTagUtil::AprilTag{1, Pose3d{}}, + AprilTagUtil::AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}} }; AprilTagFieldLayout deserialized; - EXPECT_NO_THROW(deserialized = AprilTagUtil::DeserializeAprilTagLayout( - AprilTagUtil::SerializeAprilTagLayout(layout))); + EXPECT_NO_THROW(deserialized = AprilTagFieldLayout{AprilTagUtil::DeserializeAprilTagLayout( + AprilTagUtil::SerializeAprilTagLayout(layout.GetTags()))}); EXPECT_EQ(layout.GetTags(), deserialized.GetTags()); } From 06b5c64d6b915c61a07896d348b9d5c3b0542c38 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 12:06:32 -0400 Subject: [PATCH 12/73] Move AprilTagFieldLayout.cpp to WPILibC --- .../src/main/native/cpp}/AprilTagFieldLayout.cpp | 2 +- .../src/main/native/include/frc}/AprilTagFieldLayout.h | 8 +------- .../src/test/native/cpp}/AprilTagJsonTest.cpp | 2 +- wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp | 1 - wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp | 1 - .../src/main/native/include/frc/apriltag/AprilTagUtil.h | 7 +++++++ 6 files changed, 10 insertions(+), 11 deletions(-) rename {wpimath/src/main/native/cpp/apriltag => wpilibc/src/main/native/cpp}/AprilTagFieldLayout.cpp (95%) rename {wpimath/src/main/native/include/frc/apriltag => wpilibc/src/main/native/include/frc}/AprilTagFieldLayout.h (83%) rename {wpimath/src/test/native/cpp/apriltag => wpilibc/src/test/native/cpp}/AprilTagJsonTest.cpp (95%) diff --git a/wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp similarity index 95% rename from wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp rename to wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp index e3ae171e179..0de441dc6ee 100644 --- a/wpimath/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -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. -#include "frc/apriltag/AprilTagFieldLayout.h" +#include "frc/AprilTagFieldLayout.h" #include #include "frc/geometry/Pose3d.h" diff --git a/wpimath/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h similarity index 83% rename from wpimath/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h rename to wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h index 2d23d63014f..7720df562fd 100644 --- a/wpimath/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h @@ -17,7 +17,7 @@ #include "units/curvature.h" #include "units/time.h" #include "units/velocity.h" -#include "AprilTagUtil.h" +#include "frc/apriltag/AprilTagUtil.h" namespace wpi { class json; @@ -40,10 +40,4 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { std::vector m_apriltags; bool m_mirror; }; - -WPILIB_DLLEXPORT -void to_json(wpi::json& json, const AprilTagUtil::AprilTag& apriltag); - -WPILIB_DLLEXPORT -void from_json(const wpi::json& json, AprilTagUtil::AprilTag& apriltag); } \ No newline at end of file diff --git a/wpimath/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp similarity index 95% rename from wpimath/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp rename to wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp index 95963a35ce9..a23b9acebf2 100644 --- a/wpimath/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp +++ b/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp @@ -3,7 +3,7 @@ // the WPILib BSD license file in the root directory of this project. #include -#include "frc/apriltag/AprilTagFieldLayout.h" +#include "frc/AprilTagFieldLayout.h" #include "frc/apriltag/AprilTagUtil.h" #include "frc/geometry/Pose3d.h" #include "frc/trajectory/TrajectoryConfig.h" diff --git a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp index 77c21746672..79f6c6f3c1b 100644 --- a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp +++ b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp @@ -6,7 +6,6 @@ #include #include -#include "frc/apriltag/AprilTagFieldLayout.h" #include #include diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index 574de0e82c0..436dfcc7b5f 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -14,7 +14,6 @@ #include "Eigen/QR" #include "drake/math/discrete_algebraic_riccati_equation.h" #include "edu_wpi_first_math_WPIMathJNI.h" -#include "frc/apriltag/AprilTagFieldLayout.h" #include "frc/apriltag/AprilTagUtil.h" #include "frc/geometry/Pose3d.h" #include "frc/geometry/Rotation3d.h" diff --git a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h index 202566629ec..7271c58e8b1 100644 --- a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h +++ b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h @@ -73,4 +73,11 @@ class WPILIB_DLLEXPORT AprilTagUtil { */ static std::vector DeserializeAprilTagLayout(std::string_view jsonStr); }; + +WPILIB_DLLEXPORT +void to_json(wpi::json& json, const AprilTagUtil::AprilTag& apriltag); + +WPILIB_DLLEXPORT +void from_json(const wpi::json& json, AprilTagUtil::AprilTag& apriltag); + } // namespace frc From 33bc5b7f17bdf7881dc9a00f644939b25938cae5 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 12:09:45 -0400 Subject: [PATCH 13/73] Fix alliance set function on C++ AprilTagFieldLayout --- wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp | 5 +++-- wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h | 3 ++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp index 0de441dc6ee..94b5295dbcd 100644 --- a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -5,6 +5,7 @@ #include "frc/AprilTagFieldLayout.h" #include +#include "frc/DriverStation.h" #include "frc/geometry/Pose3d.h" #include "units/angle.h" #include "units/length.h" @@ -26,8 +27,8 @@ frc::Pose3d AprilTagFieldLayout::GetTagPose(int id) const { return returnPose; } -void AprilTagFieldLayout::SetShouldMirror(bool mirror) { - m_mirror = mirror; +void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { + m_mirror = alliance == DriverStation::Alliance::kRed; } const std::vector& frc::AprilTagFieldLayout::GetTags() const { diff --git a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h index 7720df562fd..1f309e7bee3 100644 --- a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h @@ -10,6 +10,7 @@ #include +#include "frc/DriverStation.h" #include "frc/geometry/Pose2d.h" #include "frc/geometry/Pose3d.h" #include "frc/geometry/Transform2d.h" @@ -34,7 +35,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { Pose3d GetTagPose(int id) const; - void SetShouldMirror(bool mirror); + void SetAlliance(DriverStation::Alliance alliance); private: std::vector m_apriltags; From 7dd88b08141b815b75bde5d4ecacab7ba38a4c49 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 12:14:52 -0400 Subject: [PATCH 14/73] Minimize imports --- .../src/main/native/include/frc/AprilTagFieldLayout.h | 10 ---------- wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp | 2 -- wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp | 1 - wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp | 4 ---- .../main/native/include/frc/apriltag/AprilTagUtil.h | 1 - 5 files changed, 18 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h index 1f309e7bee3..8c85c1e3fbf 100644 --- a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h @@ -4,20 +4,10 @@ #pragma once -#include -#include -#include - #include #include "frc/DriverStation.h" -#include "frc/geometry/Pose2d.h" #include "frc/geometry/Pose3d.h" -#include "frc/geometry/Transform2d.h" -#include "units/acceleration.h" -#include "units/curvature.h" -#include "units/time.h" -#include "units/velocity.h" #include "frc/apriltag/AprilTagUtil.h" namespace wpi { diff --git a/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp index a23b9acebf2..6379e9cf788 100644 --- a/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp +++ b/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp @@ -6,8 +6,6 @@ #include "frc/AprilTagFieldLayout.h" #include "frc/apriltag/AprilTagUtil.h" #include "frc/geometry/Pose3d.h" -#include "frc/trajectory/TrajectoryConfig.h" -#include "frc/trajectory/TrajectoryUtil.h" #include "gtest/gtest.h" using namespace frc; diff --git a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp index 79f6c6f3c1b..8d3b54ecc09 100644 --- a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp +++ b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp @@ -8,7 +8,6 @@ #include #include -#include #include #include #include diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index 436dfcc7b5f..90dbc6bd98d 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -15,11 +15,7 @@ #include "drake/math/discrete_algebraic_riccati_equation.h" #include "edu_wpi_first_math_WPIMathJNI.h" #include "frc/apriltag/AprilTagUtil.h" -#include "frc/geometry/Pose3d.h" -#include "frc/geometry/Rotation3d.h" #include "frc/trajectory/TrajectoryUtil.h" -#include "units/angle.h" -#include "units/length.h" #include "unsupported/Eigen/MatrixFunctions" using namespace wpi::java; diff --git a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h index 7271c58e8b1..c666aa95b6f 100644 --- a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h +++ b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h @@ -5,7 +5,6 @@ #pragma once #include -#include #include #include "frc/geometry/Pose3d.h" From 908a11146091da95d58e53b64473482fbee31b16 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 12:17:29 -0400 Subject: [PATCH 15/73] Use proper unit conversion functions --- .../edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index c384087bfda..070db1bf11b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -4,6 +4,7 @@ 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.io.IOException; @@ -40,7 +41,7 @@ public Map getTags() { public Pose3d getTag(int id) { Pose3d tag = m_tags.get(id); if(m_mirror) { - tag = tag.relativeTo(new Pose3d(new Translation3d(16.4592, 8.2296, 0), new Rotation3d(0, 0, 180))); + tag = tag.relativeTo(new Pose3d(new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0), new Rotation3d(0.0, 0.0, 180.0))); } return tag; } From bfac873e8a7457e5f007713a8b3432c334b34337 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 13:46:56 -0400 Subject: [PATCH 16/73] Fix gradle check task --- .../wpilibj/apriltag/AprilTagFieldLayout.java | 102 ++++++++++----- .../first/wpilibj/apriltag/AprilTagUtil.java | 116 +++++++++--------- .../wpilibj/AprilTagFieldLayoutTest.java | 35 +++--- 3 files changed, 150 insertions(+), 103 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 070db1bf11b..d5489e6eaec 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -6,7 +6,6 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; - import java.io.IOException; import java.nio.file.Files; import java.nio.file.Path; @@ -14,39 +13,86 @@ import java.util.Map; public class AprilTagFieldLayout { - private final Map m_tags = new HashMap<>(); - private boolean m_mirror = false; + private final Map m_tags = new HashMap<>(); + private boolean m_mirror; - public AprilTagFieldLayout(String path) throws IOException { - this(Path.of(path)); - } + /** + * Construct a new AprilTagFieldLayout with values imported from a JSON file. + * + * @param path path of the JSON file to import from + * @throws IOException if reading from the file fails. + */ + public AprilTagFieldLayout(String path) throws IOException { + this(Path.of(path)); + } - public AprilTagFieldLayout(Path path) throws IOException { - this(AprilTagUtil.createAprilTagFieldLayoutFromElements(WPIMathJNI.deserializeAprilTagLayout(Files.readString(path)))); - } + /** + * Construct a new AprilTagFieldLayout with values imported from a JSON file. + * + * @param path path of the JSON file to import from + * @throws IOException if reading from the file fails. + */ + public AprilTagFieldLayout(Path path) throws IOException { + this( + AprilTagUtil.createAprilTagFieldLayoutFromElements( + WPIMathJNI.deserializeAprilTagLayout(Files.readString(path)))); + } - public AprilTagFieldLayout(Map tags) { - // To ensure the underlying semantics don't change with what kind of map is passed in - m_tags.putAll(tags); - } + /** + * Construct a new AprilTagFieldLayout from a map of AprilTag IDs to poses. + * + * @param tags map of IDs to poses + */ + public AprilTagFieldLayout(Map tags) { + // To ensure the underlying semantics don't change with what kind of map is passed in + m_tags.putAll(tags); + } - public void setAlliance(DriverStation.Alliance alliance) { - m_mirror = alliance == DriverStation.Alliance.Red; - } + /** + * Set the alliance that your team is on. This changes the {@link AprilTagFieldLayout getTag} + * method to return the correct pose for the alliance you are on. + * + * @param alliance the alliance to mirror poses for + */ + public void setAlliance(DriverStation.Alliance alliance) { + m_mirror = alliance == DriverStation.Alliance.Red; + } - public Map getTags() { - return m_tags; - } + /** + * Gets the raw tags. This is not affected by {@link AprilTagFieldLayout setAlliance}. + * + * @return map of IDs to poses. + */ + public Map getTags() { + return m_tags; + } - public Pose3d getTag(int id) { - Pose3d tag = m_tags.get(id); - if(m_mirror) { - tag = tag.relativeTo(new Pose3d(new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0), new Rotation3d(0.0, 0.0, 180.0))); - } - return tag; + /** + * Gets an AprilTag pose by its id. + * + * @param id The id of the tag + * @return The pose corresponding to the id passed in. + */ + public Pose3d getTag(int id) { + Pose3d tag = m_tags.get(id); + if (m_mirror) { + tag = + tag.relativeTo( + new Pose3d( + new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0), + new Rotation3d(0.0, 0.0, 180.0))); } - public String serialize() { - return WPIMathJNI.serializeAprilTagLayout(AprilTagUtil.getElementsFromAprilTagFieldLayout(m_tags)); - } + return tag; + } + + /** + * Serialize this layout into a JSON string. + * + * @return the serialized JSON string. + */ + public String serialize() { + return WPIMathJNI.serializeAprilTagLayout( + AprilTagUtil.getElementsFromAprilTagFieldLayout(m_tags)); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java index 87caaf35747..d60d1adfa3c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java @@ -3,74 +3,72 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Quaternion; import edu.wpi.first.math.geometry.Rotation3d; - import java.util.ArrayList; import java.util.HashMap; import java.util.Map; -public class AprilTagUtil { - /** - * Creates an AprilTag layout from a double[] of elements. - * - * @param elements A double[] containing the raw elements of the AprilTag layout. - * @return An AprilTag layout created from the elements. - */ - public static Map createAprilTagFieldLayoutFromElements(double[] elements) { - // Make sure that the elements have the correct length. - if (elements.length % 8 != 0) { - throw new AprilTagLayoutSerializationException( - "An error occurred when converting AprilTag elements into a AprilTag layout."); - } +public final class AprilTagUtil { + private AprilTagUtil() { + throw new UnsupportedOperationException("This is a utility class!"); + } + + /** + * Creates an AprilTag layout from a double[] of elements. + * + * @param elements A double[] containing the raw elements of the AprilTag layout. + * @return An AprilTag layout created from the elements. + */ + public static Map createAprilTagFieldLayoutFromElements(double[] elements) { + // Make sure that the elements have the correct length. + if (elements.length % 8 != 0) { + throw new AprilTagLayoutSerializationException( + "An error occurred when converting AprilTag elements into a AprilTag layout."); + } - // Create a list of states from the elements. - Map apriltagLayout = new HashMap<>(); - for (int i = 0; i < elements.length; i += 8) { - apriltagLayout.put( - (int) elements[i], - new Pose3d( - elements[i + 1], - elements[i + 2], - elements[i + 3], - new Rotation3d(new Quaternion( - elements[i + 4], - elements[i + 5], - elements[i + 6], - elements[i + 7] - )) - ) - ); - } - return apriltagLayout; + // Create a list of states from the elements. + Map apriltagLayout = new HashMap<>(); + for (int i = 0; i < elements.length; i += 8) { + apriltagLayout.put( + (int) elements[i], + new Pose3d( + elements[i + 1], + elements[i + 2], + elements[i + 3], + new Rotation3d( + new Quaternion( + elements[i + 4], elements[i + 5], elements[i + 6], elements[i + 7])))); } + return apriltagLayout; + } - /** - * Returns a double[] of elements from the given AprilTag layout. - * - * @param aprilTagLayout The AprilTag layout to retrieve raw elements from. - * @return A double[] of elements from the given AprilTag layout. - */ - public static double[] getElementsFromAprilTagFieldLayout(Map aprilTagLayout) { - // Create a double[] of elements and fill it from the trajectory states. - double[] elements = new double[aprilTagLayout.size() * 8]; + /** + * Returns a double[] of elements from the given AprilTag layout. + * + * @param aprilTagLayout The AprilTag layout to retrieve raw elements from. + * @return A double[] of elements from the given AprilTag layout. + */ + public static double[] getElementsFromAprilTagFieldLayout(Map aprilTagLayout) { + // Create a double[] of elements and fill it from the trajectory states. + double[] elements = new double[aprilTagLayout.size() * 8]; - ArrayList> entries = new ArrayList<>(aprilTagLayout.entrySet()); - for (int i = 0; i < aprilTagLayout.size() * 8; i += 8) { - var entry = entries.get(i / 8); - elements[i] = entry.getKey(); - elements[i + 1] = entry.getValue().getX(); - elements[i + 2] = entry.getValue().getY(); - elements[i + 3] = entry.getValue().getZ(); - elements[i + 4] = entry.getValue().getRotation().getQuaternion().getW(); - elements[i + 5] = entry.getValue().getRotation().getQuaternion().getX(); - elements[i + 6] = entry.getValue().getRotation().getQuaternion().getY(); - elements[i + 7] = entry.getValue().getRotation().getQuaternion().getZ(); - } - return elements; + ArrayList> entries = new ArrayList<>(aprilTagLayout.entrySet()); + for (int i = 0; i < aprilTagLayout.size() * 8; i += 8) { + var entry = entries.get(i / 8); + elements[i] = entry.getKey(); + elements[i + 1] = entry.getValue().getX(); + elements[i + 2] = entry.getValue().getY(); + elements[i + 3] = entry.getValue().getZ(); + elements[i + 4] = entry.getValue().getRotation().getQuaternion().getW(); + elements[i + 5] = entry.getValue().getRotation().getQuaternion().getX(); + elements[i + 6] = entry.getValue().getRotation().getQuaternion().getY(); + elements[i + 7] = entry.getValue().getRotation().getQuaternion().getZ(); } + return elements; + } - public static class AprilTagLayoutSerializationException extends RuntimeException { - public AprilTagLayoutSerializationException(String message) { - super(message); - } + public static class AprilTagLayoutSerializationException extends RuntimeException { + public AprilTagLayoutSerializationException(String message) { + super(message); } + } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java index 13f5d80e65a..eee938d1ff8 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java @@ -1,29 +1,32 @@ package edu.wpi.first.wpilibj; +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; + import edu.wpi.first.math.WPIMathJNI; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.wpilibj.apriltag.AprilTagFieldLayout; import edu.wpi.first.wpilibj.apriltag.AprilTagUtil; -import org.junit.jupiter.api.Test; - import java.util.Map; +import org.junit.jupiter.api.Test; -import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; -import static org.junit.jupiter.api.Assertions.assertEquals; - -public class AprilTagFieldLayoutTest { - @Test - void deserializeMatches() { - var layout = new AprilTagFieldLayout(Map.of( +class AprilTagFieldLayoutTest { + @Test + void deserializeMatches() { + var layout = + new AprilTagFieldLayout( + Map.of( 1, new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)), - 3, new Pose3d(0, 1, 0, new Rotation3d(0, 0, 0)) - )); + 3, new Pose3d(0, 1, 0, new Rotation3d(0, 0, 0)))); - var deserialized = assertDoesNotThrow( - () -> new AprilTagFieldLayout(AprilTagUtil.createAprilTagFieldLayoutFromElements(WPIMathJNI.deserializeAprilTagLayout(layout.serialize()))) - ); + var deserialized = + assertDoesNotThrow( + () -> + new AprilTagFieldLayout( + AprilTagUtil.createAprilTagFieldLayoutFromElements( + WPIMathJNI.deserializeAprilTagLayout(layout.serialize())))); - assertEquals(layout.getTags(), deserialized.getTags()); - } + assertEquals(layout.getTags(), deserialized.getTags()); + } } From b517215e1fb432f5ac50c922413de3124ffafb1a Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 14:02:21 -0400 Subject: [PATCH 17/73] Run wpiformat --- .../main/native/cpp/AprilTagFieldLayout.cpp | 37 ++++++++------- .../native/include/frc/AprilTagFieldLayout.h | 31 +++++++------ .../src/test/native/cpp/AprilTagJsonTest.cpp | 13 +++--- .../wpilibj/apriltag/AprilTagFieldLayout.java | 4 ++ .../first/wpilibj/apriltag/AprilTagUtil.java | 4 ++ .../wpilibj/AprilTagFieldLayoutTest.java | 4 ++ .../main/native/cpp/apriltag/AprilTagUtil.cpp | 30 ++++++------- .../src/main/native/cpp/geometry/Pose3d.cpp | 4 +- .../main/native/cpp/geometry/Rotation3d.cpp | 16 +++---- .../native/cpp/geometry/Translation3d.cpp | 8 ++-- .../src/main/native/cpp/jni/WPIMathJNI.cpp | 26 +++++------ .../include/frc/apriltag/AprilTagUtil.h | 45 ++++++++++--------- 12 files changed, 124 insertions(+), 98 deletions(-) diff --git a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp index 94b5295dbcd..af24502ca13 100644 --- a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -4,33 +4,38 @@ #include "frc/AprilTagFieldLayout.h" +#include +#include #include + #include "frc/DriverStation.h" #include "frc/geometry/Pose3d.h" -#include "units/angle.h" -#include "units/length.h" using namespace frc; -AprilTagFieldLayout::AprilTagFieldLayout(const std::vector& apriltags): m_apriltags(apriltags) {} +AprilTagFieldLayout::AprilTagFieldLayout( + const std::vector& apriltags) + : m_apriltags(apriltags) {} frc::Pose3d AprilTagFieldLayout::GetTagPose(int id) const { - Pose3d returnPose; - for(auto& tag : m_apriltags) { - if(tag.id == id) { - returnPose = tag.pose; - } - } - if(m_mirror) { - returnPose = returnPose.RelativeTo(Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}}); + Pose3d returnPose; + for (auto& tag : m_apriltags) { + if (tag.id == id) { + returnPose = tag.pose; } - return returnPose; + } + if (m_mirror) { + returnPose = returnPose.RelativeTo( + Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}}); + } + return returnPose; } void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { - m_mirror = alliance == DriverStation::Alliance::kRed; + m_mirror = alliance == DriverStation::Alliance::kRed; } -const std::vector& frc::AprilTagFieldLayout::GetTags() const { - return m_apriltags; -}; \ No newline at end of file +const std::vector& frc::AprilTagFieldLayout::GetTags() + const { + return m_apriltags; +}; diff --git a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h index 8c85c1e3fbf..97209edf8ff 100644 --- a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h @@ -4,11 +4,13 @@ #pragma once +#include + #include #include "frc/DriverStation.h" -#include "frc/geometry/Pose3d.h" #include "frc/apriltag/AprilTagUtil.h" +#include "frc/geometry/Pose3d.h" namespace wpi { class json; @@ -17,18 +19,19 @@ class json; namespace frc { class WPILIB_DLLEXPORT AprilTagFieldLayout { public: - AprilTagFieldLayout() = default; - - explicit AprilTagFieldLayout(const std::vector& apriltags); - - const std::vector& GetTags() const; - - Pose3d GetTagPose(int id) const; - - void SetAlliance(DriverStation::Alliance alliance); - + AprilTagFieldLayout() = default; + + explicit AprilTagFieldLayout( + const std::vector& apriltags); + + const std::vector& GetTags() const; + + Pose3d GetTagPose(int id) const; + + void SetAlliance(DriverStation::Alliance alliance); + private: - std::vector m_apriltags; - bool m_mirror; + std::vector m_apriltags; + bool m_mirror; }; -} \ No newline at end of file +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp index 6379e9cf788..e2c938ffc9c 100644 --- a/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp +++ b/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. #include + #include "frc/AprilTagFieldLayout.h" #include "frc/apriltag/AprilTagUtil.h" #include "frc/geometry/Pose3d.h" @@ -12,12 +13,14 @@ using namespace frc; TEST(AprilTagJsonTest, DeserializeMatches) { auto layout = AprilTagFieldLayout{std::vector{ - AprilTagUtil::AprilTag{1, Pose3d{}}, - AprilTagUtil::AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}} - }; + AprilTagUtil::AprilTag{1, Pose3d{}}, + AprilTagUtil::AprilTag{ + 3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}}}; AprilTagFieldLayout deserialized; - EXPECT_NO_THROW(deserialized = AprilTagFieldLayout{AprilTagUtil::DeserializeAprilTagLayout( - AprilTagUtil::SerializeAprilTagLayout(layout.GetTags()))}); + EXPECT_NO_THROW( + deserialized = + AprilTagFieldLayout{AprilTagUtil::DeserializeAprilTagLayout( + AprilTagUtil::SerializeAprilTagLayout(layout.GetTags()))}); EXPECT_EQ(layout.GetTags(), deserialized.GetTags()); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index d5489e6eaec..5eafaf2976b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -1,3 +1,7 @@ +// 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 edu.wpi.first.math.WPIMathJNI; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java index d60d1adfa3c..f38764a2fa6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java @@ -1,3 +1,7 @@ +// 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 edu.wpi.first.math.geometry.Pose3d; diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java index eee938d1ff8..bb32ac5caab 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java @@ -1,3 +1,7 @@ +// 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; import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; diff --git a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp index 8d3b54ecc09..fd7a3128f3f 100644 --- a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp +++ b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp @@ -14,16 +14,16 @@ using namespace frc; -bool AprilTagUtil::AprilTag::operator==(const AprilTag &other) const { - return id == other.id && pose == other.pose; +bool AprilTagUtil::AprilTag::operator==(const AprilTag& other) const { + return id == other.id && pose == other.pose; } -bool AprilTagUtil::AprilTag::operator!=(const AprilTag &other) const { - return !operator==(other); +bool AprilTagUtil::AprilTag::operator!=(const AprilTag& other) const { + return !operator==(other); } void AprilTagUtil::ToJson(const std::vector& apriltagFieldLayout, - std::string_view path) { + std::string_view path) { std::error_code error_code; wpi::raw_fd_ostream output{path, error_code}; @@ -36,7 +36,8 @@ void AprilTagUtil::ToJson(const std::vector& apriltagFieldLayout, output.flush(); } -std::vector AprilTagUtil::FromJson(std::string_view path) { +std::vector AprilTagUtil::FromJson( + std::string_view path) { std::error_code error_code; wpi::raw_fd_istream input{path, error_code}; @@ -50,24 +51,23 @@ std::vector AprilTagUtil::FromJson(std::string_view path return json.get>(); } -std::string AprilTagUtil::SerializeAprilTagLayout(const std::vector& apriltagLayout) { +std::string AprilTagUtil::SerializeAprilTagLayout( + const std::vector& apriltagLayout) { wpi::json json = apriltagLayout; return json.dump(); } -std::vector AprilTagUtil::DeserializeAprilTagLayout(std::string_view jsonStr) { +std::vector AprilTagUtil::DeserializeAprilTagLayout( + std::string_view jsonStr) { wpi::json json = wpi::json::parse(jsonStr); return json.get>(); } void frc::to_json(wpi::json& json, const AprilTagUtil::AprilTag& apriltag) { - json = wpi::json{ - {"id", apriltag.id}, - {"pose", apriltag.pose} - }; + json = wpi::json{{"id", apriltag.id}, {"pose", apriltag.pose}}; } void frc::from_json(const wpi::json& json, AprilTagUtil::AprilTag& apriltag) { - apriltag.id = json.at("id").get(); - apriltag.pose = json.at("pose").get(); -} \ No newline at end of file + apriltag.id = json.at("id").get(); + apriltag.pose = json.at("pose").get(); +} diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index 5603ee142c5..31156e2481d 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -5,7 +5,8 @@ #include "frc/geometry/Pose3d.h" #include -#include "wpi/json.h" + +#include using namespace frc; @@ -148,4 +149,3 @@ void frc::from_json(const wpi::json& json, Pose3d& pose) { pose = Pose3d{json.at("translation").get(), json.at("rotation").get()}; } - diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp index 6df9defbc84..2b6608cb682 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -6,16 +6,16 @@ #include +#include #include #include "Eigen/Core" #include "Eigen/LU" #include "Eigen/QR" -#include "units/angle.h" #include "frc/fmt/Eigen.h" +#include "units/angle.h" #include "units/math.h" #include "wpimath/MathShared.h" -#include "wpi/json.h" using namespace frc; @@ -239,15 +239,13 @@ Rotation2d Rotation3d::ToRotation2d() const { } void frc::to_json(wpi::json& json, const Rotation3d& rotation) { - json = wpi::json{ - {"yaw", rotation.X().value()}, - {"pitch", rotation.Y().value()}, - {"roll", rotation.Z().value()} - }; + json = wpi::json{{"yaw", rotation.X().value()}, + {"pitch", rotation.Y().value()}, + {"roll", rotation.Z().value()}}; } void frc::from_json(const wpi::json& json, Rotation3d& rotation) { rotation = Rotation3d{units::degree_t{json.at("yaw").get()}, - units::degree_t{json.at("pitch").get()}, - units::degree_t{json.at("roll").get()}}; + units::degree_t{json.at("pitch").get()}, + units::degree_t{json.at("roll").get()}}; } diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp index e3b63ca3880..f2d78a3674b 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp @@ -4,9 +4,10 @@ #include "frc/geometry/Translation3d.h" +#include + #include "units/length.h" #include "units/math.h" -#include "wpi/json.h" using namespace frc; @@ -73,8 +74,9 @@ bool Translation3d::operator!=(const Translation3d& other) const { } 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()}}; + 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) { diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index 90dbc6bd98d..71b157bdd30 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -113,7 +113,8 @@ std::vector GetElementsFromAprilTagLayout( return elements; } -std::vector CreateAprilTagLayoutFromElements(wpi::span elements) { +std::vector CreateAprilTagLayoutFromElements( + wpi::span elements) { // Make sure that the elements have the correct length. assert(elements.size() % 8 == 0); @@ -123,15 +124,13 @@ std::vector CreateAprilTagLayoutFromElements(wpi::s for (size_t i = 0; i < elements.size(); i += 8) { apriltags.emplace_back(frc::AprilTagUtil::AprilTag{ - static_cast(elements[i]), - frc::Pose3d{units::meter_t{elements[i + 1]}, - units::meter_t{elements[i + 2]}, - units::meter_t{elements[i + 3]}, - frc::Rotation3d{frc::Quaternion{ - elements[i + 4], - elements[i + 5], - elements[i + 6], - elements[i + 7]}}}}); + static_cast(elements[i]), + frc::Pose3d{units::meter_t{elements[i + 1]}, + units::meter_t{elements[i + 2]}, + units::meter_t{elements[i + 3]}, + frc::Rotation3d{ + frc::Quaternion{elements[i + 4], elements[i + 5], + elements[i + 6], elements[i + 7]}}}}); } return apriltags; @@ -364,7 +363,8 @@ Java_edu_wpi_first_math_WPIMathJNI_deserializeAprilTagLayout try { auto apriltagFieldLayout = frc::AprilTagUtil::DeserializeAprilTagLayout( JStringRef{env, json}.c_str()); - std::vector elements = GetElementsFromAprilTagLayout(apriltagFieldLayout); + std::vector elements = + GetElementsFromAprilTagLayout(apriltagFieldLayout); return MakeJDoubleArray(env, elements); } catch (std::exception& e) { jclass cls = env->FindClass( @@ -389,8 +389,8 @@ Java_edu_wpi_first_math_WPIMathJNI_serializeAprilTagLayout try { auto apriltagFieldLayout = CreateAprilTagLayoutFromElements(JDoubleArrayRef{env, elements}); - return MakeJString(env, - frc::AprilTagUtil::SerializeAprilTagLayout(apriltagFieldLayout)); + return MakeJString( + env, frc::AprilTagUtil::SerializeAprilTagLayout(apriltagFieldLayout)); } catch (std::exception& e) { jclass cls = env->FindClass( "edu/wpi/first/math/apriltag/AprilTagUtil$" diff --git a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h index c666aa95b6f..b452a695ffd 100644 --- a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h +++ b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h @@ -6,35 +6,36 @@ #include #include -#include "frc/geometry/Pose3d.h" #include +#include "frc/geometry/Pose3d.h" + namespace frc { class WPILIB_DLLEXPORT AprilTagUtil { public: AprilTagUtil() = delete; struct WPILIB_DLLEXPORT AprilTag { - int id; + int id; + + Pose3d pose; - Pose3d pose; - - /** - * Checks equality between this State and another object. - * - * @param other The other object. - * @return Whether the two objects are equal. - */ - bool operator==(const AprilTag& other) const; + /** + * Checks equality between this State and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const AprilTag& other) const; - /** - * Checks inequality between this State and another object. - * - * @param other The other object. - * @return Whether the two objects are not equal. - */ - bool operator!=(const AprilTag& other) const; + /** + * Checks inequality between this State and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const AprilTag& other) const; }; /** @@ -44,7 +45,7 @@ class WPILIB_DLLEXPORT AprilTagUtil { * @param path the path of the file to export to */ static void ToJson(const std::vector& apriltagFieldLayout, - std::string_view path); + std::string_view path); /** * Imports a AprilTag Layout JSON file. * @@ -61,7 +62,8 @@ class WPILIB_DLLEXPORT AprilTagUtil { * * @return the string containing the serialized JSON */ - static std::string SerializeAprilTagLayout(const std::vector& apriltagFieldLayout); + static std::string SerializeAprilTagLayout( + const std::vector& apriltagFieldLayout); /** * Serializes a AprilTag layout JSON. @@ -70,7 +72,8 @@ class WPILIB_DLLEXPORT AprilTagUtil { * * @return the AprilTag layout represented by the JSON */ - static std::vector DeserializeAprilTagLayout(std::string_view jsonStr); + static std::vector DeserializeAprilTagLayout( + std::string_view jsonStr); }; WPILIB_DLLEXPORT From 1a3432ed9a5d7f73f7b769981cf6e0b1bb19dffd Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 14:34:59 -0400 Subject: [PATCH 18/73] Fix some docs --- wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp | 2 +- .../first/wpilibj/apriltag/AprilTagFieldLayout.java | 4 ++-- .../src/main/native/cpp/apriltag/AprilTagUtil.cpp | 4 ++-- .../main/native/include/frc/apriltag/AprilTagUtil.h | 12 ++++++------ 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp index af24502ca13..0f3c56ca3e4 100644 --- a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -38,4 +38,4 @@ void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { const std::vector& frc::AprilTagFieldLayout::GetTags() const { return m_apriltags; -}; +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 5eafaf2976b..99b7fc09f0a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -53,7 +53,7 @@ public AprilTagFieldLayout(Map tags) { } /** - * Set the alliance that your team is on. This changes the {@link AprilTagFieldLayout getTag} + * Set the alliance that your team is on. This changes the {@link #getTag()} * method to return the correct pose for the alliance you are on. * * @param alliance the alliance to mirror poses for @@ -63,7 +63,7 @@ public void setAlliance(DriverStation.Alliance alliance) { } /** - * Gets the raw tags. This is not affected by {@link AprilTagFieldLayout setAlliance}. + * Gets the raw tags. This is not affected by {@link #setAlliance()}. * * @return map of IDs to poses. */ diff --git a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp index fd7a3128f3f..1165ed58dcf 100644 --- a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp +++ b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp @@ -22,7 +22,7 @@ bool AprilTagUtil::AprilTag::operator!=(const AprilTag& other) const { return !operator==(other); } -void AprilTagUtil::ToJson(const std::vector& apriltagFieldLayout, +void AprilTagUtil::ToJson(const std::vector& apriltagLayout, std::string_view path) { std::error_code error_code; @@ -31,7 +31,7 @@ void AprilTagUtil::ToJson(const std::vector& apriltagFieldLayout, throw std::runtime_error(fmt::format("Cannot open file: {}", path)); } - wpi::json json = apriltagFieldLayout; + wpi::json json = apriltagLayout; output << json; output.flush(); } diff --git a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h index b452a695ffd..1a0125cd1da 100644 --- a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h +++ b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h @@ -22,7 +22,7 @@ class WPILIB_DLLEXPORT AprilTagUtil { Pose3d pose; /** - * Checks equality between this State and another object. + * Checks equality between this AprilTag and another object. * * @param other The other object. * @return Whether the two objects are equal. @@ -30,7 +30,7 @@ class WPILIB_DLLEXPORT AprilTagUtil { bool operator==(const AprilTag& other) const; /** - * Checks inequality between this State and another object. + * Checks inequality between this AprilTag and another object. * * @param other The other object. * @return Whether the two objects are not equal. @@ -41,10 +41,10 @@ class WPILIB_DLLEXPORT AprilTagUtil { /** * Exports a AprilTag Layout JSON file. * - * @param apriltag the AprilTag layout to export + * @param apriltagLayout the AprilTag layout to export * @param path the path of the file to export to */ - static void ToJson(const std::vector& apriltagFieldLayout, + static void ToJson(const std::vector& apriltagLayout, std::string_view path); /** * Imports a AprilTag Layout JSON file. @@ -58,12 +58,12 @@ class WPILIB_DLLEXPORT AprilTagUtil { /** * Deserializes a AprilTag layout JSON. * - * @param apriltag the AprilTag layout to export + * @param apriltagLayout the AprilTag layout to export * * @return the string containing the serialized JSON */ static std::string SerializeAprilTagLayout( - const std::vector& apriltagFieldLayout); + const std::vector& apriltagLayout); /** * Serializes a AprilTag layout JSON. From e14ef1eb01378d81aa5909d5e0369194732f8904 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 15:52:26 -0400 Subject: [PATCH 19/73] Run javaFormat --- .../edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 99b7fc09f0a..1b8c3f0ab5e 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -53,8 +53,8 @@ public AprilTagFieldLayout(Map tags) { } /** - * Set the alliance that your team is on. This changes the {@link #getTag()} - * method to return the correct pose for the alliance you are on. + * Set the alliance that your team is on. This changes the {@link #getTag()} method to return the + * correct pose for the alliance you are on. * * @param alliance the alliance to mirror poses for */ From 1fd8b428dd05098f254693ab84e59815d2ce1293 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 15:57:25 -0400 Subject: [PATCH 20/73] Fix link tags in javadocs --- .../edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 1b8c3f0ab5e..4901aa8c5e3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -53,7 +53,7 @@ public AprilTagFieldLayout(Map tags) { } /** - * Set the alliance that your team is on. This changes the {@link #getTag()} method to return the + * Set the alliance that your team is on. This changes the {@link #getTag(int)} method to return the * correct pose for the alliance you are on. * * @param alliance the alliance to mirror poses for @@ -63,7 +63,7 @@ public void setAlliance(DriverStation.Alliance alliance) { } /** - * Gets the raw tags. This is not affected by {@link #setAlliance()}. + * Gets the raw tags. This is not affected by {@link #setAlliance(DriverStation.Alliance)}. * * @return map of IDs to poses. */ From 640a859f395fd9b0149da1e3491546f128d738bd Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 16:14:53 -0400 Subject: [PATCH 21/73] Resolve clang-tidy warnings --- wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp index 0f3c56ca3e4..e73e9aefdde 100644 --- a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -15,7 +15,7 @@ using namespace frc; AprilTagFieldLayout::AprilTagFieldLayout( const std::vector& apriltags) - : m_apriltags(apriltags) {} + : m_apriltags(std::move(apriltags)) {} frc::Pose3d AprilTagFieldLayout::GetTagPose(int id) const { Pose3d returnPose; From 82cf25bf7b6c85c590d276d7686a240db211d241 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 16:45:00 -0400 Subject: [PATCH 22/73] Fix checkstyle, line over 100 chars --- .../edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 4901aa8c5e3..0b145fb494c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -53,8 +53,9 @@ public AprilTagFieldLayout(Map tags) { } /** - * Set the alliance that your team is on. This changes the {@link #getTag(int)} method to return the - * correct pose for the alliance you are on. + * Set the alliance that your team is on. + * + *

This changes the {@link #getTag(int)} method to return the correct pose for your alliance. * * @param alliance the alliance to mirror poses for */ From 874ee265d028d72dbeab622d542bc2fb2ec609ea Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 18 Sep 2022 21:36:09 -0400 Subject: [PATCH 23/73] Change GetTagPose to be more idiomatic --- wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp index e73e9aefdde..2a11ec0a2ea 100644 --- a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -4,6 +4,8 @@ #include "frc/AprilTagFieldLayout.h" +#include + #include #include #include @@ -19,10 +21,10 @@ AprilTagFieldLayout::AprilTagFieldLayout( frc::Pose3d AprilTagFieldLayout::GetTagPose(int id) const { Pose3d returnPose; - for (auto& tag : m_apriltags) { - if (tag.id == id) { - returnPose = tag.pose; - } + auto it = std::find_if(m_apriltags.begin(), m_apriltags.end(), + [=](const auto& tag) { return tag.id == id; }); + if (it != m_apriltags.end()) { + returnPose = it->pose; } if (m_mirror) { returnPose = returnPose.RelativeTo( From 8619734fbd2cc27624420a6a0e9242beafc0c19b Mon Sep 17 00:00:00 2001 From: brennenputh Date: Mon, 19 Sep 2022 19:15:12 -0400 Subject: [PATCH 24/73] Remove GetTags from AprilTagFieldLayout C++ --- .../src/main/native/cpp/AprilTagFieldLayout.cpp | 7 ++----- .../main/native/include/frc/AprilTagFieldLayout.h | 1 + wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp | 15 ++++++--------- 3 files changed, 9 insertions(+), 14 deletions(-) diff --git a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp index 2a11ec0a2ea..28edb37a646 100644 --- a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -5,12 +5,14 @@ #include "frc/AprilTagFieldLayout.h" #include +#include #include #include #include #include "frc/DriverStation.h" +#include "frc/apriltag/AprilTagUtil.h" #include "frc/geometry/Pose3d.h" using namespace frc; @@ -36,8 +38,3 @@ frc::Pose3d AprilTagFieldLayout::GetTagPose(int id) const { void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { m_mirror = alliance == DriverStation::Alliance::kRed; } - -const std::vector& frc::AprilTagFieldLayout::GetTags() - const { - return m_apriltags; -} diff --git a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h index 97209edf8ff..e9a3ea05cf6 100644 --- a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h @@ -4,6 +4,7 @@ #pragma once +#include #include #include diff --git a/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp index e2c938ffc9c..067fe4adb82 100644 --- a/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp +++ b/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp @@ -4,7 +4,6 @@ #include -#include "frc/AprilTagFieldLayout.h" #include "frc/apriltag/AprilTagUtil.h" #include "frc/geometry/Pose3d.h" #include "gtest/gtest.h" @@ -12,15 +11,13 @@ using namespace frc; TEST(AprilTagJsonTest, DeserializeMatches) { - auto layout = AprilTagFieldLayout{std::vector{ + auto layout = std::vector{ AprilTagUtil::AprilTag{1, Pose3d{}}, AprilTagUtil::AprilTag{ - 3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}}}; + 3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}}; - AprilTagFieldLayout deserialized; - EXPECT_NO_THROW( - deserialized = - AprilTagFieldLayout{AprilTagUtil::DeserializeAprilTagLayout( - AprilTagUtil::SerializeAprilTagLayout(layout.GetTags()))}); - EXPECT_EQ(layout.GetTags(), deserialized.GetTags()); + std::vector deserialized; + EXPECT_NO_THROW(deserialized = AprilTagUtil::DeserializeAprilTagLayout( + AprilTagUtil::SerializeAprilTagLayout(layout))); + EXPECT_EQ(layout, deserialized); } From 1c020df8b640d854649ab8e2fca413d7dbacc0a4 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Mon, 19 Sep 2022 20:07:35 -0400 Subject: [PATCH 25/73] Clean up the Java AprilTagFieldLayout --- .../wpilibj/apriltag/AprilTagFieldLayout.java | 19 ------------------- ...st.java => AprilTagSerializationTest.java} | 18 +++++++----------- 2 files changed, 7 insertions(+), 30 deletions(-) rename wpilibj/src/test/java/edu/wpi/first/wpilibj/{AprilTagFieldLayoutTest.java => AprilTagSerializationTest.java} (54%) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 0b145fb494c..2b42cc3911c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -63,15 +63,6 @@ public void setAlliance(DriverStation.Alliance alliance) { m_mirror = alliance == DriverStation.Alliance.Red; } - /** - * Gets the raw tags. This is not affected by {@link #setAlliance(DriverStation.Alliance)}. - * - * @return map of IDs to poses. - */ - public Map getTags() { - return m_tags; - } - /** * Gets an AprilTag pose by its id. * @@ -90,14 +81,4 @@ public Pose3d getTag(int id) { return tag; } - - /** - * Serialize this layout into a JSON string. - * - * @return the serialized JSON string. - */ - public String serialize() { - return WPIMathJNI.serializeAprilTagLayout( - AprilTagUtil.getElementsFromAprilTagFieldLayout(m_tags)); - } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagSerializationTest.java similarity index 54% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java rename to wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagSerializationTest.java index bb32ac5caab..d7b2c46b3ba 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagFieldLayoutTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagSerializationTest.java @@ -7,30 +7,26 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.WPIMathJNI; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.wpilibj.apriltag.AprilTagFieldLayout; import edu.wpi.first.wpilibj.apriltag.AprilTagUtil; import java.util.Map; import org.junit.jupiter.api.Test; -class AprilTagFieldLayoutTest { +class AprilTagSerializationTest { @Test void deserializeMatches() { var layout = - new AprilTagFieldLayout( - Map.of( - 1, new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)), - 3, new Pose3d(0, 1, 0, new Rotation3d(0, 0, 0)))); + Map.of( + 1, new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)), + 3, new Pose3d(0, 1, 0, new Rotation3d(0, 0, 0))); var deserialized = assertDoesNotThrow( () -> - new AprilTagFieldLayout( - AprilTagUtil.createAprilTagFieldLayoutFromElements( - WPIMathJNI.deserializeAprilTagLayout(layout.serialize())))); + AprilTagUtil.createAprilTagFieldLayoutFromElements( + AprilTagUtil.getElementsFromAprilTagFieldLayout(layout))); - assertEquals(layout.getTags(), deserialized.getTags()); + assertEquals(layout, deserialized); } } From 279b0f43f27323dd87d8bc2c1f3b2af017beb97e Mon Sep 17 00:00:00 2001 From: brennenputh Date: Tue, 20 Sep 2022 12:41:57 -0400 Subject: [PATCH 26/73] Remove leftover function declaration --- wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h index e9a3ea05cf6..9366359041a 100644 --- a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h @@ -25,8 +25,6 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { explicit AprilTagFieldLayout( const std::vector& apriltags); - const std::vector& GetTags() const; - Pose3d GetTagPose(int id) const; void SetAlliance(DriverStation::Alliance alliance); From 6ab6c44a73c77019d3e52bc1d3880124f463d589 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Thu, 22 Sep 2022 18:57:53 -0400 Subject: [PATCH 27/73] Move FromJson and ToJson methods to AprilTagFieldLayout --- .../main/native/cpp/AprilTagFieldLayout.cpp | 32 ++++++++++++++++++- .../native/include/frc/AprilTagFieldLayout.h | 7 +++- .../main/native/cpp/apriltag/AprilTagUtil.cpp | 29 ----------------- 3 files changed, 37 insertions(+), 31 deletions(-) diff --git a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp index 28edb37a646..7b482db3686 100644 --- a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -5,11 +5,14 @@ #include "frc/AprilTagFieldLayout.h" #include -#include +#include +#include #include #include #include +#include +#include #include "frc/DriverStation.h" #include "frc/apriltag/AprilTagUtil.h" @@ -17,6 +20,20 @@ using namespace frc; +AprilTagFieldLayout::AprilTagFieldLayout(const std::string_view path) { + std::error_code error_code; + + wpi::raw_fd_istream input{path, error_code}; + if (error_code) { + throw std::runtime_error(fmt::format("Cannot open file: {}", path)); + } + + wpi::json json; + input >> json; + + m_apriltags = json.get>(); +} + AprilTagFieldLayout::AprilTagFieldLayout( const std::vector& apriltags) : m_apriltags(std::move(apriltags)) {} @@ -38,3 +55,16 @@ frc::Pose3d AprilTagFieldLayout::GetTagPose(int id) const { void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { m_mirror = alliance == DriverStation::Alliance::kRed; } + +void ToJson(const std::vector& apriltagLayout, std::string_view path) { + std::error_code error_code; + + wpi::raw_fd_ostream output{path, error_code}; + if (error_code) { + throw std::runtime_error(fmt::format("Cannot open file: {}", path)); + } + + wpi::json json = apriltagLayout; + output << json; + output.flush(); +} \ No newline at end of file diff --git a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h index 9366359041a..7adb76d9b66 100644 --- a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h @@ -4,7 +4,7 @@ #pragma once -#include +#include #include #include @@ -21,6 +21,8 @@ namespace frc { class WPILIB_DLLEXPORT AprilTagFieldLayout { public: AprilTagFieldLayout() = default; + + explicit AprilTagFieldLayout(const std::string_view path); explicit AprilTagFieldLayout( const std::vector& apriltags); @@ -28,6 +30,9 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { Pose3d GetTagPose(int id) const; void SetAlliance(DriverStation::Alliance alliance); + + void ToJson(const std::vector& apriltagLayout, + std::string_view path); private: std::vector m_apriltags; diff --git a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp index 1165ed58dcf..2db8983a594 100644 --- a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp +++ b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp @@ -22,35 +22,6 @@ bool AprilTagUtil::AprilTag::operator!=(const AprilTag& other) const { return !operator==(other); } -void AprilTagUtil::ToJson(const std::vector& apriltagLayout, - std::string_view path) { - std::error_code error_code; - - wpi::raw_fd_ostream output{path, error_code}; - if (error_code) { - throw std::runtime_error(fmt::format("Cannot open file: {}", path)); - } - - wpi::json json = apriltagLayout; - output << json; - output.flush(); -} - -std::vector AprilTagUtil::FromJson( - std::string_view path) { - std::error_code error_code; - - wpi::raw_fd_istream input{path, error_code}; - if (error_code) { - throw std::runtime_error(fmt::format("Cannot open file: {}", path)); - } - - wpi::json json; - input >> json; - - return json.get>(); -} - std::string AprilTagUtil::SerializeAprilTagLayout( const std::vector& apriltagLayout) { wpi::json json = apriltagLayout; From 15fa8275037d577c1e297850d9ca852b4a1d78e4 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Thu, 22 Sep 2022 20:10:49 -0400 Subject: [PATCH 28/73] TESTS BROKEN (large refactor part 1) --- .../src/main/native/cpp/apriltag/AprilTag.cpp | 26 ++++++++++ .../{ => apriltag}/AprilTagFieldLayout.cpp | 21 +++++--- .../native/include/frc/apriltag/AprilTag.h | 40 +++++++++++++++ .../frc/{ => apriltag}/AprilTagFieldLayout.h | 23 ++++++--- .../src/test/native/cpp/AprilTagJsonTest.cpp | 23 --------- .../native/cpp/apriltag/AprilTagJsonTest.cpp | 24 +++++++++ .../main/native/cpp/apriltag/AprilTagUtil.cpp | 28 +++------- .../src/main/native/cpp/jni/WPIMathJNI.cpp | 26 +++++----- .../include/frc/apriltag/AprilTagUtil.h | 51 ++----------------- 9 files changed, 143 insertions(+), 119 deletions(-) create mode 100644 wpilibc/src/main/native/cpp/apriltag/AprilTag.cpp rename wpilibc/src/main/native/cpp/{ => apriltag}/AprilTagFieldLayout.cpp (74%) create mode 100644 wpilibc/src/main/native/include/frc/apriltag/AprilTag.h rename wpilibc/src/main/native/include/frc/{ => apriltag}/AprilTagFieldLayout.h (63%) delete mode 100644 wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp create mode 100644 wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTag.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTag.cpp new file mode 100644 index 00000000000..86fa29e5299 --- /dev/null +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTag.cpp @@ -0,0 +1,26 @@ +// 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/AprilTag.h" + +#include + +using namespace frc; + +bool AprilTag::operator==(const AprilTag& other) const { + return id == other.id && pose == other.pose; +} + +bool AprilTag::operator!=(const AprilTag& other) const { + return !operator==(other); +} + +void frc::to_json(wpi::json& json, const AprilTag& apriltag) { + json = wpi::json{{"id", apriltag.id}, {"pose", apriltag.pose}}; +} + +void frc::from_json(const wpi::json& json, AprilTag& apriltag) { + apriltag.id = json.at("id").get(); + apriltag.pose = json.at("pose").get(); +} diff --git a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp similarity index 74% rename from wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp rename to wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index 7b482db3686..92e10fef83f 100644 --- a/wpilibc/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -2,11 +2,12 @@ // 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/AprilTagFieldLayout.h" +#include "frc/apriltag/AprilTagFieldLayout.h" #include #include #include +#include #include #include @@ -15,7 +16,6 @@ #include #include "frc/DriverStation.h" -#include "frc/apriltag/AprilTagUtil.h" #include "frc/geometry/Pose3d.h" using namespace frc; @@ -31,11 +31,10 @@ AprilTagFieldLayout::AprilTagFieldLayout(const std::string_view path) { wpi::json json; input >> json; - m_apriltags = json.get>(); + m_apriltags = json.get>(); } -AprilTagFieldLayout::AprilTagFieldLayout( - const std::vector& apriltags) +AprilTagFieldLayout::AprilTagFieldLayout(const std::vector& apriltags) : m_apriltags(std::move(apriltags)) {} frc::Pose3d AprilTagFieldLayout::GetTagPose(int id) const { @@ -56,7 +55,7 @@ void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { m_mirror = alliance == DriverStation::Alliance::kRed; } -void ToJson(const std::vector& apriltagLayout, std::string_view path) { +void ToJson(const AprilTagFieldLayout& apriltagLayout, std::string_view path) { std::error_code error_code; wpi::raw_fd_ostream output{path, error_code}; @@ -67,4 +66,12 @@ void ToJson(const std::vector& apriltagLayout, std::stri wpi::json json = apriltagLayout; output << json; output.flush(); -} \ No newline at end of file +} + +void frc::to_json(wpi::json& json, const AprilTagFieldLayout& layout) { + json = wpi::json{{"tags", layout.m_apriltags}}; +} + +void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) { + layout.m_apriltags = json.at("tags").get>(); +} diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h new file mode 100644 index 00000000000..4f1b23d9615 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h @@ -0,0 +1,40 @@ +// 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 + +#include "frc/geometry/Pose3d.h" + +namespace frc { +struct WPILIB_DLLEXPORT AprilTag { + int id; + + Pose3d pose; + + /** + * Checks equality between this AprilTag and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const AprilTag& other) const; + + /** + * Checks inequality between this AprilTag and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const AprilTag& other) const; +}; + +WPILIB_DLLEXPORT +void to_json(wpi::json& json, const AprilTag& apriltag); + +WPILIB_DLLEXPORT +void from_json(const wpi::json& json, AprilTag& apriltag); + +} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h similarity index 63% rename from wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h rename to wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 7adb76d9b66..14549f9bb42 100644 --- a/wpilibc/src/main/native/include/frc/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -4,13 +4,14 @@ #pragma once +#include #include #include #include #include "frc/DriverStation.h" -#include "frc/apriltag/AprilTagUtil.h" +#include "frc/apriltag/AprilTag.h" #include "frc/geometry/Pose3d.h" namespace wpi { @@ -20,22 +21,28 @@ class json; namespace frc { class WPILIB_DLLEXPORT AprilTagFieldLayout { public: + std::vector m_apriltags; + AprilTagFieldLayout() = default; - + explicit AprilTagFieldLayout(const std::string_view path); - explicit AprilTagFieldLayout( - const std::vector& apriltags); + explicit AprilTagFieldLayout(const std::vector& apriltags); Pose3d GetTagPose(int id) const; void SetAlliance(DriverStation::Alliance alliance); - - void ToJson(const std::vector& apriltagLayout, - std::string_view path); + + std::string ToJson(const AprilTagFieldLayout& apriltagLayout); private: - std::vector m_apriltags; bool m_mirror; }; + +WPILIB_DLLEXPORT +void to_json(wpi::json& json, const AprilTagFieldLayout& layout); + +WPILIB_DLLEXPORT +void from_json(const wpi::json& json, AprilTagFieldLayout& layout); + } // namespace frc diff --git a/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp deleted file mode 100644 index 067fe4adb82..00000000000 --- a/wpilibc/src/test/native/cpp/AprilTagJsonTest.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// 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 - -#include "frc/apriltag/AprilTagUtil.h" -#include "frc/geometry/Pose3d.h" -#include "gtest/gtest.h" - -using namespace frc; - -TEST(AprilTagJsonTest, DeserializeMatches) { - auto layout = std::vector{ - AprilTagUtil::AprilTag{1, Pose3d{}}, - AprilTagUtil::AprilTag{ - 3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}}; - - std::vector deserialized; - EXPECT_NO_THROW(deserialized = AprilTagUtil::DeserializeAprilTagLayout( - AprilTagUtil::SerializeAprilTagLayout(layout))); - EXPECT_EQ(layout, deserialized); -} diff --git a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp new file mode 100644 index 00000000000..e91742e47ab --- /dev/null +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp @@ -0,0 +1,24 @@ +// 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 +#include +#include + +#include "frc/apriltag/AprilTag.h" +#include "frc/apriltag/AprilTagFieldLayout.h" +#include "frc/geometry/Pose3d.h" +#include "gtest/gtest.h" + +using namespace frc; + +TEST(AprilTagJsonTest, DeserializeMatches) { + auto layout = AprilTagFieldLayout{std::vector{ + AprilTag{1, Pose3d{}}, + AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}}}; + + AprilTagFieldLayout deserialized; + EXPECT_NO_THROW(deserialized = AprilTagFieldLayout(layout.ToJson(layout))); + EXPECT_EQ(layout, deserialized); +} diff --git a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp index 2db8983a594..40ed2f67177 100644 --- a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp +++ b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp @@ -5,6 +5,7 @@ #include "frc/apriltag/AprilTagUtil.h" #include +#include #include #include @@ -12,33 +13,18 @@ #include #include -using namespace frc; - -bool AprilTagUtil::AprilTag::operator==(const AprilTag& other) const { - return id == other.id && pose == other.pose; -} +#include "frc/geometry/Pose3d.h" -bool AprilTagUtil::AprilTag::operator!=(const AprilTag& other) const { - return !operator==(other); -} +using namespace frc; std::string AprilTagUtil::SerializeAprilTagLayout( - const std::vector& apriltagLayout) { + const std::vector>& apriltagLayout) { wpi::json json = apriltagLayout; - return json.dump(); + return json.at("tags").dump(); } -std::vector AprilTagUtil::DeserializeAprilTagLayout( +std::vector> AprilTagUtil::DeserializeAprilTagLayout( std::string_view jsonStr) { wpi::json json = wpi::json::parse(jsonStr); - return json.get>(); -} - -void frc::to_json(wpi::json& json, const AprilTagUtil::AprilTag& apriltag) { - json = wpi::json{{"id", apriltag.id}, {"pose", apriltag.pose}}; -} - -void frc::from_json(const wpi::json& json, AprilTagUtil::AprilTag& apriltag) { - apriltag.id = json.at("id").get(); - apriltag.pose = json.at("pose").get(); + return json.at("tags").get>>(); } diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index 71b157bdd30..a61bd623794 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -5,6 +5,7 @@ #include #include +#include #include @@ -15,6 +16,7 @@ #include "drake/math/discrete_algebraic_riccati_equation.h" #include "edu_wpi_first_math_WPIMathJNI.h" #include "frc/apriltag/AprilTagUtil.h" +#include "frc/geometry/Pose3d.h" #include "frc/trajectory/TrajectoryUtil.h" #include "unsupported/Eigen/MatrixFunctions" @@ -95,35 +97,35 @@ frc::Trajectory CreateTrajectoryFromElements(wpi::span elements) { } std::vector GetElementsFromAprilTagLayout( - const std::vector& apriltagFieldLayout) { + const std::vector>& apriltagFieldLayout) { std::vector elements; elements.reserve(apriltagFieldLayout.size() * 8); for (auto&& apriltag : apriltagFieldLayout) { - elements.push_back(apriltag.id); - elements.push_back(apriltag.pose.X().value()); - elements.push_back(apriltag.pose.Y().value()); - elements.push_back(apriltag.pose.Z().value()); - elements.push_back(apriltag.pose.Rotation().GetQuaternion().W()); - elements.push_back(apriltag.pose.Rotation().GetQuaternion().X()); - elements.push_back(apriltag.pose.Rotation().GetQuaternion().Y()); - elements.push_back(apriltag.pose.Rotation().GetQuaternion().Z()); + elements.push_back(apriltag.first); + elements.push_back(apriltag.second.X().value()); + elements.push_back(apriltag.second.Y().value()); + elements.push_back(apriltag.second.Z().value()); + elements.push_back(apriltag.second.Rotation().GetQuaternion().W()); + elements.push_back(apriltag.second.Rotation().GetQuaternion().X()); + elements.push_back(apriltag.second.Rotation().GetQuaternion().Y()); + elements.push_back(apriltag.second.Rotation().GetQuaternion().Z()); } return elements; } -std::vector CreateAprilTagLayoutFromElements( +std::vector> CreateAprilTagLayoutFromElements( wpi::span elements) { // Make sure that the elements have the correct length. assert(elements.size() % 8 == 0); // Create a vector of AprilTags from the elements. - std::vector apriltags; + std::vector> apriltags; apriltags.reserve(elements.size() / 8); for (size_t i = 0; i < elements.size(); i += 8) { - apriltags.emplace_back(frc::AprilTagUtil::AprilTag{ + apriltags.emplace_back(std::pair{ static_cast(elements[i]), frc::Pose3d{units::meter_t{elements[i + 1]}, units::meter_t{elements[i + 2]}, diff --git a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h index 1a0125cd1da..f909b3334ac 100644 --- a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h +++ b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h @@ -5,6 +5,7 @@ #pragma once #include +#include #include #include @@ -16,45 +17,6 @@ class WPILIB_DLLEXPORT AprilTagUtil { public: AprilTagUtil() = delete; - struct WPILIB_DLLEXPORT AprilTag { - int id; - - Pose3d pose; - - /** - * Checks equality between this AprilTag and another object. - * - * @param other The other object. - * @return Whether the two objects are equal. - */ - bool operator==(const AprilTag& other) const; - - /** - * Checks inequality between this AprilTag and another object. - * - * @param other The other object. - * @return Whether the two objects are not equal. - */ - bool operator!=(const AprilTag& other) const; - }; - - /** - * Exports a AprilTag Layout JSON file. - * - * @param apriltagLayout the AprilTag layout to export - * @param path the path of the file to export to - */ - static void ToJson(const std::vector& apriltagLayout, - std::string_view path); - /** - * Imports a AprilTag Layout JSON file. - * - * @param path The path of the json file to import from. - * - * @return The AprilTag layout represented by the file. - */ - static std::vector FromJson(std::string_view path); - /** * Deserializes a AprilTag layout JSON. * @@ -63,7 +25,7 @@ class WPILIB_DLLEXPORT AprilTagUtil { * @return the string containing the serialized JSON */ static std::string SerializeAprilTagLayout( - const std::vector& apriltagLayout); + const std::vector>& apriltagLayout); /** * Serializes a AprilTag layout JSON. @@ -72,14 +34,7 @@ class WPILIB_DLLEXPORT AprilTagUtil { * * @return the AprilTag layout represented by the JSON */ - static std::vector DeserializeAprilTagLayout( + static std::vector> DeserializeAprilTagLayout( std::string_view jsonStr); }; - -WPILIB_DLLEXPORT -void to_json(wpi::json& json, const AprilTagUtil::AprilTag& apriltag); - -WPILIB_DLLEXPORT -void from_json(const wpi::json& json, AprilTagUtil::AprilTag& apriltag); - } // namespace frc From a691ebc6a57ffd36784982a86c8f0156d946a0ac Mon Sep 17 00:00:00 2001 From: brennenputh Date: Fri, 23 Sep 2022 20:32:40 -0400 Subject: [PATCH 29/73] PART 2 OF POSSIBLY MANY --- .../native/cpp/apriltag/AprilTagFieldLayout.cpp | 16 ++++++++++++---- .../main/native/include/frc/apriltag/AprilTag.h | 4 ++++ .../include/frc/apriltag/AprilTagFieldLayout.h | 6 +++++- .../native/cpp/apriltag/AprilTagJsonTest.cpp | 2 +- 4 files changed, 22 insertions(+), 6 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index 92e10fef83f..bc98d3d4c1e 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -20,7 +20,7 @@ using namespace frc; -AprilTagFieldLayout::AprilTagFieldLayout(const std::string_view path) { +AprilTagFieldLayout::AprilTagFieldLayout(const std::string_view path): AprilTagFieldLayout() { std::error_code error_code; wpi::raw_fd_istream input{path, error_code}; @@ -31,7 +31,7 @@ AprilTagFieldLayout::AprilTagFieldLayout(const std::string_view path) { wpi::json json; input >> json; - m_apriltags = json.get>(); + this->m_apriltags = json.get>(); } AprilTagFieldLayout::AprilTagFieldLayout(const std::vector& apriltags) @@ -55,7 +55,7 @@ void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { m_mirror = alliance == DriverStation::Alliance::kRed; } -void ToJson(const AprilTagFieldLayout& apriltagLayout, std::string_view path) { +void AprilTagFieldLayout::ToJson(std::string_view path) { std::error_code error_code; wpi::raw_fd_ostream output{path, error_code}; @@ -63,11 +63,19 @@ void ToJson(const AprilTagFieldLayout& apriltagLayout, std::string_view path) { throw std::runtime_error(fmt::format("Cannot open file: {}", path)); } - wpi::json json = apriltagLayout; + wpi::json json = this; output << json; output.flush(); } +bool AprilTagFieldLayout::operator==(const AprilTagFieldLayout& other) const { + return m_apriltags == other.m_apriltags && m_mirror == other.m_mirror; +} + +bool AprilTagFieldLayout::operator!=(const AprilTagFieldLayout& other) const { + return !operator==(other); +} + void frc::to_json(wpi::json& json, const AprilTagFieldLayout& layout) { json = wpi::json{{"tags", layout.m_apriltags}}; } diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h index 4f1b23d9615..4079a985d3b 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h @@ -8,6 +8,10 @@ #include "frc/geometry/Pose3d.h" +namespace wpi { +class json; +} + namespace frc { struct WPILIB_DLLEXPORT AprilTag { int id; diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 14549f9bb42..8852cf006eb 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -33,7 +33,11 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { void SetAlliance(DriverStation::Alliance alliance); - std::string ToJson(const AprilTagFieldLayout& apriltagLayout); + void ToJson(std::string_view path); + + bool operator==(const AprilTagFieldLayout& other) const; + + bool operator!=(const AprilTagFieldLayout& other) const; private: bool m_mirror; diff --git a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp index e91742e47ab..e20c006e331 100644 --- a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp @@ -19,6 +19,6 @@ TEST(AprilTagJsonTest, DeserializeMatches) { AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}}}; AprilTagFieldLayout deserialized; - EXPECT_NO_THROW(deserialized = AprilTagFieldLayout(layout.ToJson(layout))); + EXPECT_NO_THROW(deserialized = AprilTagFieldLayout{}); EXPECT_EQ(layout, deserialized); } From 6e7ab6e0acebc2cfde2bde391a729c199e6e1893 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Fri, 23 Sep 2022 20:45:33 -0400 Subject: [PATCH 30/73] Make general improvements --- wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp | 2 +- .../src/main/native/include/frc/apriltag/AprilTagFieldLayout.h | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index bc98d3d4c1e..0e6cb94ec8c 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -20,7 +20,7 @@ using namespace frc; -AprilTagFieldLayout::AprilTagFieldLayout(const std::string_view path): AprilTagFieldLayout() { +AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) { std::error_code error_code; wpi::raw_fd_istream input{path, error_code}; diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 8852cf006eb..994ed8d4d94 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -4,7 +4,6 @@ #pragma once -#include #include #include @@ -25,7 +24,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { AprilTagFieldLayout() = default; - explicit AprilTagFieldLayout(const std::string_view path); + explicit AprilTagFieldLayout(std::string_view path); explicit AprilTagFieldLayout(const std::vector& apriltags); From d983d8e6bd74700984bf07c7ae5cf1c8981bee5a Mon Sep 17 00:00:00 2001 From: brennenputh Date: Fri, 23 Sep 2022 20:45:51 -0400 Subject: [PATCH 31/73] A single asterisk --- wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index 0e6cb94ec8c..3816a21e918 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -63,7 +63,7 @@ void AprilTagFieldLayout::ToJson(std::string_view path) { throw std::runtime_error(fmt::format("Cannot open file: {}", path)); } - wpi::json json = this; + wpi::json json = *this; output << json; output.flush(); } From ff77fbc9e223f292dd604258fb99c6b7d936d6bc Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 25 Sep 2022 11:39:39 -0400 Subject: [PATCH 32/73] Initialize default value for m_mirror, fixes tests --- .../main/native/include/frc/apriltag/AprilTagFieldLayout.h | 2 +- wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 994ed8d4d94..4951f12cffd 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -39,7 +39,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { bool operator!=(const AprilTagFieldLayout& other) const; private: - bool m_mirror; + bool m_mirror = false; }; WPILIB_DLLEXPORT diff --git a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp index e20c006e331..08779405b62 100644 --- a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp @@ -10,15 +10,16 @@ #include "frc/apriltag/AprilTagFieldLayout.h" #include "frc/geometry/Pose3d.h" #include "gtest/gtest.h" +#include "wpi/json.h" using namespace frc; TEST(AprilTagJsonTest, DeserializeMatches) { auto layout = AprilTagFieldLayout{std::vector{ - AprilTag{1, Pose3d{}}, AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}}}; AprilTagFieldLayout deserialized; - EXPECT_NO_THROW(deserialized = AprilTagFieldLayout{}); + wpi::json json = layout; + EXPECT_NO_THROW(deserialized = json.get()); EXPECT_EQ(layout, deserialized); } From 14bafe0229f2ff43b7851cc65b6edae2d9d4c3c8 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 25 Sep 2022 17:58:10 -0400 Subject: [PATCH 33/73] Remove JNI and use Jackson instead, make tests consistent between C++ and Java --- .../native/cpp/apriltag/AprilTagJsonTest.cpp | 1 + .../wpi/first/wpilibj/apriltag/AprilTag.java | 40 ++++++++ .../wpilibj/apriltag/AprilTagFieldLayout.java | 82 ++++++++++++---- .../AprilTagSerializationTest.java | 28 +++--- .../java/edu/wpi/first/math/WPIMathJNI.java | 19 +--- .../edu/wpi/first/math/geometry/Pose3d.java | 17 +++- .../wpi/first/math/geometry/Rotation3d.java | 22 ++++- .../first/math/geometry/Translation3d.java | 19 +++- .../main/native/cpp/apriltag/AprilTagUtil.cpp | 30 ------ .../src/main/native/cpp/jni/WPIMathJNI.cpp | 94 ------------------- .../include/frc/apriltag/AprilTagUtil.h | 40 -------- 11 files changed, 171 insertions(+), 221 deletions(-) create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java rename wpilibj/src/test/java/edu/wpi/first/wpilibj/{ => apriltag}/AprilTagSerializationTest.java (56%) delete mode 100644 wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp delete mode 100644 wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h diff --git a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp index 08779405b62..fc127da3b64 100644 --- a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp @@ -16,6 +16,7 @@ using namespace frc; TEST(AprilTagJsonTest, DeserializeMatches) { auto layout = AprilTagFieldLayout{std::vector{ + AprilTag{1, Pose3d{}}, AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}}}; AprilTagFieldLayout deserialized; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java new file mode 100644 index 00000000000..fd30b64b683 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java @@ -0,0 +1,40 @@ +package edu.wpi.first.wpilibj.apriltag; + +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonProperty; + +import java.util.Objects; + +import edu.wpi.first.math.geometry.Pose3d; + +public class AprilTag { + public int id; + public Pose3d pose; + + @JsonCreator + public AprilTag( + @JsonProperty(required = true, value = "id") int id, + @JsonProperty(required = true, value = "pose") Pose3d pose) { + this.id = id; + this.pose = pose; + } + + @Override + public boolean equals(Object obj) { + if (obj instanceof AprilTag) { + var other = (AprilTag) obj; + return id == other.id && pose.equals(other.pose); + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(id, pose); + } + + @Override + public String toString() { + return "AprilTag(id: " + id + ", pose: " + pose + ")"; + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 2b42cc3911c..16e117d814f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -4,20 +4,29 @@ package edu.wpi.first.wpilibj.apriltag; -import edu.wpi.first.math.WPIMathJNI; +import com.fasterxml.jackson.annotation.JsonAutoDetect; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.databind.ObjectMapper; + +import java.io.IOException; +import java.nio.file.Path; +import java.util.ArrayList; +import java.util.List; +import java.util.Objects; + 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.io.IOException; -import java.nio.file.Files; -import java.nio.file.Path; -import java.util.HashMap; -import java.util.Map; +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class AprilTagFieldLayout { - private final Map m_tags = new HashMap<>(); + @JsonProperty(value = "tags") + private final List m_tags = new ArrayList<>(); private boolean m_mirror; /** @@ -37,9 +46,7 @@ public AprilTagFieldLayout(String path) throws IOException { * @throws IOException if reading from the file fails. */ public AprilTagFieldLayout(Path path) throws IOException { - this( - AprilTagUtil.createAprilTagFieldLayoutFromElements( - WPIMathJNI.deserializeAprilTagLayout(Files.readString(path)))); + this(new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class).m_tags); } /** @@ -47,9 +54,11 @@ public AprilTagFieldLayout(Path path) throws IOException { * * @param tags map of IDs to poses */ - public AprilTagFieldLayout(Map tags) { - // To ensure the underlying semantics don't change with what kind of map is passed in - m_tags.putAll(tags); + @JsonCreator + public AprilTagFieldLayout( + @JsonProperty(required = true, value = "tags") List tags) { + // To ensure the underlying semantics don't change with what kind of list is passed in + m_tags.addAll(tags); } /** @@ -70,15 +79,48 @@ public void setAlliance(DriverStation.Alliance alliance) { * @return The pose corresponding to the id passed in. */ public Pose3d getTag(int id) { - Pose3d tag = m_tags.get(id); + Pose3d pose = m_tags.stream().filter((it) -> id == it.id).findFirst().get().pose; if (m_mirror) { - tag = - tag.relativeTo( - new Pose3d( - new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0), - new Rotation3d(0.0, 0.0, 180.0))); + pose = pose.relativeTo( + new Pose3d( + new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0), + new Rotation3d(0.0, 0.0, 180.0))); } - return tag; + return pose; + } + + /** + * Serializes a AprilTagFieldLayout to a JSON file. + * + * @param path The path to write to + * @throws IOException if writing to the file fails + */ + public void serialize(String path) throws IOException { + serialize(Path.of(path)); + } + + /** + * Serializes a AprilTagFieldLayout to a JSON file. + * + * @param path The path to write to + * @throws IOException if writing to the file fails + */ + public void serialize(Path path) throws IOException { + new ObjectMapper().writeValue(path.toFile(), m_tags); + } + + @Override + public boolean equals(Object obj) { + if(obj instanceof AprilTagFieldLayout) { + var other = (AprilTagFieldLayout) obj; + return m_tags.equals(other.m_tags) && m_mirror == other.m_mirror; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_tags, m_mirror); } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagSerializationTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java similarity index 56% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagSerializationTest.java rename to wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java index d7b2c46b3ba..e980c435d38 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/AprilTagSerializationTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java @@ -2,30 +2,32 @@ // 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; +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 org.junit.jupiter.api.Test; + +import java.util.List; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.wpilibj.apriltag.AprilTagUtil; -import java.util.Map; -import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; class AprilTagSerializationTest { @Test void deserializeMatches() { - var layout = - Map.of( - 1, new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0)), - 3, new Pose3d(0, 1, 0, new Rotation3d(0, 0, 0))); + 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))))); + + var objectMapper = new ObjectMapper(); var deserialized = assertDoesNotThrow( - () -> - AprilTagUtil.createAprilTagFieldLayoutFromElements( - AprilTagUtil.getElementsFromAprilTagFieldLayout(layout))); + () -> objectMapper.readValue(objectMapper.writeValueAsString(layout), AprilTagFieldLayout.class)); assertEquals(layout, deserialized); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index 4b63eaf15f4..adbf4bf17e0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -4,10 +4,11 @@ package edu.wpi.first.math; -import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import java.util.concurrent.atomic.AtomicBoolean; +import edu.wpi.first.util.RuntimeLoader; + public final class WPIMathJNI { static boolean libraryLoaded = false; static RuntimeLoader loader = null; @@ -138,22 +139,6 @@ public static native void discreteAlgebraicRiccatiEquation( public static native void rankUpdate( double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular); - /** - * Deserializes an AprilTag layout JSON into a double[] of elements. - * - * @param json The JSON containing the serialized trajectory. - * @return A double array with the Apriltag layout. - */ - public static native double[] deserializeAprilTagLayout(String json); - - /** - * Serializes the AprilTag layout into a JSON string. - * - * @param elements The elements of the layout. - * @return A JSON containing the serialized layout. - */ - public static native String serializeAprilTagLayout(double[] elements); - public static class Helper { private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index 2be3dbfd3a7..df14668e0c0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -4,6 +4,13 @@ package edu.wpi.first.math.geometry; +import com.fasterxml.jackson.annotation.JsonAutoDetect; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; +import com.fasterxml.jackson.annotation.JsonProperty; + +import java.util.Objects; + import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -11,9 +18,10 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; -import java.util.Objects; /** Represents a 3D pose containing translational and rotational elements. */ +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Pose3d implements Interpolatable { private final Translation3d m_translation; private final Rotation3d m_rotation; @@ -30,7 +38,10 @@ public Pose3d() { * @param translation The translational component of the pose. * @param rotation The rotational component of the pose. */ - public Pose3d(Translation3d translation, Rotation3d rotation) { + @JsonCreator + public Pose3d( + @JsonProperty(required = true, value = "translation") Translation3d translation, + @JsonProperty(required = true, value = "rotation") Rotation3d rotation) { m_translation = translation; m_rotation = rotation; } @@ -74,6 +85,7 @@ public Transform3d minus(Pose3d other) { * * @return The translational component of the pose. */ + @JsonProperty public Translation3d getTranslation() { return m_translation; } @@ -110,6 +122,7 @@ public double getZ() { * * @return The rotational component of the pose. */ + @JsonProperty public Rotation3d getRotation() { return m_rotation; } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index c3dc7b796ab..0f874b416f7 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -4,6 +4,15 @@ package edu.wpi.first.math.geometry; +import com.fasterxml.jackson.annotation.JsonAutoDetect; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; +import com.fasterxml.jackson.annotation.JsonProperty; + +import org.ejml.dense.row.factory.DecompositionFactory_DDRM; + +import java.util.Objects; + import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; @@ -13,10 +22,10 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; -import java.util.Objects; -import org.ejml.dense.row.factory.DecompositionFactory_DDRM; /** A rotation in a 3D coordinate frame represented by a quaternion. */ +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Rotation3d implements Interpolatable { private Quaternion m_q = new Quaternion(); @@ -42,7 +51,11 @@ public Rotation3d(Quaternion q) { * @param pitch The counterclockwise rotation angle around the Y axis (pitch) in radians. * @param yaw The counterclockwise rotation angle around the Z axis (yaw) in radians. */ - public Rotation3d(double roll, double pitch, double yaw) { + @JsonCreator + public Rotation3d( + @JsonProperty(required = true, value = "roll") double roll, + @JsonProperty(required = true, value = "pitch") double pitch, + @JsonProperty(required = true, value = "yaw") double yaw) { // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion double cr = Math.cos(roll * 0.5); double sr = Math.sin(roll * 0.5); @@ -270,6 +283,7 @@ public Quaternion getQuaternion() { * * @return The counterclockwise rotation angle around the X axis (roll) in radians. */ + @JsonProperty(value = "yaw") public double getX() { final var w = m_q.getW(); final var x = m_q.getX(); @@ -285,6 +299,7 @@ public double getX() { * * @return The counterclockwise rotation angle around the Y axis (pitch) in radians. */ + @JsonProperty(value = "pitch") public double getY() { final var w = m_q.getW(); final var x = m_q.getX(); @@ -305,6 +320,7 @@ public double getY() { * * @return The counterclockwise rotation angle around the Z axis (yaw) in radians. */ + @JsonProperty(value = "roll") public double getZ() { final var w = m_q.getW(); final var x = m_q.getX(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index f8f8957b126..baf42c5ae7a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -4,9 +4,15 @@ package edu.wpi.first.math.geometry; +import com.fasterxml.jackson.annotation.JsonAutoDetect; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; +import com.fasterxml.jackson.annotation.JsonProperty; + +import java.util.Objects; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.interpolation.Interpolatable; -import java.util.Objects; /** * Represents a translation in 3D space. This object can be used to represent a point or a vector. @@ -15,6 +21,8 @@ * origin facing in the positive X direction, forward is positive X, left is positive Y, and up is * positive Z. */ +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) @SuppressWarnings({"ParameterName", "MemberName"}) public class Translation3d implements Interpolatable { private final double m_x; @@ -33,7 +41,11 @@ public Translation3d() { * @param y The y component of the translation. * @param z The z component of the translation. */ - public Translation3d(double x, double y, double z) { + @JsonCreator + public Translation3d( + @JsonProperty(required = true, value = "x") double x, + @JsonProperty(required = true, value = "y") double y, + @JsonProperty(required = true, value = "z") double z) { m_x = x; m_y = y; m_z = z; @@ -71,6 +83,7 @@ public double getDistance(Translation3d other) { * * @return The X component of the translation. */ + @JsonProperty public double getX() { return m_x; } @@ -80,6 +93,7 @@ public double getX() { * * @return The Y component of the translation. */ + @JsonProperty public double getY() { return m_y; } @@ -89,6 +103,7 @@ public double getY() { * * @return The Z component of the translation. */ + @JsonProperty public double getZ() { return m_z; } diff --git a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp b/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp deleted file mode 100644 index 40ed2f67177..00000000000 --- a/wpimath/src/main/native/cpp/apriltag/AprilTagUtil.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// 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/AprilTagUtil.h" - -#include -#include -#include - -#include -#include -#include -#include - -#include "frc/geometry/Pose3d.h" - -using namespace frc; - -std::string AprilTagUtil::SerializeAprilTagLayout( - const std::vector>& apriltagLayout) { - wpi::json json = apriltagLayout; - return json.at("tags").dump(); -} - -std::vector> AprilTagUtil::DeserializeAprilTagLayout( - std::string_view jsonStr) { - wpi::json json = wpi::json::parse(jsonStr); - return json.at("tags").get>>(); -} diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index a61bd623794..71152bdc612 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -15,7 +15,6 @@ #include "Eigen/QR" #include "drake/math/discrete_algebraic_riccati_equation.h" #include "edu_wpi_first_math_WPIMathJNI.h" -#include "frc/apriltag/AprilTagUtil.h" #include "frc/geometry/Pose3d.h" #include "frc/trajectory/TrajectoryUtil.h" #include "unsupported/Eigen/MatrixFunctions" @@ -96,48 +95,6 @@ frc::Trajectory CreateTrajectoryFromElements(wpi::span elements) { return frc::Trajectory(states); } -std::vector GetElementsFromAprilTagLayout( - const std::vector>& apriltagFieldLayout) { - std::vector elements; - elements.reserve(apriltagFieldLayout.size() * 8); - - for (auto&& apriltag : apriltagFieldLayout) { - elements.push_back(apriltag.first); - elements.push_back(apriltag.second.X().value()); - elements.push_back(apriltag.second.Y().value()); - elements.push_back(apriltag.second.Z().value()); - elements.push_back(apriltag.second.Rotation().GetQuaternion().W()); - elements.push_back(apriltag.second.Rotation().GetQuaternion().X()); - elements.push_back(apriltag.second.Rotation().GetQuaternion().Y()); - elements.push_back(apriltag.second.Rotation().GetQuaternion().Z()); - } - - return elements; -} - -std::vector> CreateAprilTagLayoutFromElements( - wpi::span elements) { - // Make sure that the elements have the correct length. - assert(elements.size() % 8 == 0); - - // Create a vector of AprilTags from the elements. - std::vector> apriltags; - apriltags.reserve(elements.size() / 8); - - for (size_t i = 0; i < elements.size(); i += 8) { - apriltags.emplace_back(std::pair{ - static_cast(elements[i]), - frc::Pose3d{units::meter_t{elements[i + 1]}, - units::meter_t{elements[i + 2]}, - units::meter_t{elements[i + 3]}, - frc::Rotation3d{ - frc::Quaternion{elements[i + 4], elements[i + 5], - elements[i + 6], elements[i + 7]}}}}); - } - - return apriltags; -} - extern "C" { /* @@ -353,57 +310,6 @@ Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory } } -/* - * Class: edu_wpi_first_math_WPIMathJNI - * Method: deserializeAprilTagLayout - * Signature: (Ljava/lang/String;)[D - */ -JNIEXPORT jdoubleArray JNICALL -Java_edu_wpi_first_math_WPIMathJNI_deserializeAprilTagLayout - (JNIEnv* env, jclass, jstring json) -{ - try { - auto apriltagFieldLayout = frc::AprilTagUtil::DeserializeAprilTagLayout( - JStringRef{env, json}.c_str()); - std::vector elements = - GetElementsFromAprilTagLayout(apriltagFieldLayout); - return MakeJDoubleArray(env, elements); - } catch (std::exception& e) { - jclass cls = env->FindClass( - "edu/wpi/first/math/apriltag/AprilTagUtil$" - "AprilTagLayoutSerializationException"); - if (cls) { - env->ThrowNew(cls, e.what()); - } - return nullptr; - } -} - -/* - * Class: edu_wpi_first_math_WPIMathJNI - * Method: serializeAprilTagLayout - * Signature: ([D)Ljava/lang/String; - */ -JNIEXPORT jstring JNICALL -Java_edu_wpi_first_math_WPIMathJNI_serializeAprilTagLayout - (JNIEnv* env, jclass, jdoubleArray elements) -{ - try { - auto apriltagFieldLayout = - CreateAprilTagLayoutFromElements(JDoubleArrayRef{env, elements}); - return MakeJString( - env, frc::AprilTagUtil::SerializeAprilTagLayout(apriltagFieldLayout)); - } catch (std::exception& e) { - jclass cls = env->FindClass( - "edu/wpi/first/math/apriltag/AprilTagUtil$" - "AprilTagLayoutSerializationException"); - if (cls) { - env->ThrowNew(cls, e.what()); - } - return nullptr; - } -} - /* * Class: edu_wpi_first_math_WPIMathJNI * Method: rankUpdate diff --git a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h b/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h deleted file mode 100644 index f909b3334ac..00000000000 --- a/wpimath/src/main/native/include/frc/apriltag/AprilTagUtil.h +++ /dev/null @@ -1,40 +0,0 @@ -// 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 -#include -#include - -#include - -#include "frc/geometry/Pose3d.h" - -namespace frc { -class WPILIB_DLLEXPORT AprilTagUtil { - public: - AprilTagUtil() = delete; - - /** - * Deserializes a AprilTag layout JSON. - * - * @param apriltagLayout the AprilTag layout to export - * - * @return the string containing the serialized JSON - */ - static std::string SerializeAprilTagLayout( - const std::vector>& apriltagLayout); - - /** - * Serializes a AprilTag layout JSON. - * - * @param jsonStr the string containing the serialized JSON - * - * @return the AprilTag layout represented by the JSON - */ - static std::vector> DeserializeAprilTagLayout( - std::string_view jsonStr); -}; -} // namespace frc From 8fe658b8e4a7c1d21226d3cd3fcb27264c9bc373 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 25 Sep 2022 18:10:11 -0400 Subject: [PATCH 34/73] Fix check --- .../wpi/first/wpilibj/apriltag/AprilTag.java | 21 +++++++------- .../wpilibj/apriltag/AprilTagFieldLayout.java | 29 +++++++++---------- .../apriltag/AprilTagSerializationTest.java | 25 ++++++++-------- .../java/edu/wpi/first/math/WPIMathJNI.java | 3 +- .../edu/wpi/first/math/geometry/Pose3d.java | 4 +-- .../wpi/first/math/geometry/Rotation3d.java | 7 ++--- .../first/math/geometry/Translation3d.java | 4 +-- 7 files changed, 43 insertions(+), 50 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java index fd30b64b683..5dbb7e6190d 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java @@ -2,39 +2,40 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonProperty; - -import java.util.Objects; - import edu.wpi.first.math.geometry.Pose3d; +import java.util.Objects; public class AprilTag { - public int id; - public Pose3d pose; + @JsonProperty(value = "id") + public int m_id; + + @JsonProperty(value = "pose") + public Pose3d m_pose; @JsonCreator public AprilTag( @JsonProperty(required = true, value = "id") int id, @JsonProperty(required = true, value = "pose") Pose3d pose) { - this.id = id; - this.pose = pose; + this.m_id = id; + this.m_pose = pose; } @Override public boolean equals(Object obj) { if (obj instanceof AprilTag) { var other = (AprilTag) obj; - return id == other.id && pose.equals(other.pose); + return m_id == other.m_id && m_pose.equals(other.m_pose); } return false; } @Override public int hashCode() { - return Objects.hash(id, pose); + return Objects.hash(m_id, m_pose); } @Override public String toString() { - return "AprilTag(id: " + id + ", pose: " + pose + ")"; + return "AprilTag(id: " + m_id + ", pose: " + m_pose + ")"; } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 16e117d814f..1e0e59f6351 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -9,24 +9,23 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import com.fasterxml.jackson.databind.ObjectMapper; - -import java.io.IOException; -import java.nio.file.Path; -import java.util.ArrayList; -import java.util.List; -import java.util.Objects; - 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.io.IOException; +import java.nio.file.Path; +import java.util.ArrayList; +import java.util.List; +import java.util.Objects; @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class AprilTagFieldLayout { @JsonProperty(value = "tags") private final List m_tags = new ArrayList<>(); + private boolean m_mirror; /** @@ -55,8 +54,7 @@ public AprilTagFieldLayout(Path path) throws IOException { * @param tags map of IDs to poses */ @JsonCreator - public AprilTagFieldLayout( - @JsonProperty(required = true, value = "tags") List tags) { + public AprilTagFieldLayout(@JsonProperty(required = true, value = "tags") List tags) { // To ensure the underlying semantics don't change with what kind of list is passed in m_tags.addAll(tags); } @@ -79,12 +77,13 @@ public void setAlliance(DriverStation.Alliance alliance) { * @return The pose corresponding to the id passed in. */ public Pose3d getTag(int id) { - Pose3d pose = m_tags.stream().filter((it) -> id == it.id).findFirst().get().pose; + Pose3d pose = m_tags.stream().filter(it -> id == it.m_id).findFirst().get().m_pose; if (m_mirror) { - pose = pose.relativeTo( - new Pose3d( - new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0), - new Rotation3d(0.0, 0.0, 180.0))); + pose = + pose.relativeTo( + new Pose3d( + new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0), + new Rotation3d(0.0, 0.0, 180.0))); } return pose; @@ -112,7 +111,7 @@ public void serialize(Path path) throws IOException { @Override public boolean equals(Object obj) { - if(obj instanceof AprilTagFieldLayout) { + if (obj instanceof AprilTagFieldLayout) { var other = (AprilTagFieldLayout) obj; return m_tags.equals(other.m_tags) && m_mirror == other.m_mirror; } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java index e980c435d38..f87f845d190 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java @@ -4,30 +4,31 @@ package edu.wpi.first.wpilibj.apriltag; -import com.fasterxml.jackson.databind.ObjectMapper; - -import org.junit.jupiter.api.Test; - -import java.util.List; +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 static org.junit.jupiter.api.Assertions.assertDoesNotThrow; -import static org.junit.jupiter.api.Assertions.assertEquals; +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))))); + 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))))); var objectMapper = new ObjectMapper(); var deserialized = assertDoesNotThrow( - () -> objectMapper.readValue(objectMapper.writeValueAsString(layout), AprilTagFieldLayout.class)); + () -> + objectMapper.readValue( + objectMapper.writeValueAsString(layout), AprilTagFieldLayout.class)); assertEquals(layout, deserialized); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index adbf4bf17e0..0ad89d09f42 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java @@ -4,11 +4,10 @@ package edu.wpi.first.math; +import edu.wpi.first.util.RuntimeLoader; import java.io.IOException; import java.util.concurrent.atomic.AtomicBoolean; -import edu.wpi.first.util.RuntimeLoader; - public final class WPIMathJNI { static boolean libraryLoaded = false; static RuntimeLoader loader = null; diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index df14668e0c0..6a7a1c46b14 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -8,9 +8,6 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; - -import java.util.Objects; - import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -18,6 +15,7 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; +import java.util.Objects; /** Represents a 3D pose containing translational and rotational elements. */ @JsonIgnoreProperties(ignoreUnknown = true) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 0f874b416f7..18004c72408 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -8,11 +8,6 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; - -import org.ejml.dense.row.factory.DecompositionFactory_DDRM; - -import java.util.Objects; - import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; @@ -22,6 +17,8 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.numbers.N3; +import java.util.Objects; +import org.ejml.dense.row.factory.DecompositionFactory_DDRM; /** A rotation in a 3D coordinate frame represented by a quaternion. */ @JsonIgnoreProperties(ignoreUnknown = true) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index baf42c5ae7a..8c1015bb072 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -8,11 +8,9 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; - -import java.util.Objects; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.interpolation.Interpolatable; +import java.util.Objects; /** * Represents a translation in 3D space. This object can be used to represent a point or a vector. From 9f2179df06906df32726923591304b1528d0117d Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 25 Sep 2022 19:29:03 -0400 Subject: [PATCH 35/73] Remove AprilTagUtil --- .../first/wpilibj/apriltag/AprilTagUtil.java | 78 ------------------- 1 file changed, 78 deletions(-) delete mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java deleted file mode 100644 index f38764a2fa6..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagUtil.java +++ /dev/null @@ -1,78 +0,0 @@ -// 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 edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Quaternion; -import edu.wpi.first.math.geometry.Rotation3d; -import java.util.ArrayList; -import java.util.HashMap; -import java.util.Map; - -public final class AprilTagUtil { - private AprilTagUtil() { - throw new UnsupportedOperationException("This is a utility class!"); - } - - /** - * Creates an AprilTag layout from a double[] of elements. - * - * @param elements A double[] containing the raw elements of the AprilTag layout. - * @return An AprilTag layout created from the elements. - */ - public static Map createAprilTagFieldLayoutFromElements(double[] elements) { - // Make sure that the elements have the correct length. - if (elements.length % 8 != 0) { - throw new AprilTagLayoutSerializationException( - "An error occurred when converting AprilTag elements into a AprilTag layout."); - } - - // Create a list of states from the elements. - Map apriltagLayout = new HashMap<>(); - for (int i = 0; i < elements.length; i += 8) { - apriltagLayout.put( - (int) elements[i], - new Pose3d( - elements[i + 1], - elements[i + 2], - elements[i + 3], - new Rotation3d( - new Quaternion( - elements[i + 4], elements[i + 5], elements[i + 6], elements[i + 7])))); - } - return apriltagLayout; - } - - /** - * Returns a double[] of elements from the given AprilTag layout. - * - * @param aprilTagLayout The AprilTag layout to retrieve raw elements from. - * @return A double[] of elements from the given AprilTag layout. - */ - public static double[] getElementsFromAprilTagFieldLayout(Map aprilTagLayout) { - // Create a double[] of elements and fill it from the trajectory states. - double[] elements = new double[aprilTagLayout.size() * 8]; - - ArrayList> entries = new ArrayList<>(aprilTagLayout.entrySet()); - for (int i = 0; i < aprilTagLayout.size() * 8; i += 8) { - var entry = entries.get(i / 8); - elements[i] = entry.getKey(); - elements[i + 1] = entry.getValue().getX(); - elements[i + 2] = entry.getValue().getY(); - elements[i + 3] = entry.getValue().getZ(); - elements[i + 4] = entry.getValue().getRotation().getQuaternion().getW(); - elements[i + 5] = entry.getValue().getRotation().getQuaternion().getX(); - elements[i + 6] = entry.getValue().getRotation().getQuaternion().getY(); - elements[i + 7] = entry.getValue().getRotation().getQuaternion().getZ(); - } - return elements; - } - - public static class AprilTagLayoutSerializationException extends RuntimeException { - public AprilTagLayoutSerializationException(String message) { - super(message); - } - } -} From 12ebbab5de954aef7475d85c4b52d72932267673 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 25 Sep 2022 20:06:10 -0400 Subject: [PATCH 36/73] Serialize Rotation3d as Quaternion --- .../wpi/first/math/geometry/Quaternion.java | 18 +++++++++++++++++- .../wpi/first/math/geometry/Rotation3d.java | 14 ++++++-------- .../main/native/cpp/geometry/Quaternion.cpp | 17 +++++++++++++++++ .../main/native/cpp/geometry/Rotation3d.cpp | 10 ++++------ .../native/include/frc/geometry/Quaternion.h | 7 +++++++ 5 files changed, 51 insertions(+), 15 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java index 2f25cd606fa..64406ed0947 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java @@ -4,11 +4,18 @@ package edu.wpi.first.math.geometry; +import com.fasterxml.jackson.annotation.JsonAutoDetect; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; +import com.fasterxml.jackson.annotation.JsonProperty; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.numbers.N3; import java.util.Objects; + +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Quaternion { private final double m_r; private final Vector m_v; @@ -27,7 +34,12 @@ public Quaternion() { * @param y Y component of the quaternion. * @param z Z component of the quaternion. */ - public Quaternion(double w, double x, double y, double z) { + @JsonCreator + public Quaternion( + @JsonProperty(required = true, value = "W") double w, + @JsonProperty(required = true, value = "X") double x, + @JsonProperty(required = true, value = "Y") double y, + @JsonProperty(required = true, value = "Z") double z) { m_r = w; m_v = VecBuilder.fill(x, y, z); } @@ -113,6 +125,7 @@ public Quaternion normalize() { * * @return W component of the quaternion. */ + @JsonProperty(value = "W") public double getW() { return m_r; } @@ -122,6 +135,7 @@ public double getW() { * * @return X component of the quaternion. */ + @JsonProperty(value = "X") public double getX() { return m_v.get(0, 0); } @@ -131,6 +145,7 @@ public double getX() { * * @return Y component of the quaternion. */ + @JsonProperty(value = "Y") public double getY() { return m_v.get(1, 0); } @@ -140,6 +155,7 @@ public double getY() { * * @return Z component of the quaternion. */ + @JsonProperty(value = "Z") public double getZ() { return m_v.get(2, 0); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index c84b6e330d6..52f56318697 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -34,7 +34,8 @@ public Rotation3d() {} * * @param q The quaternion. */ - public Rotation3d(Quaternion q) { + @JsonCreator + public Rotation3d(@JsonProperty(required = true, value = "quaternion") Quaternion q) { m_q = q.normalize(); } @@ -48,11 +49,10 @@ public Rotation3d(Quaternion q) { * @param pitch The counterclockwise rotation angle around the Y axis (pitch) in radians. * @param yaw The counterclockwise rotation angle around the Z axis (yaw) in radians. */ - @JsonCreator public Rotation3d( - @JsonProperty(required = true, value = "roll") double roll, - @JsonProperty(required = true, value = "pitch") double pitch, - @JsonProperty(required = true, value = "yaw") double yaw) { + double roll, + double pitch, + double yaw) { // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion double cr = Math.cos(roll * 0.5); double sr = Math.sin(roll * 0.5); @@ -270,6 +270,7 @@ public Rotation3d rotateBy(Rotation3d other) { * * @return The quaternion representation of the Rotation3d. */ + @JsonProperty(value = "quaternion") public Quaternion getQuaternion() { return m_q; } @@ -279,7 +280,6 @@ public Quaternion getQuaternion() { * * @return The counterclockwise rotation angle around the X axis (roll) in radians. */ - @JsonProperty(value = "yaw") public double getX() { final var w = m_q.getW(); final var x = m_q.getX(); @@ -295,7 +295,6 @@ public double getX() { * * @return The counterclockwise rotation angle around the Y axis (pitch) in radians. */ - @JsonProperty(value = "pitch") public double getY() { final var w = m_q.getW(); final var x = m_q.getX(); @@ -316,7 +315,6 @@ public double getY() { * * @return The counterclockwise rotation angle around the Z axis (yaw) in radians. */ - @JsonProperty(value = "roll") public double getZ() { final var w = m_q.getW(); final var x = m_q.getX(); diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp index 94590b23818..0f636dbb89f 100644 --- a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp +++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp @@ -81,3 +81,20 @@ 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(), + json.at("X").get(), + json.at("Y").get(), + json.at("Z").get()}; +} \ No newline at end of file diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp index 2b6608cb682..f865d388d6f 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -239,13 +239,11 @@ Rotation2d Rotation3d::ToRotation2d() const { } void frc::to_json(wpi::json& json, const Rotation3d& rotation) { - json = wpi::json{{"yaw", rotation.X().value()}, - {"pitch", rotation.Y().value()}, - {"roll", rotation.Z().value()}}; + json = wpi::json{ + {"quaternion", rotation.GetQuaternion()} + }; } void frc::from_json(const wpi::json& json, Rotation3d& rotation) { - rotation = Rotation3d{units::degree_t{json.at("yaw").get()}, - units::degree_t{json.at("pitch").get()}, - units::degree_t{json.at("roll").get()}}; + rotation = Rotation3d{json.at("quaternion").get()}; } diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h index c21db3c8084..bd435d0fd13 100644 --- a/wpimath/src/main/native/include/frc/geometry/Quaternion.h +++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h @@ -7,6 +7,7 @@ #include #include "frc/EigenCore.h" +#include "wpi/json.h" namespace frc { @@ -92,4 +93,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 From 7d5ba746a29043775f81e52448c518afdec49ef9 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 25 Sep 2022 20:09:54 -0400 Subject: [PATCH 37/73] Run wpiformat and spotlessApply --- .../native/include/frc/apriltag/AprilTag.h | 2 +- .../frc/apriltag/AprilTagFieldLayout.h | 2 +- .../native/cpp/apriltag/AprilTagJsonTest.cpp | 3 ++- .../wpi/first/wpilibj/apriltag/AprilTag.java | 4 ++++ .../wpi/first/math/geometry/Quaternion.java | 1 - .../wpi/first/math/geometry/Rotation3d.java | 5 +--- .../main/native/cpp/geometry/Quaternion.cpp | 24 ++++++++----------- .../main/native/cpp/geometry/Rotation3d.cpp | 4 +--- .../native/include/frc/geometry/Quaternion.h | 2 +- 9 files changed, 21 insertions(+), 26 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h index 4079a985d3b..4d0b2e4f4cf 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h @@ -10,7 +10,7 @@ namespace wpi { class json; -} +} // namespace wpi namespace frc { struct WPILIB_DLLEXPORT AprilTag { diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 4951f12cffd..eafdd0e0e17 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -35,7 +35,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { void ToJson(std::string_view path); bool operator==(const AprilTagFieldLayout& other) const; - + bool operator!=(const AprilTagFieldLayout& other) const; private: diff --git a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp index fc127da3b64..e84c43a826c 100644 --- a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp @@ -6,11 +6,12 @@ #include #include +#include + #include "frc/apriltag/AprilTag.h" #include "frc/apriltag/AprilTagFieldLayout.h" #include "frc/geometry/Pose3d.h" #include "gtest/gtest.h" -#include "wpi/json.h" using namespace frc; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java index 5dbb7e6190d..febe9dcc2d0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java @@ -1,3 +1,7 @@ +// 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 com.fasterxml.jackson.annotation.JsonCreator; diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java index 64406ed0947..cadfaa4d08c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java @@ -13,7 +13,6 @@ import edu.wpi.first.math.numbers.N3; import java.util.Objects; - @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Quaternion { diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 52f56318697..c4202b0c1aa 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -49,10 +49,7 @@ public Rotation3d(@JsonProperty(required = true, value = "quaternion") Quaternio * @param pitch The counterclockwise rotation angle around the Y axis (pitch) in radians. * @param yaw The counterclockwise rotation angle around the Z axis (yaw) in radians. */ - public Rotation3d( - double roll, - double pitch, - double yaw) { + public Rotation3d(double roll, double pitch, double yaw) { // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Euler_angles_to_quaternion_conversion double cr = Math.cos(roll * 0.5); double sr = Math.sin(roll * 0.5); diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp index 0f636dbb89f..a6fa7ceba69 100644 --- a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp +++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp @@ -82,19 +82,15 @@ 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::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(), - json.at("X").get(), - json.at("Y").get(), - json.at("Z").get()}; -} \ No newline at end of file +void frc::from_json(const wpi::json& json, Quaternion& quaternion) { + quaternion = + Quaternion{json.at("W").get(), json.at("X").get(), + json.at("Y").get(), json.at("Z").get()}; +} diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp index f865d388d6f..00082cf4a7c 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -239,9 +239,7 @@ Rotation2d Rotation3d::ToRotation2d() const { } void frc::to_json(wpi::json& json, const Rotation3d& rotation) { - json = wpi::json{ - {"quaternion", rotation.GetQuaternion()} - }; + json = wpi::json{{"quaternion", rotation.GetQuaternion()}}; } void frc::from_json(const wpi::json& json, Rotation3d& rotation) { diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h index bd435d0fd13..c200b17319c 100644 --- a/wpimath/src/main/native/include/frc/geometry/Quaternion.h +++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h @@ -5,9 +5,9 @@ #pragma once #include +#include #include "frc/EigenCore.h" -#include "wpi/json.h" namespace frc { From dd038214fdc696d24f78c1713fa15ed48fb83bea Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 25 Sep 2022 21:59:09 -0400 Subject: [PATCH 38/73] Make to and from json methods friend functions --- .../main/native/include/frc/apriltag/AprilTagFieldLayout.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index eafdd0e0e17..6079f031163 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -20,8 +20,6 @@ class json; namespace frc { class WPILIB_DLLEXPORT AprilTagFieldLayout { public: - std::vector m_apriltags; - AprilTagFieldLayout() = default; explicit AprilTagFieldLayout(std::string_view path); @@ -37,8 +35,13 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { bool operator==(const AprilTagFieldLayout& other) const; bool operator!=(const AprilTagFieldLayout& other) const; + + friend void to_json(wpi::json &json, const AprilTagFieldLayout &layout); + + friend void from_json(const wpi::json &json, AprilTagFieldLayout &layout); private: + std::vector m_apriltags; bool m_mirror = false; }; From 1d6f56063bfac4da96dbf74a31fe5d55fb93c14a Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 25 Sep 2022 21:59:52 -0400 Subject: [PATCH 39/73] Rename function to be consistent with Java --- wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp | 2 +- .../src/main/native/include/frc/apriltag/AprilTagFieldLayout.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index 3816a21e918..73081a53b0e 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -55,7 +55,7 @@ void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { m_mirror = alliance == DriverStation::Alliance::kRed; } -void AprilTagFieldLayout::ToJson(std::string_view path) { +void AprilTagFieldLayout::Serialize(std::string_view path) { std::error_code error_code; wpi::raw_fd_ostream output{path, error_code}; diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 6079f031163..bc1fed91bb4 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -30,7 +30,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { void SetAlliance(DriverStation::Alliance alliance); - void ToJson(std::string_view path); + void Serialize(std::string_view path); bool operator==(const AprilTagFieldLayout& other) const; From b31bc5a4159d86bb7316f8837f11aacc3930902a Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 25 Sep 2022 22:01:07 -0400 Subject: [PATCH 40/73] Run wpiformat --- .../native/include/frc/apriltag/AprilTagFieldLayout.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index bc1fed91bb4..6d5c82af976 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -35,10 +35,10 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { bool operator==(const AprilTagFieldLayout& other) const; bool operator!=(const AprilTagFieldLayout& other) const; - - friend void to_json(wpi::json &json, const AprilTagFieldLayout &layout); - - friend void from_json(const wpi::json &json, AprilTagFieldLayout &layout); + + friend void to_json(wpi::json& json, const AprilTagFieldLayout& layout); + + friend void from_json(const wpi::json& json, AprilTagFieldLayout& layout); private: std::vector m_apriltags; From 8c6201b2f4cb343eaf0a6b49a71d599f4a4493d6 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 25 Sep 2022 22:14:17 -0400 Subject: [PATCH 41/73] Remove exteraneous imports, add DLLEXPORT where needed, and remove unneccesary call to this pointer --- .../src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp | 7 +------ .../native/include/frc/apriltag/AprilTagFieldLayout.h | 8 ++++---- wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp | 1 - wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp | 1 - 4 files changed, 5 insertions(+), 12 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index 73081a53b0e..c87732c7cd5 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -5,9 +5,7 @@ #include "frc/apriltag/AprilTagFieldLayout.h" #include -#include #include -#include #include #include @@ -15,9 +13,6 @@ #include #include -#include "frc/DriverStation.h" -#include "frc/geometry/Pose3d.h" - using namespace frc; AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) { @@ -31,7 +26,7 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) { wpi::json json; input >> json; - this->m_apriltags = json.get>(); + m_apriltags = json.get>(); } AprilTagFieldLayout::AprilTagFieldLayout(const std::vector& apriltags) diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 6d5c82af976..a0f1ae12eeb 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -36,13 +36,13 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { bool operator!=(const AprilTagFieldLayout& other) const; - friend void to_json(wpi::json& json, const AprilTagFieldLayout& layout); - - friend void from_json(const wpi::json& json, AprilTagFieldLayout& layout); - private: std::vector m_apriltags; bool m_mirror = false; + + friend WPILIB_DLLEXPORT void to_json(wpi::json& json, const AprilTagFieldLayout& layout); + + friend WPILIB_DLLEXPORT void from_json(const wpi::json& json, AprilTagFieldLayout& layout); }; WPILIB_DLLEXPORT diff --git a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp index e84c43a826c..c8aa0e092a1 100644 --- a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp @@ -3,7 +3,6 @@ // the WPILib BSD license file in the root directory of this project. #include -#include #include #include diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index 71152bdc612..c11e8ca3d42 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -5,7 +5,6 @@ #include #include -#include #include From b731467ff70d4fd50e4a413948564dd5499643f1 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 25 Sep 2022 22:16:14 -0400 Subject: [PATCH 42/73] Add degreesToRadians --- .../edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 1e0e59f6351..6df37fd3ca6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -83,7 +83,7 @@ public Pose3d getTag(int id) { pose.relativeTo( new Pose3d( new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0), - new Rotation3d(0.0, 0.0, 180.0))); + new Rotation3d(0.0, 0.0, Units.degreesToRadians(180.0)))); } return pose; From 77d07e6f1f569994141f08051fb717e4a2d02903 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Sun, 25 Sep 2022 22:18:19 -0400 Subject: [PATCH 43/73] Run wpiformat --- .../main/native/include/frc/apriltag/AprilTagFieldLayout.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index a0f1ae12eeb..071568d62ac 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -40,9 +40,11 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { std::vector m_apriltags; bool m_mirror = false; - friend WPILIB_DLLEXPORT void to_json(wpi::json& json, const AprilTagFieldLayout& layout); + friend WPILIB_DLLEXPORT void to_json(wpi::json& json, + const AprilTagFieldLayout& layout); - friend WPILIB_DLLEXPORT void from_json(const wpi::json& json, AprilTagFieldLayout& layout); + friend WPILIB_DLLEXPORT void from_json(const wpi::json& json, + AprilTagFieldLayout& layout); }; WPILIB_DLLEXPORT From 6e38d9d3785b97ab73dca033630d2795f36f1ca9 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Mon, 26 Sep 2022 07:21:44 -0400 Subject: [PATCH 44/73] Extra consistency --- .../wpilibj/apriltag/AprilTagFieldLayout.java | 32 ++++++++++--------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 6df37fd3ca6..ec615be807f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -9,22 +9,24 @@ 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.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; + import java.io.IOException; import java.nio.file.Path; import java.util.ArrayList; import java.util.List; import java.util.Objects; +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; + @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class AprilTagFieldLayout { @JsonProperty(value = "tags") - private final List m_tags = new ArrayList<>(); + private final List m_apriltags = new ArrayList<>(); private boolean m_mirror; @@ -45,18 +47,18 @@ public AprilTagFieldLayout(String path) throws IOException { * @throws IOException if reading from the file fails. */ public AprilTagFieldLayout(Path path) throws IOException { - this(new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class).m_tags); + this(new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class).m_apriltags); } /** - * Construct a new AprilTagFieldLayout from a map of AprilTag IDs to poses. + * Construct a new AprilTagFieldLayout from a list of {@link AprilTag} objects to poses. * - * @param tags map of IDs to poses + * @param apriltags list of {@link AprilTag} */ @JsonCreator - public AprilTagFieldLayout(@JsonProperty(required = true, value = "tags") List tags) { + public AprilTagFieldLayout(@JsonProperty(required = true, value = "tags") List apriltags) { // To ensure the underlying semantics don't change with what kind of list is passed in - m_tags.addAll(tags); + m_apriltags.addAll(apriltags); } /** @@ -77,7 +79,7 @@ public void setAlliance(DriverStation.Alliance alliance) { * @return The pose corresponding to the id passed in. */ public Pose3d getTag(int id) { - Pose3d pose = m_tags.stream().filter(it -> id == it.m_id).findFirst().get().m_pose; + Pose3d pose = m_apriltags.stream().filter(it -> id == it.m_id).findFirst().get().m_pose; if (m_mirror) { pose = pose.relativeTo( @@ -106,20 +108,20 @@ public void serialize(String path) throws IOException { * @throws IOException if writing to the file fails */ public void serialize(Path path) throws IOException { - new ObjectMapper().writeValue(path.toFile(), m_tags); + new ObjectMapper().writeValue(path.toFile(), m_apriltags); } @Override public boolean equals(Object obj) { if (obj instanceof AprilTagFieldLayout) { var other = (AprilTagFieldLayout) obj; - return m_tags.equals(other.m_tags) && m_mirror == other.m_mirror; + return m_apriltags.equals(other.m_apriltags) && m_mirror == other.m_mirror; } return false; } @Override public int hashCode() { - return Objects.hash(m_tags, m_mirror); + return Objects.hash(m_apriltags, m_mirror); } } From 2815f69a7e05430746f216ce974f095c7b462e98 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Mon, 26 Sep 2022 16:57:31 -0400 Subject: [PATCH 45/73] Add doxygen docs to AprilTagFieldLayout --- .../cpp/apriltag/AprilTagFieldLayout.cpp | 8 ++-- .../frc/apriltag/AprilTagFieldLayout.h | 40 ++++++++++++++++++- .../wpilibj/apriltag/AprilTagFieldLayout.java | 2 +- 3 files changed, 43 insertions(+), 7 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index c87732c7cd5..f9f5a9f8d1b 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -29,6 +29,10 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) { m_apriltags = json.get>(); } +void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { + m_mirror = alliance == DriverStation::Alliance::kRed; +} + AprilTagFieldLayout::AprilTagFieldLayout(const std::vector& apriltags) : m_apriltags(std::move(apriltags)) {} @@ -46,10 +50,6 @@ frc::Pose3d AprilTagFieldLayout::GetTagPose(int id) const { return returnPose; } -void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { - m_mirror = alliance == DriverStation::Alliance::kRed; -} - void AprilTagFieldLayout::Serialize(std::string_view path) { std::error_code error_code; diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 071568d62ac..c2a8a8cb2a2 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -22,18 +22,54 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { public: AprilTagFieldLayout() = default; + /** + * Construct a new AprilTagFieldLayout with values imported from a JSON file. + */ explicit AprilTagFieldLayout(std::string_view path); + /** + * Construct a new AprilTagFieldLayout from a vector of AprilTag objects. + */ explicit AprilTagFieldLayout(const std::vector& apriltags); - Pose3d GetTagPose(int id) const; - + /** + * Set the alliance that your team is on. + * + * This changes the AprilTagFieldLayout::getTag(int) method to return the correct pose for your alliance. + * + * @param alliance the alliance to mirror poses for + */ void SetAlliance(DriverStation::Alliance alliance); + + /** + * Gets an AprilTag pose by its id. + * + * @param id the id of the tag + * @return The pose corresponding to the id that was passed in. + */ + Pose3d GetTagPose(int id) const; + /** + * Serializes an AprilTagFieldLayout to a JSON file. + * + * @param path The path to write to + */ void Serialize(std::string_view path); + /* + * Checks equality between this AprilTagFieldLayout and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ bool operator==(const AprilTagFieldLayout& other) const; + /** + * Checks inequality between this AprilTagFieldLayout and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ bool operator!=(const AprilTagFieldLayout& other) const; private: diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index ec615be807f..0ac269c3875 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -51,7 +51,7 @@ public AprilTagFieldLayout(Path path) throws IOException { } /** - * Construct a new AprilTagFieldLayout from a list of {@link AprilTag} objects to poses. + * Construct a new AprilTagFieldLayout from a list of {@link AprilTag} objects. * * @param apriltags list of {@link AprilTag} */ From b5528f3ebd178f8942c43f23702589137f2c630a Mon Sep 17 00:00:00 2001 From: brennenputh Date: Mon, 26 Sep 2022 17:03:24 -0400 Subject: [PATCH 46/73] Run wpiformat --- .../frc/apriltag/AprilTagFieldLayout.h | 49 ++++++++++--------- 1 file changed, 25 insertions(+), 24 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index c2a8a8cb2a2..905af6809f2 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -24,52 +24,53 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { /** * Construct a new AprilTagFieldLayout with values imported from a JSON file. - */ + */ explicit AprilTagFieldLayout(std::string_view path); /** - * Construct a new AprilTagFieldLayout from a vector of AprilTag objects. - */ + * Construct a new AprilTagFieldLayout from a vector of AprilTag objects. + */ explicit AprilTagFieldLayout(const std::vector& apriltags); /** * Set the alliance that your team is on. * - * This changes the AprilTagFieldLayout::getTag(int) method to return the correct pose for your alliance. + * This changes the AprilTagFieldLayout::getTag(int) method to return the + * correct pose for your alliance. * * @param alliance the alliance to mirror poses for */ void SetAlliance(DriverStation::Alliance alliance); - + /** - * Gets an AprilTag pose by its id. - * - * @param id the id of the tag - * @return The pose corresponding to the id that was passed in. - */ + * Gets an AprilTag pose by its id. + * + * @param id the id of the tag + * @return The pose corresponding to the id that was passed in. + */ Pose3d GetTagPose(int id) const; /** - * Serializes an AprilTagFieldLayout to a JSON file. - * - * @param path The path to write to - */ + * Serializes an AprilTagFieldLayout to a JSON file. + * + * @param path The path to write to + */ void Serialize(std::string_view path); /* - * Checks equality between this AprilTagFieldLayout and another object. - * - * @param other The other object. - * @return Whether the two objects are equal. - */ + * Checks equality between this AprilTagFieldLayout and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ bool operator==(const AprilTagFieldLayout& other) const; /** - * Checks inequality between this AprilTagFieldLayout and another object. - * - * @param other The other object. - * @return Whether the two objects are not equal. - */ + * Checks inequality between this AprilTagFieldLayout and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ bool operator!=(const AprilTagFieldLayout& other) const; private: From 97892d370257d4185d55c8c311a57139dccb197e Mon Sep 17 00:00:00 2001 From: brennenputh Date: Mon, 26 Sep 2022 18:30:25 -0400 Subject: [PATCH 47/73] Add AprilTagPoseMirroringTest and format AprilTagFieldLayout --- .../apriltag/AprilTagPoseMirroringTest.cpp | 30 +++++++++++++ .../wpilibj/apriltag/AprilTagFieldLayout.java | 15 +++---- .../apriltag/AprilTagPoseMirroringTest.java | 45 +++++++++++++++++++ 3 files changed, 82 insertions(+), 8 deletions(-) create mode 100644 wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp create mode 100644 wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java diff --git a/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp b/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp new file mode 100644 index 00000000000..32b0ba38f76 --- /dev/null +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp @@ -0,0 +1,30 @@ +// 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 +#include + +#include + +#include "frc/apriltag/AprilTag.h" +#include "frc/apriltag/AprilTagFieldLayout.h" +#include "frc/geometry/Pose3d.h" +#include "gtest/gtest.h" + +using namespace frc; + +TEST(AprilTagPoseMirroringTest, MirroringMatches) { + auto layout = AprilTagFieldLayout{std::vector{ + AprilTag{1, Pose3d{0_ft, 0_ft, 0_ft, Rotation3d{0_deg, 0_deg, 0_deg}}}, + AprilTag{2, + Pose3d{4_ft, 4_ft, 4_ft, Rotation3d{0_deg, 0_deg, 180_deg}}}}}; + + layout.SetAlliance(DriverStation::Alliance::kRed); + + auto mirrorPose = + Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}}; + EXPECT_EQ(mirrorPose, layout.GetTagPose(1)); + mirrorPose = Pose3d{50_ft, 23_ft, 4_ft, Rotation3d{0_deg, 0_deg, 0_deg}}; + EXPECT_EQ(mirrorPose, layout.GetTagPose(2)); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 0ac269c3875..c4c4f21a46b 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -9,18 +9,16 @@ import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; import com.fasterxml.jackson.databind.ObjectMapper; - -import java.io.IOException; -import java.nio.file.Path; -import java.util.ArrayList; -import java.util.List; -import java.util.Objects; - 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.io.IOException; +import java.nio.file.Path; +import java.util.ArrayList; +import java.util.List; +import java.util.Objects; @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) @@ -56,7 +54,8 @@ public AprilTagFieldLayout(Path path) throws IOException { * @param apriltags list of {@link AprilTag} */ @JsonCreator - public AprilTagFieldLayout(@JsonProperty(required = true, value = "tags") List apriltags) { + public AprilTagFieldLayout( + @JsonProperty(required = true, value = "tags") List apriltags) { // To ensure the underlying semantics don't change with what kind of list is passed in m_apriltags.addAll(apriltags); } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java new file mode 100644 index 00000000000..bace6ec1c1e --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java @@ -0,0 +1,45 @@ +// 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)))))); + 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.getTag(1)); + + assertEquals( + new Pose3d( + new Translation3d( + Units.feetToMeters(50.0), Units.feetToMeters(23.0), Units.feetToMeters(4)), + new Rotation3d(0.0, 0.0, 0)), + layout.getTag(2)); + } +} From e6675cf696e68eaac1010a8392401fb6472960e5 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Mon, 26 Sep 2022 18:40:22 -0400 Subject: [PATCH 48/73] Remove unused includes --- wpilibc/src/main/native/include/frc/apriltag/AprilTag.h | 4 ---- wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp | 1 - .../test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp | 1 - 3 files changed, 6 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h index 4d0b2e4f4cf..4f1b23d9615 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h @@ -8,10 +8,6 @@ #include "frc/geometry/Pose3d.h" -namespace wpi { -class json; -} // namespace wpi - namespace frc { struct WPILIB_DLLEXPORT AprilTag { int id; diff --git a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp index c8aa0e092a1..41c7af8402c 100644 --- a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp @@ -2,7 +2,6 @@ // 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 #include #include diff --git a/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp b/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp index 32b0ba38f76..b3d439e70c3 100644 --- a/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp @@ -2,7 +2,6 @@ // 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 #include #include From 87fb00fda634e0c905ada5c0fdc19ae26c905030 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Mon, 26 Sep 2022 22:27:13 -0400 Subject: [PATCH 49/73] Fix docs formatting --- .../native/include/frc/apriltag/AprilTagFieldLayout.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 905af6809f2..210fe597cbe 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -38,14 +38,14 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { * This changes the AprilTagFieldLayout::getTag(int) method to return the * correct pose for your alliance. * - * @param alliance the alliance to mirror poses for + * @param alliance The alliance to mirror poses for. */ void SetAlliance(DriverStation::Alliance alliance); /** - * Gets an AprilTag pose by its id. + * Gets an AprilTag pose by its ID. * - * @param id the id of the tag + * @param id The id of the tag. * @return The pose corresponding to the id that was passed in. */ Pose3d GetTagPose(int id) const; @@ -53,7 +53,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { /** * Serializes an AprilTagFieldLayout to a JSON file. * - * @param path The path to write to + * @param path The path to write to. */ void Serialize(std::string_view path); From 4ddd39c6afc7ea745ba50c5d6254ea961d4cbe3b Mon Sep 17 00:00:00 2001 From: brennenputh Date: Mon, 26 Sep 2022 22:29:43 -0400 Subject: [PATCH 50/73] Remove move semantics from constructor, use copy semantics --- .../src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp | 6 +++--- .../main/native/include/frc/apriltag/AprilTagFieldLayout.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index f9f5a9f8d1b..3ac14dc3c8e 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -29,13 +29,13 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) { m_apriltags = json.get>(); } +AprilTagFieldLayout::AprilTagFieldLayout(std::vector& apriltags) + : m_apriltags(apriltags) {} + void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { m_mirror = alliance == DriverStation::Alliance::kRed; } -AprilTagFieldLayout::AprilTagFieldLayout(const std::vector& apriltags) - : m_apriltags(std::move(apriltags)) {} - frc::Pose3d AprilTagFieldLayout::GetTagPose(int id) const { Pose3d returnPose; auto it = std::find_if(m_apriltags.begin(), m_apriltags.end(), diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 210fe597cbe..dc19d6ce7d5 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -30,7 +30,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { /** * Construct a new AprilTagFieldLayout from a vector of AprilTag objects. */ - explicit AprilTagFieldLayout(const std::vector& apriltags); + explicit AprilTagFieldLayout(std::vector& apriltags); /** * Set the alliance that your team is on. From 7eb6bc1d3605a4ff70736750e1ebb7b7e8532156 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Mon, 26 Sep 2022 22:31:32 -0400 Subject: [PATCH 51/73] One more docs pass --- .../wpilibj/apriltag/AprilTagFieldLayout.java | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index c4c4f21a46b..28e23faf01a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -31,8 +31,8 @@ public class AprilTagFieldLayout { /** * Construct a new AprilTagFieldLayout with values imported from a JSON file. * - * @param path path of the JSON file to import from - * @throws IOException if reading from the file fails. + * @param path Path of the JSON file to import from. + * @throws IOException If reading from the file fails. */ public AprilTagFieldLayout(String path) throws IOException { this(Path.of(path)); @@ -41,8 +41,8 @@ public AprilTagFieldLayout(String path) throws IOException { /** * Construct a new AprilTagFieldLayout with values imported from a JSON file. * - * @param path path of the JSON file to import from - * @throws IOException if reading from the file fails. + * @param path Path of the JSON file to import from. + * @throws IOException If reading from the file fails. */ public AprilTagFieldLayout(Path path) throws IOException { this(new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class).m_apriltags); @@ -51,7 +51,7 @@ public AprilTagFieldLayout(Path path) throws IOException { /** * Construct a new AprilTagFieldLayout from a list of {@link AprilTag} objects. * - * @param apriltags list of {@link AprilTag} + * @param apriltags List of {@link AprilTag}. */ @JsonCreator public AprilTagFieldLayout( @@ -65,7 +65,7 @@ public AprilTagFieldLayout( * *

This changes the {@link #getTag(int)} method to return the correct pose for your alliance. * - * @param alliance the alliance to mirror poses for + * @param alliance The alliance to mirror poses for. */ public void setAlliance(DriverStation.Alliance alliance) { m_mirror = alliance == DriverStation.Alliance.Red; @@ -74,7 +74,7 @@ public void setAlliance(DriverStation.Alliance alliance) { /** * Gets an AprilTag pose by its id. * - * @param id The id of the tag + * @param id The id of the tag. * @return The pose corresponding to the id passed in. */ public Pose3d getTag(int id) { @@ -93,8 +93,8 @@ public Pose3d getTag(int id) { /** * Serializes a AprilTagFieldLayout to a JSON file. * - * @param path The path to write to - * @throws IOException if writing to the file fails + * @param path The path to write to. + * @throws IOException If writing to the file fails. */ public void serialize(String path) throws IOException { serialize(Path.of(path)); @@ -103,8 +103,8 @@ public void serialize(String path) throws IOException { /** * Serializes a AprilTagFieldLayout to a JSON file. * - * @param path The path to write to - * @throws IOException if writing to the file fails + * @param path The path to write to. + * @throws IOException If writing to the file fails. */ public void serialize(Path path) throws IOException { new ObjectMapper().writeValue(path.toFile(), m_apriltags); From da26b327d75a11df1f4c383ae966ca532e6b2b29 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Tue, 27 Sep 2022 07:15:08 -0400 Subject: [PATCH 52/73] Fix up docs --- .../src/main/native/cpp/apriltag/AprilTag.cpp | 6 ++--- .../native/include/frc/apriltag/AprilTag.h | 2 +- .../frc/apriltag/AprilTagFieldLayout.h | 24 +++++++++++++++---- .../wpi/first/wpilibj/apriltag/AprilTag.java | 14 +++++------ .../wpilibj/apriltag/AprilTagFieldLayout.java | 22 +++++++++++++---- 5 files changed, 47 insertions(+), 21 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTag.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTag.cpp index 86fa29e5299..46d696a04c8 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTag.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTag.cpp @@ -9,7 +9,7 @@ using namespace frc; bool AprilTag::operator==(const AprilTag& other) const { - return id == other.id && pose == other.pose; + return ID == other.ID && pose == other.pose; } bool AprilTag::operator!=(const AprilTag& other) const { @@ -17,10 +17,10 @@ bool AprilTag::operator!=(const AprilTag& other) const { } void frc::to_json(wpi::json& json, const AprilTag& apriltag) { - json = wpi::json{{"id", apriltag.id}, {"pose", apriltag.pose}}; + json = wpi::json{{"ID", apriltag.ID}, {"pose", apriltag.pose}}; } void frc::from_json(const wpi::json& json, AprilTag& apriltag) { - apriltag.id = json.at("id").get(); + apriltag.ID = json.at("ID").get(); apriltag.pose = json.at("pose").get(); } diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h index 4f1b23d9615..56b2e02020c 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h @@ -10,7 +10,7 @@ namespace frc { struct WPILIB_DLLEXPORT AprilTag { - int id; + int ID; Pose3d pose; diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index dc19d6ce7d5..a3e63b695e3 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -18,6 +18,20 @@ class json; } // namespace wpi namespace frc { +/** + * Class for representing a layout of AprilTags on a field and reading them from + * a JSON format. + * + * The JSON format contains a top-level "tags" field, which is a list of all + * AprilTags contained within a layout. Each AprilTag serializes to a JSON + * object containing an ID and a Pose3d. + * + * 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 GetTagPose(int). Setting an alliance color via + * SetAllianceColor(DriverStation::Alliance) will mirror the poses returned from + * GetTagPose(int) to be correct relative to the other alliance. + */ class WPILIB_DLLEXPORT AprilTagFieldLayout { public: AprilTagFieldLayout() = default; @@ -35,8 +49,8 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { /** * Set the alliance that your team is on. * - * This changes the AprilTagFieldLayout::getTag(int) method to return the - * correct pose for your alliance. + * This changes the GetTagPose(int) method to return the correct pose for your + * alliance. * * @param alliance The alliance to mirror poses for. */ @@ -45,15 +59,15 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { /** * Gets an AprilTag pose by its ID. * - * @param id The id of the tag. + * @param ID The ID of the tag. * @return The pose corresponding to the id that was passed in. */ - Pose3d GetTagPose(int id) const; + Pose3d GetTagPose(int ID) const; /** * Serializes an AprilTagFieldLayout to a JSON file. * - * @param path The path to write to. + * @param path The path to write the JSON file to. */ void Serialize(std::string_view path); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java index febe9dcc2d0..8e26d408a9f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java @@ -10,17 +10,17 @@ import java.util.Objects; public class AprilTag { - @JsonProperty(value = "id") - public int m_id; + @JsonProperty(value = "ID") + public int m_ID; @JsonProperty(value = "pose") public Pose3d m_pose; @JsonCreator public AprilTag( - @JsonProperty(required = true, value = "id") int id, + @JsonProperty(required = true, value = "ID") int ID, @JsonProperty(required = true, value = "pose") Pose3d pose) { - this.m_id = id; + this.m_ID = ID; this.m_pose = pose; } @@ -28,18 +28,18 @@ public AprilTag( public boolean equals(Object obj) { if (obj instanceof AprilTag) { var other = (AprilTag) obj; - return m_id == other.m_id && m_pose.equals(other.m_pose); + return m_ID == other.m_ID && m_pose.equals(other.m_pose); } return false; } @Override public int hashCode() { - return Objects.hash(m_id, m_pose); + return Objects.hash(m_ID, m_pose); } @Override public String toString() { - return "AprilTag(id: " + m_id + ", pose: " + m_pose + ")"; + return "AprilTag(ID: " + m_ID + ", pose: " + m_pose + ")"; } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 28e23faf01a..7a7cc0152f0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -20,6 +20,18 @@ import java.util.List; import java.util.Objects; +/** + * Class for representing a layout of AprilTags on a field and reading them from a JSON format. + * + *

The JSON format contains a top-level "tags" field, which is a list of all AprilTags contained + * within a layout. Each AprilTag serializes to a JSON object containing an ID and a Pose3d. + * + *

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#getTag(int)}. Setting an alliance color via {@link + * AprilTagFieldLayout#setAlliance(DriverStation.Alliance)} will mirror the poses returned from + * {@link AprilTagFieldLayout#getTag(int)} to be correct relative to the other alliance. + */ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class AprilTagFieldLayout { @@ -72,13 +84,13 @@ public void setAlliance(DriverStation.Alliance alliance) { } /** - * Gets an AprilTag pose by its id. + * Gets an AprilTag pose by its ID. * - * @param id The id of the tag. - * @return The pose corresponding to the id passed in. + * @param ID The ID of the tag. + * @return The pose corresponding to the ID passed in. */ - public Pose3d getTag(int id) { - Pose3d pose = m_apriltags.stream().filter(it -> id == it.m_id).findFirst().get().m_pose; + public Pose3d getTag(int ID) { + Pose3d pose = m_apriltags.stream().filter(it -> ID == it.m_ID).findFirst().get().m_pose; if (m_mirror) { pose = pose.relativeTo( From 0f6e9e7e3db8c66cfc2b9e245557658993b7b54f Mon Sep 17 00:00:00 2001 From: brennenputh Date: Tue, 27 Sep 2022 07:24:14 -0400 Subject: [PATCH 53/73] Make Java and C++ docs more consistent and add class doc --- .../native/include/frc/apriltag/AprilTagFieldLayout.h | 6 +++++- .../wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java | 9 +++++---- .../wpilibj/apriltag/AprilTagPoseMirroringTest.java | 4 ++-- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index a3e63b695e3..ae7ae18f064 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -29,7 +29,7 @@ namespace frc { * 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 GetTagPose(int). Setting an alliance color via - * SetAllianceColor(DriverStation::Alliance) will mirror the poses returned from + * SetAlliance(DriverStation::Alliance) will mirror the poses returned from * GetTagPose(int) to be correct relative to the other alliance. */ class WPILIB_DLLEXPORT AprilTagFieldLayout { @@ -38,11 +38,15 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { /** * Construct a new AprilTagFieldLayout with values imported from a JSON file. + * + * @param path Path of the JSON file to import from. */ explicit AprilTagFieldLayout(std::string_view path); /** * Construct a new AprilTagFieldLayout from a vector of AprilTag objects. + * + * @param apriltags Vector of AprilTags. */ explicit AprilTagFieldLayout(std::vector& apriltags); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 7a7cc0152f0..0d588060929 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -28,9 +28,9 @@ * *

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#getTag(int)}. Setting an alliance color via {@link + * AprilTagFieldLayout#getTagPose(int)}. Setting an alliance color via {@link * AprilTagFieldLayout#setAlliance(DriverStation.Alliance)} will mirror the poses returned from - * {@link AprilTagFieldLayout#getTag(int)} to be correct relative to the other alliance. + * {@link AprilTagFieldLayout#getTagPose(int)} to be correct relative to the other alliance. */ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) @@ -75,7 +75,8 @@ public AprilTagFieldLayout( /** * Set the alliance that your team is on. * - *

This changes the {@link #getTag(int)} method to return the correct pose for your alliance. + *

This changes the {@link #getTagPose(int)} method to return the correct pose for your + * alliance. * * @param alliance The alliance to mirror poses for. */ @@ -89,7 +90,7 @@ public void setAlliance(DriverStation.Alliance alliance) { * @param ID The ID of the tag. * @return The pose corresponding to the ID passed in. */ - public Pose3d getTag(int ID) { + public Pose3d getTagPose(int ID) { Pose3d pose = m_apriltags.stream().filter(it -> ID == it.m_ID).findFirst().get().m_pose; if (m_mirror) { pose = diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java index bace6ec1c1e..f8cc52a6973 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java @@ -33,13 +33,13 @@ void poseMirroring() { 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.getTag(1)); + layout.getTagPose(1)); assertEquals( new Pose3d( new Translation3d( Units.feetToMeters(50.0), Units.feetToMeters(23.0), Units.feetToMeters(4)), new Rotation3d(0.0, 0.0, 0)), - layout.getTag(2)); + layout.getTagPose(2)); } } From a60f3abdb5bfaa46d736485aafe5432a7cf77852 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Tue, 27 Sep 2022 07:26:23 -0400 Subject: [PATCH 54/73] Re-add const to constructor delaration --- wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp | 2 +- .../src/main/native/include/frc/apriltag/AprilTagFieldLayout.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index 3ac14dc3c8e..48856d225e2 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -29,7 +29,7 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) { m_apriltags = json.get>(); } -AprilTagFieldLayout::AprilTagFieldLayout(std::vector& apriltags) +AprilTagFieldLayout::AprilTagFieldLayout(const std::vector& apriltags) : m_apriltags(apriltags) {} void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index ae7ae18f064..30c866902fb 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -48,7 +48,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { * * @param apriltags Vector of AprilTags. */ - explicit AprilTagFieldLayout(std::vector& apriltags); + explicit AprilTagFieldLayout(const std::vector& apriltags); /** * Set the alliance that your team is on. From 038c32ff57b56966fe057ae18d2a7a0e8c01df56 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Tue, 27 Sep 2022 17:12:07 -0400 Subject: [PATCH 55/73] Revert last commit --- wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp | 2 +- .../src/main/native/include/frc/apriltag/AprilTagFieldLayout.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index 48856d225e2..3ac14dc3c8e 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -29,7 +29,7 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) { m_apriltags = json.get>(); } -AprilTagFieldLayout::AprilTagFieldLayout(const std::vector& apriltags) +AprilTagFieldLayout::AprilTagFieldLayout(std::vector& apriltags) : m_apriltags(apriltags) {} void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 30c866902fb..ae7ae18f064 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -48,7 +48,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { * * @param apriltags Vector of AprilTags. */ - explicit AprilTagFieldLayout(const std::vector& apriltags); + explicit AprilTagFieldLayout(std::vector& apriltags); /** * Set the alliance that your team is on. From 6a4ab7aa188b8651b21a27be2820a29941842e6c Mon Sep 17 00:00:00 2001 From: brennenputh Date: Tue, 27 Sep 2022 17:15:51 -0400 Subject: [PATCH 56/73] Continue id to ID change --- wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp | 4 ++-- .../main/native/include/frc/apriltag/AprilTagFieldLayout.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index 3ac14dc3c8e..41ca1f2b2ef 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -36,10 +36,10 @@ void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { m_mirror = alliance == DriverStation::Alliance::kRed; } -frc::Pose3d AprilTagFieldLayout::GetTagPose(int id) const { +frc::Pose3d AprilTagFieldLayout::GetTagPose(int ID) const { Pose3d returnPose; auto it = std::find_if(m_apriltags.begin(), m_apriltags.end(), - [=](const auto& tag) { return tag.id == id; }); + [=](const auto& tag) { return tag.ID == ID; }); if (it != m_apriltags.end()) { returnPose = it->pose; } diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index ae7ae18f064..ec4a7f25b63 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -64,7 +64,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { * Gets an AprilTag pose by its ID. * * @param ID The ID of the tag. - * @return The pose corresponding to the id that was passed in. + * @return The pose corresponding to the ID that was passed in. */ Pose3d GetTagPose(int ID) const; From 093d63276826142dadae0ce8dc2e1d257a3ee9e9 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Tue, 27 Sep 2022 17:17:12 -0400 Subject: [PATCH 57/73] Pass by value and move --- wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp | 4 ++-- .../main/native/include/frc/apriltag/AprilTagFieldLayout.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index 41ca1f2b2ef..fb591ef8434 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -29,8 +29,8 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) { m_apriltags = json.get>(); } -AprilTagFieldLayout::AprilTagFieldLayout(std::vector& apriltags) - : m_apriltags(apriltags) {} +AprilTagFieldLayout::AprilTagFieldLayout(std::vector apriltags) + : m_apriltags(std::move(apriltags)) {} void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { m_mirror = alliance == DriverStation::Alliance::kRed; diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index ec4a7f25b63..eb95ec7a200 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -48,7 +48,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { * * @param apriltags Vector of AprilTags. */ - explicit AprilTagFieldLayout(std::vector& apriltags); + explicit AprilTagFieldLayout(std::vector apriltags); /** * Set the alliance that your team is on. From fd2907b29eb60bf547ca5aff653166f2f859a025 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Tue, 27 Sep 2022 17:18:31 -0400 Subject: [PATCH 58/73] Add warning suppression --- .../src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java | 2 ++ .../edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java | 1 + 2 files changed, 3 insertions(+) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java index 8e26d408a9f..07460680862 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java @@ -10,12 +10,14 @@ import java.util.Objects; public class AprilTag { + @SuppressWarnings("MemberName") @JsonProperty(value = "ID") public int m_ID; @JsonProperty(value = "pose") public Pose3d m_pose; + @SuppressWarnings("ParameterName") @JsonCreator public AprilTag( @JsonProperty(required = true, value = "ID") int ID, diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 0d588060929..a3a63c68014 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -90,6 +90,7 @@ public void setAlliance(DriverStation.Alliance alliance) { * @param ID The ID of the tag. * @return The pose corresponding to the ID passed in. */ + @SuppressWarnings("ParameterName") public Pose3d getTagPose(int ID) { Pose3d pose = m_apriltags.stream().filter(it -> ID == it.m_ID).findFirst().get().m_pose; if (m_mirror) { From ad28a6187a07e2b90dfa2c75ed9f9fa6b3b08e2f Mon Sep 17 00:00:00 2001 From: brennenputh Date: Tue, 27 Sep 2022 17:49:27 -0400 Subject: [PATCH 59/73] Clean up includes --- wpimath/src/main/native/cpp/geometry/Rotation3d.cpp | 1 - wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp | 1 - wpimath/src/main/native/include/frc/geometry/Pose3d.h | 4 ++++ wpimath/src/main/native/include/frc/geometry/Quaternion.h | 5 ++++- wpimath/src/main/native/include/frc/geometry/Rotation3d.h | 5 ++++- wpimath/src/main/native/include/frc/geometry/Translation3d.h | 4 ++++ 6 files changed, 16 insertions(+), 4 deletions(-) diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp index 00082cf4a7c..b4f2d4b2a90 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -13,7 +13,6 @@ #include "Eigen/LU" #include "Eigen/QR" #include "frc/fmt/Eigen.h" -#include "units/angle.h" #include "units/math.h" #include "wpimath/MathShared.h" diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index c11e8ca3d42..e7ed5b36262 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -14,7 +14,6 @@ #include "Eigen/QR" #include "drake/math/discrete_algebraic_riccati_equation.h" #include "edu_wpi_first_math_WPIMathJNI.h" -#include "frc/geometry/Pose3d.h" #include "frc/trajectory/TrajectoryUtil.h" #include "unsupported/Eigen/MatrixFunctions" diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 66b37502204..30e24f9d2e2 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/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 { /** diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h index c200b17319c..3acd09c30a6 100644 --- a/wpimath/src/main/native/include/frc/geometry/Quaternion.h +++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h @@ -5,10 +5,13 @@ #pragma once #include -#include #include "frc/EigenCore.h" +namespace wpi { +class json; +} // namespace wpi + namespace frc { class WPILIB_DLLEXPORT Quaternion { diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index 409fe7377df..4a4c3f0c3ce 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -9,7 +9,10 @@ #include "Quaternion.h" #include "Rotation2d.h" #include "frc/EigenCore.h" -#include "units/angle.h" + +namespace wpi { + class json; +} // namespace wpi namespace frc { diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index 1ac9c170c97..dc09a6083e5 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/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 { /** From 7e1e506c9db116f937679c8be6669539d54d4699 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Tue, 27 Sep 2022 17:50:04 -0400 Subject: [PATCH 60/73] Run wpiformat --- wpimath/src/main/native/include/frc/geometry/Pose3d.h | 2 +- wpimath/src/main/native/include/frc/geometry/Quaternion.h | 2 +- wpimath/src/main/native/include/frc/geometry/Rotation3d.h | 4 ++-- wpimath/src/main/native/include/frc/geometry/Translation3d.h | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index 30e24f9d2e2..0a75da65617 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -13,7 +13,7 @@ namespace wpi { class json; -} // namespace wpi +} // namespace wpi namespace frc { diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h index 3acd09c30a6..97933abe5bf 100644 --- a/wpimath/src/main/native/include/frc/geometry/Quaternion.h +++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h @@ -10,7 +10,7 @@ namespace wpi { class json; -} // namespace wpi +} // namespace wpi namespace frc { diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index 4a4c3f0c3ce..03460bc7f7b 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -11,8 +11,8 @@ #include "frc/EigenCore.h" namespace wpi { - class json; -} // namespace wpi +class json; +} // namespace wpi namespace frc { diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index dc09a6083e5..bcae175aa8d 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -11,8 +11,8 @@ #include "units/length.h" namespace wpi { - class json; -} // namespace wpi +class json; +} // namespace wpi namespace frc { From 477081fba5f7504eb32a3754342cdef9ee996bea Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 27 Sep 2022 17:50:09 -0700 Subject: [PATCH 61/73] Add missing include and forward declaration --- wpilibc/src/main/native/include/frc/apriltag/AprilTag.h | 5 +++++ wpimath/src/main/native/cpp/geometry/Quaternion.cpp | 2 ++ wpimath/src/main/native/include/frc/geometry/Rotation3d.h | 1 + 3 files changed, 8 insertions(+) diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h index 56b2e02020c..444106df23d 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h @@ -8,7 +8,12 @@ #include "frc/geometry/Pose3d.h" +namespace wpi { +class json; +} // namespace wpi + namespace frc { + struct WPILIB_DLLEXPORT AprilTag { int ID; diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp index a6fa7ceba69..7f77b139ca5 100644 --- a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp +++ b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp @@ -4,6 +4,8 @@ #include "frc/geometry/Quaternion.h" +#include + using namespace frc; Quaternion::Quaternion(double w, double x, double y, double z) diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index 03460bc7f7b..f9de1d76a8a 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -9,6 +9,7 @@ #include "Quaternion.h" #include "Rotation2d.h" #include "frc/EigenCore.h" +#include "units/angle.h" namespace wpi { class json; From 3a92d68424c2e598fc123c36de1fcb73ffcd790e Mon Sep 17 00:00:00 2001 From: brennenputh Date: Wed, 28 Sep 2022 22:28:22 -0400 Subject: [PATCH 62/73] Add field descriptor object C++ --- .../cpp/apriltag/AprilTagFieldLayout.cpp | 25 ++++++++++++++----- .../frc/apriltag/AprilTagFieldLayout.h | 16 +++++++++--- .../native/cpp/apriltag/AprilTagJsonTest.cpp | 8 +++--- .../apriltag/AprilTagPoseMirroringTest.cpp | 11 +++++--- 4 files changed, 43 insertions(+), 17 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index fb591ef8434..4beefa53c59 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -26,11 +26,17 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) { wpi::json json; input >> json; - m_apriltags = json.get>(); + m_apriltags = json.at("tags").get>(); + m_fieldWidth = units::foot_t{json.at("field").at("width").get()}; + m_fieldHeight = units::foot_t{json.at("field").at("height").get()}; } -AprilTagFieldLayout::AprilTagFieldLayout(std::vector apriltags) - : m_apriltags(std::move(apriltags)) {} +AprilTagFieldLayout::AprilTagFieldLayout(std::vector apriltags, + units::foot_t fieldWidth, + units::foot_t fieldHeight) + : m_apriltags(std::move(apriltags)), + m_fieldWidth(std::move(fieldWidth)), + m_fieldHeight(std::move(fieldHeight)) {} void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { m_mirror = alliance == DriverStation::Alliance::kRed; @@ -44,8 +50,8 @@ frc::Pose3d AprilTagFieldLayout::GetTagPose(int ID) const { returnPose = it->pose; } if (m_mirror) { - returnPose = returnPose.RelativeTo( - Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}}); + returnPose = returnPose.RelativeTo(Pose3d{ + m_fieldWidth, m_fieldHeight, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}}); } return returnPose; } @@ -72,9 +78,16 @@ bool AprilTagFieldLayout::operator!=(const AprilTagFieldLayout& other) const { } void frc::to_json(wpi::json& json, const AprilTagFieldLayout& layout) { - json = wpi::json{{"tags", layout.m_apriltags}}; + json = wpi::json{{"field", + {{"width", layout.m_fieldWidth.value()}, + {"height", layout.m_fieldHeight.value()}}}, + {"tags", layout.m_apriltags}}; } void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) { layout.m_apriltags = json.at("tags").get>(); + layout.m_fieldWidth = + units::foot_t{json.at("field").at("width").get()}; + layout.m_fieldHeight = + units::foot_t{json.at("field").at("height").get()}; } diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index eb95ec7a200..851cf73fb1f 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -7,6 +7,7 @@ #include #include +#include #include #include "frc/DriverStation.h" @@ -22,9 +23,12 @@ namespace frc { * Class for representing a layout of AprilTags on a field and reading them from * a JSON format. * - * The JSON format contains a top-level "tags" field, which is a list of all - * AprilTags contained within a layout. Each AprilTag serializes to a JSON - * object containing an ID and a Pose3d. + * The JSON format contains two top-level objects, "tags" and "field". + * The "tags" object is a list of all AprilTags contained within a layout. Each + * AprilTag serializes to a JSON object containing an ID and a Pose3d. The + * "field" object is a descriptor of the size of the field in feet with "width" + * and "height" values. This is to account for arbitrary field sizes when + * mirroring the poses. * * 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 @@ -48,7 +52,9 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { * * @param apriltags Vector of AprilTags. */ - explicit AprilTagFieldLayout(std::vector apriltags); + explicit AprilTagFieldLayout(std::vector apriltags, + units::foot_t fieldWidth, + units::foot_t fieldHeight); /** * Set the alliance that your team is on. @@ -93,6 +99,8 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { private: std::vector m_apriltags; + units::foot_t m_fieldWidth; + units::foot_t m_fieldHeight; bool m_mirror = false; friend WPILIB_DLLEXPORT void to_json(wpi::json& json, diff --git a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp index 41c7af8402c..157c40f4662 100644 --- a/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp @@ -14,9 +14,11 @@ using namespace frc; TEST(AprilTagJsonTest, DeserializeMatches) { - auto layout = AprilTagFieldLayout{std::vector{ - AprilTag{1, Pose3d{}}, - AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}}}; + auto layout = AprilTagFieldLayout{ + std::vector{ + AprilTag{1, Pose3d{}}, + AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}}, + 54_ft, 27_ft}; AprilTagFieldLayout deserialized; wpi::json json = layout; diff --git a/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp b/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp index b3d439e70c3..f51f8a50a85 100644 --- a/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp @@ -14,10 +14,13 @@ using namespace frc; TEST(AprilTagPoseMirroringTest, MirroringMatches) { - auto layout = AprilTagFieldLayout{std::vector{ - AprilTag{1, Pose3d{0_ft, 0_ft, 0_ft, Rotation3d{0_deg, 0_deg, 0_deg}}}, - AprilTag{2, - Pose3d{4_ft, 4_ft, 4_ft, Rotation3d{0_deg, 0_deg, 180_deg}}}}}; + auto layout = AprilTagFieldLayout{ + std::vector{ + AprilTag{1, + Pose3d{0_ft, 0_ft, 0_ft, Rotation3d{0_deg, 0_deg, 0_deg}}}, + AprilTag{ + 2, Pose3d{4_ft, 4_ft, 4_ft, Rotation3d{0_deg, 0_deg, 180_deg}}}}, + 54_ft, 27_ft}; layout.SetAlliance(DriverStation::Alliance::kRed); From 97f79894f39c16606b6d7bfb10aadef0c81fd57e Mon Sep 17 00:00:00 2001 From: brennenputh Date: Wed, 28 Sep 2022 22:48:27 -0400 Subject: [PATCH 63/73] Add field size descriptor to Java --- .../wpilibj/apriltag/AprilTagFieldLayout.java | 44 ++++++++++++++++--- .../apriltag/AprilTagPoseMirroringTest.java | 3 +- .../apriltag/AprilTagSerializationTest.java | 10 ++++- 3 files changed, 49 insertions(+), 8 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index a3a63c68014..fcc0f40d463 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -23,8 +23,11 @@ /** * Class for representing a layout of AprilTags on a field and reading them from a JSON format. * - *

The JSON format contains a top-level "tags" field, which is a list of all AprilTags contained - * within a layout. Each AprilTag serializes to a JSON object containing an ID and a Pose3d. + *

The JSON format contains two top-level objects, "tags" and "field". The "tags" object is a + * list of all AprilTags contained within a layout. Each AprilTag serializes to a JSON object + * containing an ID and a Pose3d. The "field" object is a descriptor of the size of the field in + * feet with "width" and "height" values. This is to account for arbitrary field sizes when + * mirroring the poses * *

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 @@ -38,6 +41,9 @@ public class AprilTagFieldLayout { @JsonProperty(value = "tags") private final List m_apriltags = new ArrayList<>(); + @JsonProperty(value = "field") + private FieldSize m_fieldSize; + private boolean m_mirror; /** @@ -57,7 +63,10 @@ public AprilTagFieldLayout(String path) throws IOException { * @throws IOException If reading from the file fails. */ public AprilTagFieldLayout(Path path) throws IOException { - this(new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class).m_apriltags); + AprilTagFieldLayout layout = + new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class); + m_apriltags.addAll(layout.m_apriltags); + m_fieldSize = layout.m_fieldSize; } /** @@ -67,9 +76,11 @@ public AprilTagFieldLayout(Path path) throws IOException { */ @JsonCreator public AprilTagFieldLayout( - @JsonProperty(required = true, value = "tags") List apriltags) { + @JsonProperty(required = true, value = "tags") List apriltags, + @JsonProperty(required = true, value = "field") FieldSize fieldSize) { // To ensure the underlying semantics don't change with what kind of list is passed in m_apriltags.addAll(apriltags); + m_fieldSize = fieldSize; } /** @@ -97,7 +108,10 @@ public Pose3d getTagPose(int ID) { pose = pose.relativeTo( new Pose3d( - new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0), + new Translation3d( + Units.feetToMeters(m_fieldSize.m_fieldWidth), + Units.feetToMeters(m_fieldSize.m_fieldHeight), + 0.0), new Rotation3d(0.0, 0.0, Units.degreesToRadians(180.0)))); } @@ -121,7 +135,7 @@ public void serialize(String path) throws IOException { * @throws IOException If writing to the file fails. */ public void serialize(Path path) throws IOException { - new ObjectMapper().writeValue(path.toFile(), m_apriltags); + new ObjectMapper().writeValue(path.toFile(), this); } @Override @@ -137,4 +151,22 @@ public boolean equals(Object obj) { public int hashCode() { return Objects.hash(m_apriltags, m_mirror); } + + @JsonIgnoreProperties(ignoreUnknown = true) + @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) + public static class FieldSize { + @JsonProperty(value = "width") + public double m_fieldWidth; + + @JsonProperty(value = "height") + public double m_fieldHeight; + + @JsonCreator() + public FieldSize( + @JsonProperty(required = true, value = "width") double fieldWidth, + @JsonProperty(required = true, value = "height") double fieldHeight) { + m_fieldWidth = fieldWidth; + m_fieldHeight = fieldHeight; + } + } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java index f8cc52a6973..a028c730495 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java @@ -26,7 +26,8 @@ void poseMirroring() { new Pose3d( new Translation3d( Units.feetToMeters(4.0), Units.feetToMeters(4), Units.feetToMeters(4)), - new Rotation3d(0, 0, Units.degreesToRadians(180)))))); + new Rotation3d(0, 0, Units.degreesToRadians(180))))), + new AprilTagFieldLayout.FieldSize(54.0, 27.0)); layout.setAlliance(DriverStation.Alliance.Red); assertEquals( diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java index f87f845d190..afdb76a1a77 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java @@ -10,6 +10,7 @@ import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; +import java.io.IOException; import java.util.List; import org.junit.jupiter.api.Test; @@ -20,10 +21,17 @@ void deserializeMatches() { 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))))); + new AprilTag(3, new Pose3d(0, 1, 0, new Rotation3d(0, 0, 0)))), + new AprilTagFieldLayout.FieldSize(54.0, 27.0)); var objectMapper = new ObjectMapper(); + try { + layout.serialize("/home/bagatelle/Coding/FRC/test.json"); + } catch (IOException e) { + throw new RuntimeException(e); + } + var deserialized = assertDoesNotThrow( () -> From fcf9f8871c8e752659a1c15a4d6f58e02cadcee4 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Thu, 29 Sep 2022 15:30:11 -0400 Subject: [PATCH 64/73] Change to meters units C++ --- .../cpp/apriltag/AprilTagFieldLayout.cpp | 28 ++++++++++--------- .../frc/apriltag/AprilTagFieldLayout.h | 17 +++++------ 2 files changed, 24 insertions(+), 21 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index 4beefa53c59..6ce6e51872e 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -27,16 +27,16 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) { input >> json; m_apriltags = json.at("tags").get>(); - m_fieldWidth = units::foot_t{json.at("field").at("width").get()}; - m_fieldHeight = units::foot_t{json.at("field").at("height").get()}; + m_fieldWidth = units::meter_t{json.at("field").at("width").get()}; + m_fieldLength = units::meter_t{json.at("field").at("height").get()}; } AprilTagFieldLayout::AprilTagFieldLayout(std::vector apriltags, - units::foot_t fieldWidth, - units::foot_t fieldHeight) + units::meter_t fieldLength, + units::meter_t fieldWidth) : m_apriltags(std::move(apriltags)), - m_fieldWidth(std::move(fieldWidth)), - m_fieldHeight(std::move(fieldHeight)) {} + m_fieldLength(std::move(fieldLength)), + m_fieldWidth(std::move(fieldWidth)) {} void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { m_mirror = alliance == DriverStation::Alliance::kRed; @@ -51,7 +51,7 @@ frc::Pose3d AprilTagFieldLayout::GetTagPose(int ID) const { } if (m_mirror) { returnPose = returnPose.RelativeTo(Pose3d{ - m_fieldWidth, m_fieldHeight, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}}); + m_fieldLength, m_fieldWidth, 0_m, Rotation3d{0_deg, 0_deg, 180_deg}}); } return returnPose; } @@ -70,7 +70,9 @@ 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_mirror == other.m_mirror && + m_fieldLength == other.m_fieldLength && + m_fieldWidth == other.m_fieldWidth; } bool AprilTagFieldLayout::operator!=(const AprilTagFieldLayout& other) const { @@ -79,15 +81,15 @@ bool AprilTagFieldLayout::operator!=(const AprilTagFieldLayout& other) const { void frc::to_json(wpi::json& json, const AprilTagFieldLayout& layout) { json = wpi::json{{"field", - {{"width", layout.m_fieldWidth.value()}, - {"height", layout.m_fieldHeight.value()}}}, + {{"length", layout.m_fieldLength.value()}, + {"width", layout.m_fieldWidth.value()}}}, {"tags", layout.m_apriltags}}; } void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) { layout.m_apriltags = json.at("tags").get>(); + layout.m_fieldLength = + units::meter_t{json.at("field").at("length").get()}; layout.m_fieldWidth = - units::foot_t{json.at("field").at("width").get()}; - layout.m_fieldHeight = - units::foot_t{json.at("field").at("height").get()}; + units::meter_t{json.at("field").at("width").get()}; } diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 851cf73fb1f..8c5d14704d1 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -26,9 +26,9 @@ namespace frc { * The JSON format contains two top-level objects, "tags" and "field". * The "tags" object is a list of all AprilTags contained within a layout. Each * AprilTag serializes to a JSON object containing an ID and a Pose3d. The - * "field" object is a descriptor of the size of the field in feet with "width" - * and "height" values. This is to account for arbitrary field sizes when - * mirroring the poses. + * "field" object is a descriptor of the size of the field in meters with + * "width" and "height" values. This is to account for arbitrary field sizes + * when mirroring the poses. * * 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 @@ -51,10 +51,11 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { * Construct a new AprilTagFieldLayout from a vector of AprilTag objects. * * @param apriltags Vector of AprilTags. + * @param fieldLength Length of field the layout of representing. + * @param fieldWidth Width of field the layout is representing. */ - explicit AprilTagFieldLayout(std::vector apriltags, - units::foot_t fieldWidth, - units::foot_t fieldHeight); + AprilTagFieldLayout(std::vector apriltags, + units::meter_t fieldLength, units::meter_t fieldWidth); /** * Set the alliance that your team is on. @@ -99,8 +100,8 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { private: std::vector m_apriltags; - units::foot_t m_fieldWidth; - units::foot_t m_fieldHeight; + units::meter_t m_fieldLength; + units::meter_t m_fieldWidth; bool m_mirror = false; friend WPILIB_DLLEXPORT void to_json(wpi::json& json, From db70d06563768a2b5691eb82929d19df0d4078fa Mon Sep 17 00:00:00 2001 From: brennenputh Date: Thu, 29 Sep 2022 15:39:16 -0400 Subject: [PATCH 65/73] Switch units to meters and change FieldSize to FieldDimensions Java --- .../wpi/first/wpilibj/apriltag/AprilTag.java | 12 +++--- .../wpilibj/apriltag/AprilTagFieldLayout.java | 37 +++++++++---------- .../apriltag/AprilTagPoseMirroringTest.java | 3 +- .../apriltag/AprilTagSerializationTest.java | 4 +- 4 files changed, 29 insertions(+), 27 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java index 07460680862..87990e36554 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java @@ -15,33 +15,33 @@ public class AprilTag { public int m_ID; @JsonProperty(value = "pose") - public Pose3d m_pose; + public Pose3d m_poseMeters; @SuppressWarnings("ParameterName") @JsonCreator public AprilTag( @JsonProperty(required = true, value = "ID") int ID, - @JsonProperty(required = true, value = "pose") Pose3d pose) { + @JsonProperty(required = true, value = "pose") Pose3d poseMeters) { this.m_ID = ID; - this.m_pose = pose; + this.m_poseMeters = poseMeters; } @Override public boolean equals(Object obj) { if (obj instanceof AprilTag) { var other = (AprilTag) obj; - return m_ID == other.m_ID && m_pose.equals(other.m_pose); + return m_ID == other.m_ID && m_poseMeters.equals(other.m_poseMeters); } return false; } @Override public int hashCode() { - return Objects.hash(m_ID, m_pose); + return Objects.hash(m_ID, m_poseMeters); } @Override public String toString() { - return "AprilTag(ID: " + m_ID + ", pose: " + m_pose + ")"; + return "AprilTag(ID: " + m_ID + ", pose: " + m_poseMeters + ")"; } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index fcc0f40d463..dcc5fca5c7a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -12,7 +12,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.io.IOException; import java.nio.file.Path; @@ -26,8 +25,8 @@ *

The JSON format contains two top-level objects, "tags" and "field". The "tags" object is a * list of all AprilTags contained within a layout. Each AprilTag serializes to a JSON object * containing an ID and a Pose3d. The "field" object is a descriptor of the size of the field in - * feet with "width" and "height" values. This is to account for arbitrary field sizes when - * mirroring the poses + * meters with "width" and "height" values. This is to account for arbitrary field sizes when + * mirroring the poses. * *

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 @@ -42,7 +41,7 @@ public class AprilTagFieldLayout { private final List m_apriltags = new ArrayList<>(); @JsonProperty(value = "field") - private FieldSize m_fieldSize; + private FieldDimensions m_fieldDimensions; private boolean m_mirror; @@ -66,7 +65,7 @@ public AprilTagFieldLayout(Path path) throws IOException { AprilTagFieldLayout layout = new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class); m_apriltags.addAll(layout.m_apriltags); - m_fieldSize = layout.m_fieldSize; + m_fieldDimensions = layout.m_fieldDimensions; } /** @@ -77,10 +76,10 @@ public AprilTagFieldLayout(Path path) throws IOException { @JsonCreator public AprilTagFieldLayout( @JsonProperty(required = true, value = "tags") List apriltags, - @JsonProperty(required = true, value = "field") FieldSize fieldSize) { + @JsonProperty(required = true, value = "field") FieldDimensions fieldDimensions) { // To ensure the underlying semantics don't change with what kind of list is passed in m_apriltags.addAll(apriltags); - m_fieldSize = fieldSize; + m_fieldDimensions = fieldDimensions; } /** @@ -103,16 +102,16 @@ public void setAlliance(DriverStation.Alliance alliance) { */ @SuppressWarnings("ParameterName") public Pose3d getTagPose(int ID) { - Pose3d pose = m_apriltags.stream().filter(it -> ID == it.m_ID).findFirst().get().m_pose; + Pose3d pose = m_apriltags.stream().filter(it -> ID == it.m_ID).findFirst().get().m_poseMeters; if (m_mirror) { pose = pose.relativeTo( new Pose3d( new Translation3d( - Units.feetToMeters(m_fieldSize.m_fieldWidth), - Units.feetToMeters(m_fieldSize.m_fieldHeight), + m_fieldDimensions.m_fieldWidthMeters, + m_fieldDimensions.m_fieldHeightMeters, 0.0), - new Rotation3d(0.0, 0.0, Units.degreesToRadians(180.0)))); + new Rotation3d(0.0, 0.0, Math.PI))); } return pose; @@ -154,19 +153,19 @@ public int hashCode() { @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) - public static class FieldSize { + public static class FieldDimensions { @JsonProperty(value = "width") - public double m_fieldWidth; + public double m_fieldWidthMeters; @JsonProperty(value = "height") - public double m_fieldHeight; + public double m_fieldHeightMeters; @JsonCreator() - public FieldSize( - @JsonProperty(required = true, value = "width") double fieldWidth, - @JsonProperty(required = true, value = "height") double fieldHeight) { - m_fieldWidth = fieldWidth; - m_fieldHeight = fieldHeight; + public FieldDimensions( + @JsonProperty(required = true, value = "width") double fieldWidthMeters, + @JsonProperty(required = true, value = "height") double fieldHeightMeters) { + m_fieldWidthMeters = fieldWidthMeters; + m_fieldHeightMeters = fieldHeightMeters; } } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java index a028c730495..88ab38cf95d 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java @@ -27,7 +27,8 @@ void poseMirroring() { new Translation3d( Units.feetToMeters(4.0), Units.feetToMeters(4), Units.feetToMeters(4)), new Rotation3d(0, 0, Units.degreesToRadians(180))))), - new AprilTagFieldLayout.FieldSize(54.0, 27.0)); + new AprilTagFieldLayout.FieldDimensions( + Units.feetToMeters(54.0), Units.feetToMeters(27.0))); layout.setAlliance(DriverStation.Alliance.Red); assertEquals( diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java index afdb76a1a77..c07057503be 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java @@ -10,6 +10,7 @@ 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.io.IOException; import java.util.List; import org.junit.jupiter.api.Test; @@ -22,7 +23,8 @@ void deserializeMatches() { 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)))), - new AprilTagFieldLayout.FieldSize(54.0, 27.0)); + new AprilTagFieldLayout.FieldDimensions( + Units.feetToMeters(54.0), Units.feetToMeters(27.0))); var objectMapper = new ObjectMapper(); From 6c0670e767ae6500353072b0e82209443006a465 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Thu, 29 Sep 2022 15:59:09 -0400 Subject: [PATCH 66/73] Remove FieldDimensions from public interface --- .../wpi/first/wpilibj/apriltag/AprilTag.java | 17 +++++----- .../wpilibj/apriltag/AprilTagFieldLayout.java | 32 +++++++++++-------- .../apriltag/AprilTagPoseMirroringTest.java | 4 +-- .../apriltag/AprilTagSerializationTest.java | 4 +-- 4 files changed, 32 insertions(+), 25 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java index 87990e36554..0ef8f3cd3be 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java @@ -12,36 +12,37 @@ public class AprilTag { @SuppressWarnings("MemberName") @JsonProperty(value = "ID") - public int m_ID; + public int ID; + @SuppressWarnings("MemberName") @JsonProperty(value = "pose") - public Pose3d m_poseMeters; + public Pose3d pose; @SuppressWarnings("ParameterName") @JsonCreator public AprilTag( @JsonProperty(required = true, value = "ID") int ID, - @JsonProperty(required = true, value = "pose") Pose3d poseMeters) { - this.m_ID = ID; - this.m_poseMeters = poseMeters; + @JsonProperty(required = true, value = "pose") Pose3d pose) { + this.ID = ID; + this.pose = pose; } @Override public boolean equals(Object obj) { if (obj instanceof AprilTag) { var other = (AprilTag) obj; - return m_ID == other.m_ID && m_poseMeters.equals(other.m_poseMeters); + return ID == other.ID && pose.equals(other.pose); } return false; } @Override public int hashCode() { - return Objects.hash(m_ID, m_poseMeters); + return Objects.hash(ID, pose); } @Override public String toString() { - return "AprilTag(ID: " + m_ID + ", pose: " + m_poseMeters + ")"; + return "AprilTag(ID: " + ID + ", pose: " + pose + ")"; } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index dcc5fca5c7a..0ebb23c9f57 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -72,9 +72,15 @@ public AprilTagFieldLayout(Path path) throws IOException { * Construct a new AprilTagFieldLayout from a list of {@link AprilTag} objects. * * @param apriltags List of {@link AprilTag}. + * @param fieldLength Length of the field in meters. + * @param fieldWidth Width of the field in meters. */ + public AprilTagFieldLayout(List apriltags, double fieldLength, double fieldWidth) { + this(apriltags, new FieldDimensions(fieldLength, fieldWidth)); + } + @JsonCreator - public AprilTagFieldLayout( + private AprilTagFieldLayout( @JsonProperty(required = true, value = "tags") List apriltags, @JsonProperty(required = true, value = "field") FieldDimensions fieldDimensions) { // To ensure the underlying semantics don't change with what kind of list is passed in @@ -102,15 +108,13 @@ public void setAlliance(DriverStation.Alliance alliance) { */ @SuppressWarnings("ParameterName") public Pose3d getTagPose(int ID) { - Pose3d pose = m_apriltags.stream().filter(it -> ID == it.m_ID).findFirst().get().m_poseMeters; + Pose3d pose = m_apriltags.stream().filter(it -> ID == it.ID).findFirst().get().pose; if (m_mirror) { pose = pose.relativeTo( new Pose3d( new Translation3d( - m_fieldDimensions.m_fieldWidthMeters, - m_fieldDimensions.m_fieldHeightMeters, - 0.0), + m_fieldDimensions.fieldWidth, m_fieldDimensions.fieldLength, 0.0), new Rotation3d(0.0, 0.0, Math.PI))); } @@ -153,19 +157,21 @@ public int hashCode() { @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) - public static class FieldDimensions { + private static class FieldDimensions { + @SuppressWarnings("MemberName") @JsonProperty(value = "width") - public double m_fieldWidthMeters; + public double fieldWidth; + @SuppressWarnings("MemberName") @JsonProperty(value = "height") - public double m_fieldHeightMeters; + public double fieldLength; @JsonCreator() - public FieldDimensions( - @JsonProperty(required = true, value = "width") double fieldWidthMeters, - @JsonProperty(required = true, value = "height") double fieldHeightMeters) { - m_fieldWidthMeters = fieldWidthMeters; - m_fieldHeightMeters = fieldHeightMeters; + FieldDimensions( + @JsonProperty(required = true, value = "width") double fieldWidth, + @JsonProperty(required = true, value = "height") double fieldLength) { + this.fieldWidth = fieldWidth; + this.fieldLength = fieldLength; } } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java index 88ab38cf95d..f650de39d2b 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java @@ -27,8 +27,8 @@ void poseMirroring() { new Translation3d( Units.feetToMeters(4.0), Units.feetToMeters(4), Units.feetToMeters(4)), new Rotation3d(0, 0, Units.degreesToRadians(180))))), - new AprilTagFieldLayout.FieldDimensions( - Units.feetToMeters(54.0), Units.feetToMeters(27.0))); + Units.feetToMeters(54.0), + Units.feetToMeters(27.0)); layout.setAlliance(DriverStation.Alliance.Red); assertEquals( diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java index c07057503be..4d438cfdda4 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java @@ -23,8 +23,8 @@ void deserializeMatches() { 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)))), - new AprilTagFieldLayout.FieldDimensions( - Units.feetToMeters(54.0), Units.feetToMeters(27.0))); + Units.feetToMeters(54.0), + Units.feetToMeters(27.0)); var objectMapper = new ObjectMapper(); From 65e03cfed27c2cc1bdb41ba99469452ddea7f728 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Thu, 29 Sep 2022 16:00:06 -0400 Subject: [PATCH 67/73] Suppress warnings properly --- .../src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java index 0ef8f3cd3be..29e3e74ce49 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java @@ -9,12 +9,11 @@ import edu.wpi.first.math.geometry.Pose3d; import java.util.Objects; +@SuppressWarnings("MemberName") public class AprilTag { - @SuppressWarnings("MemberName") @JsonProperty(value = "ID") public int ID; - @SuppressWarnings("MemberName") @JsonProperty(value = "pose") public Pose3d pose; From 348670e07cabd5130aa4bf080431fcaa612020f6 Mon Sep 17 00:00:00 2001 From: brennenputh Date: Thu, 29 Sep 2022 22:01:47 -0400 Subject: [PATCH 68/73] Remove debug code --- .../first/wpilibj/apriltag/AprilTagSerializationTest.java | 7 ------- 1 file changed, 7 deletions(-) diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java index 4d438cfdda4..ac47cdcf256 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java @@ -11,7 +11,6 @@ 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.List; import org.junit.jupiter.api.Test; @@ -28,12 +27,6 @@ void deserializeMatches() { var objectMapper = new ObjectMapper(); - try { - layout.serialize("/home/bagatelle/Coding/FRC/test.json"); - } catch (IOException e) { - throw new RuntimeException(e); - } - var deserialized = assertDoesNotThrow( () -> From 1e087a4f5c7d0142707c7f438ae4a4bbb457f28d Mon Sep 17 00:00:00 2001 From: Brennen Puth Date: Thu, 3 Nov 2022 15:38:03 -0400 Subject: [PATCH 69/73] Change getTagPose method to remove streams and return an Optional --- .../wpilibj/apriltag/AprilTagFieldLayout.java | 17 +++++++++++++---- .../apriltag/AprilTagPoseMirroringTest.java | 4 ++-- 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 0ebb23c9f57..87cd9baac10 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -18,6 +18,7 @@ import java.util.ArrayList; import java.util.List; import java.util.Objects; +import java.util.Optional; /** * Class for representing a layout of AprilTags on a field and reading them from a JSON format. @@ -107,8 +108,17 @@ public void setAlliance(DriverStation.Alliance alliance) { * @return The pose corresponding to the ID passed in. */ @SuppressWarnings("ParameterName") - public Pose3d getTagPose(int ID) { - Pose3d pose = m_apriltags.stream().filter(it -> ID == it.ID).findFirst().get().pose; + public Optional getTagPose(int ID) { + Pose3d pose = null; + for (AprilTag apriltag : m_apriltags) { + if (apriltag.ID == ID) { + pose = apriltag.pose; + break; + } + } + if (pose == null) { + return Optional.empty(); + } if (m_mirror) { pose = pose.relativeTo( @@ -117,8 +127,7 @@ public Pose3d getTagPose(int ID) { m_fieldDimensions.fieldWidth, m_fieldDimensions.fieldLength, 0.0), new Rotation3d(0.0, 0.0, Math.PI))); } - - return pose; + return Optional.of(pose); } /** diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java index f650de39d2b..169b2493c98 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java @@ -35,13 +35,13 @@ void poseMirroring() { 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)); + 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)); + layout.getTagPose(2).orElse(null)); } } From 9e4f2388f8b3edec38b8d67bc453becaf703925e Mon Sep 17 00:00:00 2001 From: Brennen Puth Date: Thu, 3 Nov 2022 22:14:41 -0400 Subject: [PATCH 70/73] Change to use std::optional --- .../src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp | 7 +++++-- .../main/native/include/frc/apriltag/AprilTagFieldLayout.h | 3 ++- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index 6ce6e51872e..dd3f13b413c 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -6,6 +6,7 @@ #include #include +#include "frc/geometry/Pose3d.h" #include #include @@ -42,18 +43,20 @@ void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { m_mirror = alliance == DriverStation::Alliance::kRed; } -frc::Pose3d AprilTagFieldLayout::GetTagPose(int ID) const { +std::optional AprilTagFieldLayout::GetTagPose(int ID) const { Pose3d returnPose; auto it = std::find_if(m_apriltags.begin(), m_apriltags.end(), [=](const auto& tag) { return tag.ID == ID; }); if (it != m_apriltags.end()) { returnPose = it->pose; + } else { + return std::optional(); } if (m_mirror) { returnPose = returnPose.RelativeTo(Pose3d{ m_fieldLength, m_fieldWidth, 0_m, Rotation3d{0_deg, 0_deg, 180_deg}}); } - return returnPose; + return std::make_optional(returnPose); } void AprilTagFieldLayout::Serialize(std::string_view path) { diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index 8c5d14704d1..f8ffdf37ba1 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -4,6 +4,7 @@ #pragma once +#include #include #include @@ -73,7 +74,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { * @param ID The ID of the tag. * @return The pose corresponding to the ID that was passed in. */ - Pose3d GetTagPose(int ID) const; + std::optional GetTagPose(int ID) const; /** * Serializes an AprilTagFieldLayout to a JSON file. From 2001cd225fe2fc988a7be1e28b9e598f58310418 Mon Sep 17 00:00:00 2001 From: Brennen Puth Date: Thu, 3 Nov 2022 22:15:56 -0400 Subject: [PATCH 71/73] Change to use std::optional --- wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp | 3 ++- .../test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index dd3f13b413c..5dc847057dc 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -6,7 +6,6 @@ #include #include -#include "frc/geometry/Pose3d.h" #include #include @@ -14,6 +13,8 @@ #include #include +#include "frc/geometry/Pose3d.h" + using namespace frc; AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) { diff --git a/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp b/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp index f51f8a50a85..2c3a42bb326 100644 --- a/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp @@ -26,7 +26,7 @@ TEST(AprilTagPoseMirroringTest, MirroringMatches) { auto mirrorPose = Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}}; - EXPECT_EQ(mirrorPose, layout.GetTagPose(1)); + EXPECT_EQ(mirrorPose, *layout.GetTagPose(1)); mirrorPose = Pose3d{50_ft, 23_ft, 4_ft, Rotation3d{0_deg, 0_deg, 0_deg}}; - EXPECT_EQ(mirrorPose, layout.GetTagPose(2)); + EXPECT_EQ(mirrorPose, *layout.GetTagPose(2)); } From e5fed5b8a1c8328f56b83ac9dcacbefb2c141b52 Mon Sep 17 00:00:00 2001 From: Brennen Puth Date: Thu, 3 Nov 2022 22:19:43 -0400 Subject: [PATCH 72/73] Fix accidental include --- wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp index 5dc847057dc..9efc586feab 100644 --- a/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -13,8 +13,6 @@ #include #include -#include "frc/geometry/Pose3d.h" - using namespace frc; AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) { From fcea33385bc21a2aabcdfd363aeb073016971bc1 Mon Sep 17 00:00:00 2001 From: Brennen Puth Date: Thu, 3 Nov 2022 22:29:14 -0400 Subject: [PATCH 73/73] Update docs --- .../src/main/native/include/frc/apriltag/AprilTagFieldLayout.h | 3 ++- .../edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index f8ffdf37ba1..535bc51b36c 100644 --- a/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -72,7 +72,8 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { * Gets an AprilTag pose by its ID. * * @param ID The ID of the tag. - * @return The pose corresponding to the ID that was passed in. + * @return The pose corresponding to the ID that was passed in or an empty + * optional if a tag with that ID is not found. */ std::optional GetTagPose(int ID) const; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java index 87cd9baac10..93397228acb 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -105,7 +105,8 @@ public void setAlliance(DriverStation.Alliance alliance) { * Gets an AprilTag pose by its ID. * * @param ID The ID of the tag. - * @return The pose corresponding to the ID passed in. + * @return The pose corresponding to the ID passed in or an empty optional if a tag with that ID + * was not found. */ @SuppressWarnings("ParameterName") public Optional getTagPose(int ID) {