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 0000000000..46d696a04c --- /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/apriltag/AprilTagFieldLayout.cpp b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp new file mode 100644 index 0000000000..9efc586fea --- /dev/null +++ b/wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp @@ -0,0 +1,97 @@ +// 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 + +#include +#include +#include +#include +#include + +using namespace frc; + +AprilTagFieldLayout::AprilTagFieldLayout(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.at("tags").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::meter_t fieldLength, + units::meter_t fieldWidth) + : m_apriltags(std::move(apriltags)), + m_fieldLength(std::move(fieldLength)), + m_fieldWidth(std::move(fieldWidth)) {} + +void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) { + m_mirror = alliance == DriverStation::Alliance::kRed; +} + +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 std::make_optional(returnPose); +} + +void AprilTagFieldLayout::Serialize(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 = *this; + output << json; + output.flush(); +} + +bool AprilTagFieldLayout::operator==(const AprilTagFieldLayout& other) const { + 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 { + return !operator==(other); +} + +void frc::to_json(wpi::json& json, const AprilTagFieldLayout& layout) { + json = wpi::json{{"field", + {{"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::meter_t{json.at("field").at("width").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 0000000000..444106df23 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTag.h @@ -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. + +#pragma once + +#include + +#include "frc/geometry/Pose3d.h" + +namespace wpi { +class json; +} // namespace wpi + +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/apriltag/AprilTagFieldLayout.h b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h new file mode 100644 index 0000000000..535bc51b36 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -0,0 +1,122 @@ +// 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 + +#include "frc/DriverStation.h" +#include "frc/apriltag/AprilTag.h" +#include "frc/geometry/Pose3d.h" + +namespace wpi { +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 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 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 GetTagPose(int). Setting an alliance color via + * SetAlliance(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; + + /** + * 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. + * @param fieldLength Length of field the layout of representing. + * @param fieldWidth Width of field the layout is representing. + */ + AprilTagFieldLayout(std::vector apriltags, + units::meter_t fieldLength, units::meter_t fieldWidth); + + /** + * Set the alliance that your team is on. + * + * This changes the GetTagPose(int) method to return the correct pose for your + * alliance. + * + * @param alliance The alliance to mirror poses for. + */ + void SetAlliance(DriverStation::Alliance alliance); + + /** + * 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 or an empty + * optional if a tag with that ID is not found. + */ + std::optional GetTagPose(int ID) const; + + /** + * Serializes an AprilTagFieldLayout to a JSON file. + * + * @param path The path to write the JSON file 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: + std::vector m_apriltags; + units::meter_t m_fieldLength; + units::meter_t m_fieldWidth; + 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 +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/apriltag/AprilTagJsonTest.cpp b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp new file mode 100644 index 0000000000..157c40f466 --- /dev/null +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp @@ -0,0 +1,27 @@ +// 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 "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}}}}, + 54_ft, 27_ft}; + + AprilTagFieldLayout deserialized; + wpi::json json = layout; + EXPECT_NO_THROW(deserialized = json.get()); + EXPECT_EQ(layout, deserialized); +} 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 0000000000..2c3a42bb32 --- /dev/null +++ b/wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.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 + +#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}}}}, + 54_ft, 27_ft}; + + 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/AprilTag.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java new file mode 100644 index 0000000000..29e3e74ce4 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTag.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.apriltag; + +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.geometry.Pose3d; +import java.util.Objects; + +@SuppressWarnings("MemberName") +public class AprilTag { + @JsonProperty(value = "ID") + public int ID; + + @JsonProperty(value = "pose") + public Pose3d pose; + + @SuppressWarnings("ParameterName") + @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 new file mode 100644 index 0000000000..93397228ac --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java @@ -0,0 +1,187 @@ +// 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.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 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.Path; +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. + * + *

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 + * 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 + * AprilTagFieldLayout#getTagPose(int)}. Setting an alliance color via {@link + * AprilTagFieldLayout#setAlliance(DriverStation.Alliance)} will mirror the poses returned from + * {@link AprilTagFieldLayout#getTagPose(int)} to be correct relative to the other alliance. + */ +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) +public class AprilTagFieldLayout { + @JsonProperty(value = "tags") + private final List m_apriltags = new ArrayList<>(); + + @JsonProperty(value = "field") + private FieldDimensions m_fieldDimensions; + + private boolean m_mirror; + + /** + * 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)); + } + + /** + * 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 { + AprilTagFieldLayout layout = + new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class); + m_apriltags.addAll(layout.m_apriltags); + m_fieldDimensions = layout.m_fieldDimensions; + } + + /** + * 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 + 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 + m_apriltags.addAll(apriltags); + m_fieldDimensions = fieldDimensions; + } + + /** + * Set the alliance that your team is on. + * + *

