From 04a781b4d7e37c3c4d0609b12af3e119a00a8cc1 Mon Sep 17 00:00:00 2001
From: Drew Williams <williams.r.drew@gmail.com>
Date: Sat, 4 Nov 2023 13:26:07 -0400
Subject: [PATCH] [apriltag] Add GetTags to C++ version of AprilTagFieldLayout
 (#5872)

---
 apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp     | 9 +++++++++
 .../native/include/frc/apriltag/AprilTagFieldLayout.h    | 6 ++++++
 2 files changed, 15 insertions(+)

diff --git a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp
index fd570ac4967..c971a488fc3 100644
--- a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp
+++ b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp
@@ -49,6 +49,15 @@ units::meter_t AprilTagFieldLayout::GetFieldWidth() const {
   return m_fieldWidth;
 }
 
+std::vector<AprilTag> AprilTagFieldLayout::GetTags() const {
+  std::vector<AprilTag> tags;
+  tags.reserve(m_apriltags.size());
+  for (const auto& tag : m_apriltags) {
+    tags.emplace_back(tag.second);
+  }
+  return tags;
+}
+
 void AprilTagFieldLayout::SetOrigin(OriginPosition origin) {
   switch (origin) {
     case OriginPosition::kBlueAllianceWallRightSide:
diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h
index ddc1fed0aeb..2b57d2ce4aa 100644
--- a/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h
+++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h
@@ -74,6 +74,12 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout {
    */
   units::meter_t GetFieldWidth() const;
 
+  /**
+   * Returns a vector of all the april tags used in this layout.
+   * @return list of tags
+   */
+  std::vector<AprilTag> GetTags() const;
+
   /**
    * Sets the origin based on a predefined enumeration of coordinate frame
    * origins. The origins are calculated from the field dimensions.