diff --git a/apriltag/convert_apriltag_layouts.py b/apriltag/convert_apriltag_layouts.py index 75d1f143cb..73b8aa7ed1 100755 --- a/apriltag/convert_apriltag_layouts.py +++ b/apriltag/convert_apriltag_layouts.py @@ -7,10 +7,11 @@ AprilTagFields expects. The input CSV has the following format: -* Columns: ID, X, Y, Z, Rotation +* Columns: ID, X, Y, Z, Z Rotation, Y Rotation * ID is a positive integer * X, Y, and Z are decimal inches -* Rotation is yaw in degrees +* Z Rotation is yaw in degrees +* Y Rotation is pitch 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). @@ -48,13 +49,14 @@ def main(): x = float(row[1]) y = float(row[2]) z = float(row[3]) - rotation = float(row[4]) + zRotation = float(row[4]) + yRotation = float(row[5]) # Turn yaw into quaternion q = geometry.Rotation3d( - units.radians(0.0), - units.radians(0.0), - units.degreesToRadians(rotation), + units.radians(0), + units.degreesToRadians(yRotation), + units.degreesToRadians(zRotation), ).getQuaternion() json_data["tags"].append( diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java index 016c982fe7..63a516aa38 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFields.java @@ -13,13 +13,15 @@ public enum AprilTagFields { /** 2023 Charged Up. */ k2023ChargedUp("2023-chargedup.json"), /** 2024 Crescendo. */ - k2024Crescendo("2024-crescendo.json"); + k2024Crescendo("2024-crescendo.json"), + /** 2025 Reefscape. */ + k2025Reefscape("2025-reefscape.json"); /** Base resource directory. */ public static final String kBaseResourceDir = "/edu/wpi/first/apriltag/"; /** Alias to the current game. */ - public static final AprilTagFields kDefaultField = k2024Crescendo; + public static final AprilTagFields kDefaultField = k2025Reefscape; /** Resource filename. */ public final String m_resourceFile; diff --git a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp index c90fad3626..04d7a78c60 100644 --- a/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp +++ b/apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp @@ -133,6 +133,7 @@ namespace frc { std::string_view GetResource_2022_rapidreact_json(); std::string_view GetResource_2023_chargedup_json(); std::string_view GetResource_2024_crescendo_json(); +std::string_view GetResource_2025_reefscape_json(); } // namespace frc @@ -148,6 +149,9 @@ AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) { case AprilTagField::k2024Crescendo: fieldString = GetResource_2024_crescendo_json(); break; + case AprilTagField::k2025Reefscape: + fieldString = GetResource_2025_reefscape_json(); + break; case AprilTagField::kNumFields: throw std::invalid_argument("Invalid Field"); } diff --git a/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h b/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h index 24040b6423..94ac799c85 100644 --- a/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h +++ b/apriltag/src/main/native/include/frc/apriltag/AprilTagFields.h @@ -20,8 +20,10 @@ enum class AprilTagField { k2023ChargedUp, /// 2024 Crescendo. k2024Crescendo, + /// 2025 Reefscape. + k2025Reefscape, /// Alias to the current game. - kDefaultField = k2024Crescendo, + kDefaultField = k2025Reefscape, // 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 diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv deleted file mode 100644 index cf570ce1b5..0000000000 --- a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2024-crescendo.csv +++ /dev/null @@ -1,17 +0,0 @@ -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 diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.csv b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.csv new file mode 100644 index 0000000000..b8fd985408 --- /dev/null +++ b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.csv @@ -0,0 +1,23 @@ +ID,X,Y,Z,Z-Rotation,X-Rotation +1,657.37,25.8,58.5,126,0 +2,657.37,291.2,58.5,234,0 +3,455.15,317.15,51.25,270,0 +4,365.2,241.64,73.54,0,30 +5,365.2,75.39,73.54,0,30 +6,530.49,130.17,12.13,300,0 +7,546.87,158.5,12.13,0,0 +8,530.49,186.83,12.13,60,0 +9,497.77,186.83,12.13,120,0 +10,481.39,158.5,12.13,180,0 +11,497.77,130.17,12.13,240,0 +12,33.51,25.8,58.5,54,0 +13,33.51,291.2,58.5,306,0 +14,325.68,241.64,73.54,180,30 +15,325.68,75.39,73.54,180,30 +16,235.73,-0.15,51.25,90,0 +17,160.39,130.17,12.13,240,0 +18,144,158.5,12.13,180,0 +19,160.39,186.83,12.13,120,0 +20,193.1,186.83,12.13,60,0 +21,209.49,158.5,12.13,0,0 +22,193.1,130.17,12.13,300,0 diff --git a/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.json b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.json new file mode 100644 index 0000000000..eb395c0b67 --- /dev/null +++ b/apriltag/src/main/native/resources/edu/wpi/first/apriltag/2025-reefscape.json @@ -0,0 +1,404 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 16.697198, + "y": 0.65532, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": 0.4539904997395468, + "X": 0.0, + "Y": 0.0, + "Z": 0.8910065241883678 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.697198, + "y": 7.3964799999999995, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": -0.45399049973954675, + "X": -0.0, + "Y": 0.0, + "Z": 0.8910065241883679 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.560809999999998, + "y": 8.05561, + "z": 1.30175 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 9.276079999999999, + "y": 6.137656, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 0.9659258262890683, + "X": 0.0, + "Y": 0.25881904510252074, + "Z": 0.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 9.276079999999999, + "y": 1.914906, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 0.9659258262890683, + "X": 0.0, + "Y": 0.25881904510252074, + "Z": 0.0 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 13.474446, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 13.890498, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 13.474446, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.643358, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.227305999999999, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.643358, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 0.851154, + "y": 0.65532, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": 0.8910065241883679, + "X": 0.0, + "Y": 0.0, + "Z": 0.45399049973954675 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 0.851154, + "y": 7.3964799999999995, + "z": 1.4859 + }, + "rotation": { + "quaternion": { + "W": -0.8910065241883678, + "X": -0.0, + "Y": 0.0, + "Z": 0.45399049973954686 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 8.272272, + "y": 6.137656, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 5.914589856893349e-17, + "X": -0.25881904510252074, + "Y": 1.5848095757158825e-17, + "Z": 0.9659258262890683 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 8.272272, + "y": 1.914906, + "z": 1.8679160000000001 + }, + "rotation": { + "quaternion": { + "W": 5.914589856893349e-17, + "X": -0.25881904510252074, + "Y": 1.5848095757158825e-17, + "Z": 0.9659258262890683 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 5.9875419999999995, + "y": -0.0038099999999999996, + "z": 1.30175 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.073905999999999, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 3.6576, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 4.073905999999999, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 4.904739999999999, + "y": 4.745482, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 5.321046, + "y": 4.0259, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.904739999999999, + "y": 3.3063179999999996, + "z": 0.308102 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + } + ], + "field": { + "length": 17.548, + "width": 8.052 + } +} diff --git a/fieldImages/src/main/native/cpp/fields.cpp b/fieldImages/src/main/native/cpp/fields.cpp index 6fd6af0936..06e5f8ed40 100644 --- a/fieldImages/src/main/native/cpp/fields.cpp +++ b/fieldImages/src/main/native/cpp/fields.cpp @@ -16,10 +16,13 @@ #include "fields/2022-rapidreact.h" #include "fields/2023-chargedup.h" #include "fields/2024-crescendo.h" +#include "fields/2025-reefscape.h" using namespace fields; static const Field kFields[] = { + {"2025 Reefscape", GetResource_2025_reefscape_json, + GetResource_2025_field_png}, {"2024 Crescendo", GetResource_2024_crescendo_json, GetResource_2024_field_png}, {"2023 Charged Up", GetResource_2023_chargedup_json, diff --git a/fieldImages/src/main/native/include/fields/2025-reefscape.h b/fieldImages/src/main/native/include/fields/2025-reefscape.h new file mode 100644 index 0000000000..ada49b88a6 --- /dev/null +++ b/fieldImages/src/main/native/include/fields/2025-reefscape.h @@ -0,0 +1,12 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +namespace fields { +std::string_view GetResource_2025_reefscape_json(); +std::string_view GetResource_2025_field_png(); +} // namespace fields diff --git a/fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-field.png b/fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-field.png new file mode 100644 index 0000000000..66e54d915e Binary files /dev/null and b/fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-field.png differ diff --git a/fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-reefscape.json b/fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-reefscape.json new file mode 100644 index 0000000000..c8c9521dea --- /dev/null +++ b/fieldImages/src/main/native/resources/edu/wpi/first/fields/2025-reefscape.json @@ -0,0 +1,19 @@ +{ + "game": "Reefscape", + "field-image": "2025-field.png", + "field-corners": { + "top-left": [ + 176, + 38 + ], + "bottom-right": [ + 1401, + 601 + ] + }, + "field-size": [ + 57.573, + 26.417 + ], + "field-unit": "foot" +}