Skip to content

Commit

Permalink
[apriltag] Add 2024 AprilTag locations (#6168)
Browse files Browse the repository at this point in the history
  • Loading branch information
calcmogul authored Jan 6, 2024
1 parent dd90965 commit 4809f3d
Show file tree
Hide file tree
Showing 7 changed files with 439 additions and 2 deletions.
28 changes: 28 additions & 0 deletions apriltag/README.md
Original file line number Diff line number Diff line change
@@ -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()`
88 changes: 88 additions & 0 deletions apriltag/convert_apriltag_layouts.py
Original file line number Diff line number Diff line change
@@ -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()
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
4 changes: 4 additions & 0 deletions apriltag/src/main/native/cpp/AprilTagFields.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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");
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Original file line number Diff line number Diff line change
@@ -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
Loading

0 comments on commit 4809f3d

Please sign in to comment.