diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java index 3759f7a2cac..ce2336c6b15 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java @@ -113,6 +113,26 @@ public List getTags() { return new ArrayList<>(m_apriltags.values()); } + /** + * Returns the length of the field the layout is representing in meters. + * + * @return length, in meters + */ + @JsonIgnore + public double getFieldLength() { + return m_fieldDimensions.fieldLength; + } + + /** + * Returns the length of the field the layout is representing in meters. + * + * @return width, in meters + */ + @JsonIgnore + public double getFieldWidth() { + return m_fieldDimensions.fieldWidth; + } + /** * Sets the origin based on a predefined enumeration of coordinate frame origins. The origins are * calculated from the field dimensions. @@ -152,6 +172,16 @@ public void setOrigin(Pose3d origin) { m_origin = origin; } + /** + * Returns the origin used for tag pose transformation. + * + * @return the origin + */ + @JsonIgnore + public Pose3d getOrigin() { + return m_origin; + } + /** * Gets an AprilTag pose by its ID. * diff --git a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp index 78956121a5a..fd570ac4967 100644 --- a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -41,6 +41,14 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::vector apriltags, } } +units::meter_t AprilTagFieldLayout::GetFieldLength() const { + return m_fieldLength; +} + +units::meter_t AprilTagFieldLayout::GetFieldWidth() const { + return m_fieldWidth; +} + void AprilTagFieldLayout::SetOrigin(OriginPosition origin) { switch (origin) { case OriginPosition::kBlueAllianceWallRightSide: @@ -59,6 +67,10 @@ void AprilTagFieldLayout::SetOrigin(const Pose3d& origin) { m_origin = origin; } +Pose3d AprilTagFieldLayout::GetOrigin() const { + return m_origin; +} + std::optional AprilTagFieldLayout::GetTagPose(int ID) const { const auto& it = m_apriltags.find(ID); if (it == m_apriltags.end()) { diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h index dce9bedc070..ddc1fed0aeb 100644 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h @@ -62,6 +62,18 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { AprilTagFieldLayout(std::vector apriltags, units::meter_t fieldLength, units::meter_t fieldWidth); + /** + * Returns the length of the field the layout is representing. + * @return length + */ + units::meter_t GetFieldLength() const; + + /** + * Returns the length of the field the layout is representing. + * @return width + */ + units::meter_t GetFieldWidth() const; + /** * Sets the origin based on a predefined enumeration of coordinate frame * origins. The origins are calculated from the field dimensions. @@ -83,6 +95,12 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout { */ void SetOrigin(const Pose3d& origin); + /** + * Returns the origin used for tag pose transformation. + * @return the origin + */ + Pose3d GetOrigin() const; + /** * Gets an AprilTag pose by its ID. *