[apriltag] Add 2024 AprilTag locations (#6168)

apriltag/README.md

@@ -0,0 +1,28 @@
+# AprilTag
+
+## Adding new field to AprilTagFields
+
+### Adding field JSON
+
+1. Add a field layout CSV file to `src/main/native/resources/edu/wpi/first/apriltag`
+    1. See docstring in `convert_apriltag_layouts.py` for more
+2. Run `convert_apriltag_layouts.py` in the same directory as this readme to generate the JSON
+3. That script overwrites all generated JSONs, so undo undesired changes if necessary
+4. Update the field dimensions at the bottom of the JSON
+    1. Length should be in meters from alliance wall to alliance wall
+    2. Width should be in meters from inside guardrail plastic to plastic
+
+### Java updates
+
+1. Update `src/main/java/edu/wpi/first/apriltag/AprilTagFields.java`
+    1. Add enum value for new field to `AprilTagFields`
+    2. Update `AprilTagFields.kDefaultField` if necessary
+
+### C++ updates
+
+1. Update `src/main/native/include/frc/apriltag/AprilTagFields.h`
+    1. Add enum value for new field to `AprilTagFields`
+    2. Update `AprilTagFields::kDefaultField` if necessary
+2. Update `src/main/native/cpp/AprilTagFields.cpp`
+    1. Add resource getter prototype like `std::string_view GetResource_2024_crescendo_json()`
+    2. Add case for new field to switch in `LoadAprilTagLayoutField()`

apriltag/convert_apriltag_layouts.py

@@ -0,0 +1,88 @@
+#!/usr/bin/env python3
+
+"""
+This script converts all AprilTag field layout CSV files in
+src/main/native/resources/edu/wpi/first/apriltag to the JSON format
+AprilTagFields expects.
+
+The input CSV has the following format:
+
+* Columns: ID, X, Y, Z, Rotation
+* ID is a positive integer
+* X, Y, and Z are decimal inches
+* Rotation is yaw in degrees
+
+The values come from a table in the layout marking diagram (e.g.,
+https://firstfrc.blob.core.windows.net/frc2024/FieldAssets/2024LayoutMarkingDiagram.pdf).
+"""
+
+import csv
+import json
+import os
+
+from wpimath import geometry, units
+import numpy as np
+
+
+def main():
+    # Find AprilTag field layout CSVs
+    filenames = [
+        os.path.join(dp, f)
+        for dp, dn, fn in os.walk("src/main/native/resources/edu/wpi/first/apriltag")
+        for f in fn
+        if f.endswith(".csv")
+    ]
+
+    for filename in filenames:
+        json_data = {"tags": [], "field": {"length": 0.0, "width": 0.0}}
+
+        # Read CSV and fill in JSON data
+        with open(filename, newline="") as csvfile:
+            reader = csv.reader(csvfile, delimiter=",")
+
+            # Skip header
+            next(reader)
+
+            for row in reader:
+                # Unpack row elements
+                id = int(row[0])
+                x = float(row[1])
+                y = float(row[2])
+                z = float(row[3])
+                rotation = float(row[4])
+
+                # Turn yaw into quaternion
+                q = geometry.Rotation3d(
+                    units.radians(0.0),
+                    units.radians(0.0),
+                    units.degreesToRadians(rotation),
+                ).getQuaternion()
+
+                json_data["tags"].append(
+                    {
+                        "ID": id,
+                        "pose": {
+                            "translation": {
+                                "x": units.inchesToMeters(x),
+                                "y": units.inchesToMeters(y),
+                                "z": units.inchesToMeters(z),
+                            },
+                            "rotation": {
+                                "quaternion": {
+                                    "W": q.W(),
+                                    "X": q.X(),
+                                    "Y": q.Y(),
+                                    "Z": q.Z(),
+                                }
+                            },
+                        },
+                    }
+                )
+
+        # Write JSON
+        with open(filename.replace(".csv", ".json"), "w") as f:
+            json.dump(json_data, f, indent=2)
+
+
+if __name__ == "__main__":
+    main()

apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java

@@ -12,13 +12,15 @@ public enum AprilTagFields {
   /** 2022 Rapid React. */
   k2022RapidReact("2022-rapidreact.json"),
   /** 2023 Charged Up. */
-  k2023ChargedUp("2023-chargedup.json");
+  k2023ChargedUp("2023-chargedup.json"),
+  /** 2024 Crescendo. */
+  k2024Crescendo("2024-crescendo.json");
 
   /** Base resource directory. */
   public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/";
 
   /** Alias to the current game. */
-  public static final AprilTagFields kDefaultField = k2023ChargedUp;
+  public static final AprilTagFields kDefaultField = k2024Crescendo;
 
   /** Resource filename. */
   public final String m_resourceFile;

apriltag/src/main/native/cpp/AprilTagFields.cpp

@@ -11,6 +11,7 @@ namespace frc {
 // C++ generated from resource files
 std::string_view GetResource_2022_rapidreact_json();
 std::string_view GetResource_2023_chargedup_json();
+std::string_view GetResource_2024_crescendo_json();
 
 AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) {
   std::string_view fieldString;
@@ -21,6 +22,9 @@ AprilTagFieldLayout LoadAprilTagLayoutField(AprilTagField field) {
     case AprilTagField::k2023ChargedUp:
       fieldString = GetResource_2023_chargedup_json();
       break;
+    case AprilTagField::k2024Crescendo:
+      fieldString = GetResource_2024_crescendo_json();
+      break;
     case AprilTagField::kNumFields:
       throw std::invalid_argument("Invalid Field");
   }

apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h

@@ -20,6 +20,8 @@ enum class AprilTagField {
   k2022RapidReact,
   /// 2023 Charged Up.
   k2023ChargedUp,
+  /// 2024 Crescendo.
+  k2024Crescendo,
 
   // This is a placeholder for denoting the last supported field. This should
   // always be the last entry in the enum and should not be used by users

apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv

@@ -0,0 +1,17 @@
+ID,X,Y,Z,Rotation
+1,593.68,9.68,53.38,120
+2,637.21,34.79,53.38,120
+3,652.73,196.17,57.13,180
+4,652.73,218.42,57.13,180
+5,578.77,323.00,53.38,270
+6,72.5,323.00,53.38,270
+7,-1.50,218.42,57.13,0
+8,-1.50,196.17,57.13,0
+9,14.02,34.79,53.38,60
+10,57.54,9.68,53.38,60
+11,468.69,146.19,52.00,300
+12,468.69,177.10,52.00,60
+13,441.74,161.62,52.00,180
+14,209.48,161.62,52.00,0
+15,182.73,177.10,52.00,120
+16,182.73,146.19,52.00,240