This changes the {@link #getTagPose(int)} method to return the correct pose for your + * alliance. + * + * @param alliance The alliance to mirror poses for. + */ + public void setAlliance(DriverStation.Alliance alliance) { + m_mirror = alliance == DriverStation.Alliance.Red; + } + + /** + * Gets an AprilTag pose by its ID. + * + * @param ID The ID of the tag. + * @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) { + 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( + new Pose3d( + new Translation3d( + m_fieldDimensions.fieldWidth, m_fieldDimensions.fieldLength, 0.0), + new Rotation3d(0.0, 0.0, Math.PI))); + } + return Optional.of(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(), this); + } + + @Override + public boolean equals(Object obj) { + if (obj instanceof AprilTagFieldLayout) { + var other = (AprilTagFieldLayout) obj; + return m_apriltags.equals(other.m_apriltags) && m_mirror == other.m_mirror; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(m_apriltags, m_mirror); + } + + @JsonIgnoreProperties(ignoreUnknown = true) + @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) + private static class FieldDimensions { + @SuppressWarnings("MemberName") + @JsonProperty(value = "width") + public double fieldWidth; + + @SuppressWarnings("MemberName") + @JsonProperty(value = "height") + public double fieldLength; + + @JsonCreator() + 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 new file mode 100644 index 0000000000..169b2493c9 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java @@ -0,0 +1,47 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.apriltag; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import java.util.List; +import org.junit.jupiter.api.Test; + +class AprilTagPoseMirroringTest { + @Test + void poseMirroring() { + var layout = + new AprilTagFieldLayout( + List.of( + new AprilTag(1, new Pose3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0))), + new AprilTag( + 2, + new Pose3d( + new Translation3d( + Units.feetToMeters(4.0), Units.feetToMeters(4), Units.feetToMeters(4)), + new Rotation3d(0, 0, Units.degreesToRadians(180))))), + Units.feetToMeters(54.0), + Units.feetToMeters(27.0)); + layout.setAlliance(DriverStation.Alliance.Red); + + assertEquals( + new Pose3d( + new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0), + new Rotation3d(0.0, 0.0, Units.degreesToRadians(180.0))), + layout.getTagPose(1).orElse(null)); + + assertEquals( + new Pose3d( + new Translation3d( + Units.feetToMeters(50.0), Units.feetToMeters(23.0), Units.feetToMeters(4)), + new Rotation3d(0.0, 0.0, 0)), + layout.getTagPose(2).orElse(null)); + } +} 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 new file mode 100644 index 0000000000..ac47cdcf25 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java @@ -0,0 +1,38 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.wpilibj.apriltag; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; + +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import java.util.List; +import org.junit.jupiter.api.Test; + +class AprilTagSerializationTest { + @Test + void deserializeMatches() { + var layout = + new AprilTagFieldLayout( + List.of( + new AprilTag(1, new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0))), + new AprilTag(3, new Pose3d(0, 1, 0, new Rotation3d(0, 0, 0)))), + Units.feetToMeters(54.0), + Units.feetToMeters(27.0)); + + var objectMapper = new ObjectMapper(); + + var deserialized = + assertDoesNotThrow( + () -> + objectMapper.readValue( + objectMapper.writeValueAsString(layout), AprilTagFieldLayout.class)); + + assertEquals(layout, deserialized); + } +} 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 34dde8e767..4e32909d6a 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,10 @@ 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.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -14,6 +18,8 @@ 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 +36,10 @@ public class Pose3d implements Interpolatable { * @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; } @@ -84,6 +93,7 @@ public class Pose3d implements Interpolatable { * * @return The translational component of the pose. */ + @JsonProperty public Translation3d getTranslation() { return m_translation; } @@ -120,6 +130,7 @@ public class Pose3d implements Interpolatable { * * @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/Quaternion.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java index 2f25cd606f..cadfaa4d08 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,17 @@ 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 +33,12 @@ public class 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 +124,7 @@ public class Quaternion { * * @return W component of the quaternion. */ + @JsonProperty(value = "W") public double getW() { return m_r; } @@ -122,6 +134,7 @@ public class Quaternion { * * @return X component of the quaternion. */ + @JsonProperty(value = "X") public double getX() { return m_v.get(0, 0); } @@ -131,6 +144,7 @@ public class Quaternion { * * @return Y component of the quaternion. */ + @JsonProperty(value = "Y") public double getY() { return m_v.get(1, 0); } @@ -140,6 +154,7 @@ public class Quaternion { * * @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 91daf486d8..e7bbfa92b6 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,10 @@ 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.MatBuilder; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; @@ -17,6 +21,8 @@ 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(); @@ -28,7 +34,8 @@ public class Rotation3d implements Interpolatable { * * @param q The quaternion. */ - public Rotation3d(Quaternion q) { + @JsonCreator + public Rotation3d(@JsonProperty(required = true, value = "quaternion") Quaternion q) { m_q = q.normalize(); } @@ -270,6 +277,7 @@ public class Rotation3d implements Interpolatable { * * @return The quaternion representation of the Rotation3d. */ + @JsonProperty(value = "quaternion") public Quaternion getQuaternion() { return m_q; } 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 02d49db244..810f56cc0b 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,6 +4,10 @@ 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.MathUtil; import edu.wpi.first.math.interpolation.Interpolatable; import java.util.Objects; @@ -15,6 +19,8 @@ import java.util.Objects; * 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) public class Translation3d implements Interpolatable { private final double m_x; private final double m_y; @@ -32,7 +38,11 @@ public class Translation3d implements Interpolatable { * @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; @@ -70,6 +80,7 @@ public class Translation3d implements Interpolatable { * * @return The X component of the translation. */ + @JsonProperty public double getX() { return m_x; } @@ -79,6 +90,7 @@ public class Translation3d implements Interpolatable { * * @return The Y component of the translation. */ + @JsonProperty public double getY() { return m_y; } @@ -88,6 +100,7 @@ public class Translation3d implements Interpolatable { * * @return The Z component of the translation. */ + @JsonProperty public double getZ() { return m_z; } diff --git a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp index 1f4cb6ae76..7c5aa8be41 100644 --- a/wpimath/src/main/native/cpp/geometry/Pose3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Pose3d.cpp @@ -6,6 +6,8 @@ #include +#include + using namespace frc; namespace { @@ -149,3 +151,13 @@ Twist3d Pose3d::Log(const Pose3d& end) const { Pose2d Pose3d::ToPose2d() const { return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()}; } + +void frc::to_json(wpi::json& json, const Pose3d& pose) { + json = wpi::json{{"translation", pose.Translation()}, + {"rotation", pose.Rotation()}}; +} + +void frc::from_json(const wpi::json& json, Pose3d& pose) { + pose = Pose3d{json.at("translation").get(), + json.at("rotation").get()}; +} diff --git a/wpimath/src/main/native/cpp/geometry/Quaternion.cpp b/wpimath/src/main/native/cpp/geometry/Quaternion.cpp index 94590b2381..7f77b139ca 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) @@ -81,3 +83,16 @@ Eigen::Vector3d Quaternion::ToRotationVector() const { } } } + +void frc::to_json(wpi::json& json, const Quaternion& quaternion) { + json = wpi::json{{"W", quaternion.W()}, + {"X", quaternion.X()}, + {"Y", quaternion.Y()}, + {"Z", quaternion.Z()}}; +} + +void frc::from_json(const wpi::json& json, Quaternion& quaternion) { + quaternion = + Quaternion{json.at("W").get(), 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 d752afc864..7146df6d03 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -7,6 +7,8 @@ #include #include +#include + #include "Eigen/Core" #include "Eigen/LU" #include "Eigen/QR" @@ -238,3 +240,11 @@ units::radian_t Rotation3d::Angle() const { Rotation2d Rotation3d::ToRotation2d() const { return Rotation2d{Z()}; } + +void frc::to_json(wpi::json& json, const Rotation3d& rotation) { + json = wpi::json{{"quaternion", rotation.GetQuaternion()}}; +} + +void frc::from_json(const wpi::json& json, Rotation3d& rotation) { + rotation = Rotation3d{json.at("quaternion").get()}; +} diff --git a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp index ff3dedc9de..690a59eaf8 100644 --- a/wpimath/src/main/native/cpp/geometry/Translation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Translation3d.cpp @@ -4,6 +4,11 @@ #include "frc/geometry/Translation3d.h" +#include + +#include "units/length.h" +#include "units/math.h" + using namespace frc; Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) { @@ -39,3 +44,15 @@ bool Translation3d::operator==(const Translation3d& other) const { bool Translation3d::operator!=(const Translation3d& other) const { return !operator==(other); } + +void frc::to_json(wpi::json& json, const Translation3d& translation) { + json = wpi::json{{"x", translation.X().value()}, + {"y", translation.Y().value()}, + {"z", translation.Z().value()}}; +} + +void frc::from_json(const wpi::json& json, Translation3d& translation) { + translation = Translation3d{units::meter_t{json.at("x").get()}, + units::meter_t{json.at("y").get()}, + units::meter_t{json.at("z").get()}}; +} diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index b75e845d98..32ac7f3ab7 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 { /** @@ -202,4 +206,10 @@ class WPILIB_DLLEXPORT Pose3d { Rotation3d m_rotation; }; +WPILIB_DLLEXPORT +void to_json(wpi::json& json, const Pose3d& pose); + +WPILIB_DLLEXPORT +void from_json(const wpi::json& json, Pose3d& pose); + } // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/Quaternion.h b/wpimath/src/main/native/include/frc/geometry/Quaternion.h index c21db3c808..97933abe5b 100644 --- a/wpimath/src/main/native/include/frc/geometry/Quaternion.h +++ b/wpimath/src/main/native/include/frc/geometry/Quaternion.h @@ -8,6 +8,10 @@ #include "frc/EigenCore.h" +namespace wpi { +class json; +} // namespace wpi + namespace frc { class WPILIB_DLLEXPORT Quaternion { @@ -92,4 +96,10 @@ class WPILIB_DLLEXPORT Quaternion { Eigen::Vector3d m_v{0.0, 0.0, 0.0}; }; +WPILIB_DLLEXPORT +void to_json(wpi::json& json, const Quaternion& quaternion); + +WPILIB_DLLEXPORT +void from_json(const wpi::json& json, Quaternion& quaternion); + } // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index fe74ba433d..a2bffbbdb9 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -11,6 +11,10 @@ #include "frc/EigenCore.h" #include "units/angle.h" +namespace wpi { +class json; +} // namespace wpi + namespace frc { /** @@ -180,4 +184,10 @@ class WPILIB_DLLEXPORT Rotation3d { Quaternion m_q; }; +WPILIB_DLLEXPORT +void to_json(wpi::json& json, const Rotation3d& rotation); + +WPILIB_DLLEXPORT +void from_json(const wpi::json& json, Rotation3d& rotation); + } // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index 36dc258835..4477749f49 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 { /** @@ -182,6 +186,12 @@ class WPILIB_DLLEXPORT Translation3d { units::meter_t m_z = 0_m; }; +WPILIB_DLLEXPORT +void to_json(wpi::json& json, const Translation3d& state); + +WPILIB_DLLEXPORT +void from_json(const wpi::json& json, Translation3d& state); + } // namespace frc #include "Translation3d.inc"