mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
[apriltag] Add 2024 AprilTag locations (#6168)
This commit is contained in:
Tyler Veness
28
apriltag/README.md
Normal file
28
apriltag/README.md
Normal file
@@ -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
apriltag/convert_apriltag_layouts.py
Executable file
88
apriltag/convert_apriltag_layouts.py
Executable file
@@ -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()
|
||||
@@ -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;
|
||||
|
||||
@@ -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");
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
@@ -0,0 +1,296 @@
|
||||
{
|
||||
"tags": [
|
||||
{
|
||||
"ID": 1,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 15.079471999999997,
|
||||
"y": 0.24587199999999998,
|
||||
"z": 1.355852
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": 0.5000000000000001,
|
||||
"X": 0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 0.8660254037844386
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 2,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 16.185134,
|
||||
"y": 0.883666,
|
||||
"z": 1.355852
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": 0.5000000000000001,
|
||||
"X": 0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 0.8660254037844386
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 3,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 16.579342,
|
||||
"y": 4.982717999999999,
|
||||
"z": 1.4511020000000001
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": 6.123233995736766e-17,
|
||||
"X": 0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 1.0
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 4,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 16.579342,
|
||||
"y": 5.547867999999999,
|
||||
"z": 1.4511020000000001
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": 6.123233995736766e-17,
|
||||
"X": 0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 1.0
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 5,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 14.700757999999999,
|
||||
"y": 8.2042,
|
||||
"z": 1.355852
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": -0.7071067811865475,
|
||||
"X": -0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 0.7071067811865476
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 6,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 1.8415,
|
||||
"y": 8.2042,
|
||||
"z": 1.355852
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": -0.7071067811865475,
|
||||
"X": -0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 0.7071067811865476
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 7,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": -0.038099999999999995,
|
||||
"y": 5.547867999999999,
|
||||
"z": 1.4511020000000001
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": 1.0,
|
||||
"X": 0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 0.0
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 8,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": -0.038099999999999995,
|
||||
"y": 4.982717999999999,
|
||||
"z": 1.4511020000000001
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": 1.0,
|
||||
"X": 0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 0.0
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 9,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 0.356108,
|
||||
"y": 0.883666,
|
||||
"z": 1.355852
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": 0.8660254037844387,
|
||||
"X": 0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 0.49999999999999994
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 10,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 1.4615159999999998,
|
||||
"y": 0.24587199999999998,
|
||||
"z": 1.355852
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": 0.8660254037844387,
|
||||
"X": 0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 0.49999999999999994
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 11,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 11.904726,
|
||||
"y": 3.7132259999999997,
|
||||
"z": 1.3208
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": -0.8660254037844387,
|
||||
"X": -0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 0.49999999999999994
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 12,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 11.904726,
|
||||
"y": 4.49834,
|
||||
"z": 1.3208
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": 0.8660254037844387,
|
||||
"X": 0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 0.49999999999999994
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 13,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 11.220196,
|
||||
"y": 4.105148,
|
||||
"z": 1.3208
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": 6.123233995736766e-17,
|
||||
"X": 0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 1.0
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 14,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 5.320792,
|
||||
"y": 4.105148,
|
||||
"z": 1.3208
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": 1.0,
|
||||
"X": 0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 0.0
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 15,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 4.641342,
|
||||
"y": 4.49834,
|
||||
"z": 1.3208
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": 0.5000000000000001,
|
||||
"X": 0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 0.8660254037844386
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"ID": 16,
|
||||
"pose": {
|
||||
"translation": {
|
||||
"x": 4.641342,
|
||||
"y": 3.7132259999999997,
|
||||
"z": 1.3208
|
||||
},
|
||||
"rotation": {
|
||||
"quaternion": {
|
||||
"W": -0.4999999999999998,
|
||||
"X": -0.0,
|
||||
"Y": 0.0,
|
||||
"Z": 0.8660254037844387
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
],
|
||||
"field": {
|
||||
"length": 16.451,
|
||||
"width": 8.211
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user