Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

SCRIPT: wpiformat

Browse Source
This commit is contained in:
PJ Reiniger
2025-11-07 20:01:58 -05:00
committed by Peter Johnson
parent ae6bdc9d25
commit 2109161534
749 changed files with 5504 additions and 3936 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
apriltag/src/main/native/cpp/AprilTagFieldLayout.cpp
View File

@@ -22,13 +22,16 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) {
throw std::runtime_error(fmt::format("Cannot open file: {}", path));
}
wpi::util::json json = wpi::util::json::parse(fileBuffer.value()->GetCharBuffer());
wpi::util::json json =
wpi::util::json::parse(fileBuffer.value()->GetCharBuffer());
for (const auto& tag : json.at("tags").get<std::vector<AprilTag>>()) {
m_apriltags[tag.ID] = tag;
}
m_fieldWidth = wpi::units::meter_t{json.at("field").at("width").get<double>()};
m_fieldLength = wpi::units::meter_t{json.at("field").at("length").get<double>()};
m_fieldWidth =
wpi::units::meter_t{json.at("field").at("width").get<double>()};
m_fieldLength =
wpi::units::meter_t{json.at("field").at("length").get<double>()};
}
AprilTagFieldLayout::AprilTagFieldLayout(std::vector<AprilTag> apriltags,
@@ -64,8 +67,9 @@ void AprilTagFieldLayout::SetOrigin(OriginPosition origin) {
SetOrigin(wpi::math::Pose3d{});
break;
case OriginPosition::kRedAllianceWallRightSide:
SetOrigin(wpi::math::Pose3d{wpi::math::Translation3d{m_fieldLength, m_fieldWidth, 0_m},
wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}});
SetOrigin(wpi::math::Pose3d{
wpi::math::Translation3d{m_fieldLength, m_fieldWidth, 0_m},
wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}});
break;
default:
throw std::invalid_argument("Invalid origin");
@@ -101,7 +105,8 @@ void AprilTagFieldLayout::Serialize(std::string_view path) {
output.flush();
}
void wpi::apriltag::to_json(wpi::util::json& json, const AprilTagFieldLayout& layout) {
void wpi::apriltag::to_json(wpi::util::json& json,
const AprilTagFieldLayout& layout) {
std::vector<AprilTag> tagVector;
tagVector.reserve(layout.m_apriltags.size());
for (const auto& pair : layout.m_apriltags) {
@@ -109,12 +114,13 @@ void wpi::apriltag::to_json(wpi::util::json& json, const AprilTagFieldLayout& la
}
json = wpi::util::json{{"field",
{{"length", layout.m_fieldLength.value()},
{"width", layout.m_fieldWidth.value()}}},
{"tags", tagVector}};
{{"length", layout.m_fieldLength.value()},
{"width", layout.m_fieldWidth.value()}}},
{"tags", tagVector}};
}
void wpi::apriltag::from_json(const wpi::util::json& json, AprilTagFieldLayout& layout) {
void wpi::apriltag::from_json(const wpi::util::json& json,
AprilTagFieldLayout& layout) {
layout.m_apriltags.clear();
for (const auto& tag : json.at("tags").get<std::vector<AprilTag>>()) {
layout.m_apriltags[tag.ID] = tag;
@@ -164,6 +170,7 @@ AprilTagFieldLayout AprilTagFieldLayout::LoadField(AprilTagField field) {
return json.get<AprilTagFieldLayout>();
}
AprilTagFieldLayout wpi::apriltag::LoadAprilTagLayoutField(AprilTagField field) {
AprilTagFieldLayout wpi::apriltag::LoadAprilTagLayoutField(
AprilTagField field) {
return AprilTagFieldLayout::LoadField(field);
}

9
apriltag/src/main/native/cpp/AprilTagPoseEstimator.cpp
View File

@@ -47,8 +47,8 @@ static wpi::math::Transform3d MakePose(const apriltag_pose_t& pose) {
return {};
}
return {wpi::math::Translation3d{wpi::units::meter_t{pose.t->data[0]},
wpi::units::meter_t{pose.t->data[1]},
wpi::units::meter_t{pose.t->data[2]}},
wpi::units::meter_t{pose.t->data[1]},
wpi::units::meter_t{pose.t->data[2]}},
wpi::math::Rotation3d{OrthogonalizeRotationMatrix(
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>{
pose.R->data})}};
@@ -130,8 +130,9 @@ AprilTagPoseEstimate AprilTagPoseEstimator::EstimateOrthogonalIteration(
return rv;
}
static wpi::math::Transform3d DoEstimate(const apriltag_detection_t* detection,
const AprilTagPoseEstimator::Config& config) {
static wpi::math::Transform3d DoEstimate(
const apriltag_detection_t* detection,
const AprilTagPoseEstimator::Config& config) {
auto info = MakeDetectionInfo(detection, config);
apriltag_pose_t pose;
estimate_tag_pose(&info, &pose);

9
apriltag/src/main/native/cpp/jni/AprilTagJNI.cpp
View File

@@ -517,7 +517,8 @@ Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_estimatePoseHomography
return nullptr;
}
AprilTagPoseEstimator estimator({wpi::units::meter_t{tagSize}, fx, fy, cx, cy});
AprilTagPoseEstimator estimator(
{wpi::units::meter_t{tagSize}, fx, fy, cx, cy});
return MakeJObject(env, estimator.EstimateHomography(harr));
}
@@ -553,7 +554,8 @@ Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_estimatePoseOrthogonalIteration
return nullptr;
}
AprilTagPoseEstimator estimator({wpi::units::meter_t{tagSize}, fx, fy, cx, cy});
AprilTagPoseEstimator estimator(
{wpi::units::meter_t{tagSize}, fx, fy, cx, cy});
return MakeJObject(env,
estimator.EstimateOrthogonalIteration(harr, carr, nIters));
}
@@ -590,7 +592,8 @@ Java_org_wpilib_vision_apriltag_jni_AprilTagJNI_estimatePose
return nullptr;
}
AprilTagPoseEstimator estimator({wpi::units::meter_t{tagSize}, fx, fy, cx, cy});
AprilTagPoseEstimator estimator(
{wpi::units::meter_t{tagSize}, fx, fy, cx, cy});
return MakeJObject(env, estimator.Estimate(harr, carr));
}

11
apriltag/src/main/native/include/wpi/apriltag/AprilTagFieldLayout.hpp
View File

@@ -23,10 +23,10 @@ namespace wpi::apriltag {
*
* The JSON format contains two top-level objects, "tags" and "field".
* The "tags" object is a list of all AprilTags contained within a layout. Each
* AprilTag serializes to a JSON object containing an ID and a wpi::math::Pose3d. The
* "field" object is a descriptor of the size of the field in meters with
* "width" and "length" values. This is to account for arbitrary field sizes
* when transforming the poses.
* AprilTag serializes to a JSON object containing an ID and a
* wpi::math::Pose3d. The "field" object is a descriptor of the size of the
* field in meters with "width" and "length" values. This is to account for
* arbitrary field sizes when transforming the poses.
*
* Pose3ds in the JSON are measured using the normal FRC coordinate system, NWU
* with the origin at the bottom-right corner of the blue alliance wall.
@@ -73,7 +73,8 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout {
* @param fieldWidth Width of field the layout is representing.
*/
AprilTagFieldLayout(std::vector<AprilTag> apriltags,
wpi::units::meter_t fieldLength, wpi::units::meter_t fieldWidth);
wpi::units::meter_t fieldLength,
wpi::units::meter_t fieldWidth);
/**
* Returns the length of the field the layout is representing.

8
apriltag/src/main/native/include/wpi/apriltag/AprilTagPoseEstimator.hpp
View File

@@ -65,7 +65,8 @@ class WPILIB_DLLEXPORT AprilTagPoseEstimator {
* @param detection Tag detection
* @return Pose estimate
*/
wpi::math::Transform3d EstimateHomography(const AprilTagDetection& detection) const;
wpi::math::Transform3d EstimateHomography(
const AprilTagDetection& detection) const;
/**
* Estimates the pose of the tag using the homography method described in [1].
@@ -73,7 +74,8 @@ class WPILIB_DLLEXPORT AprilTagPoseEstimator {
* @param homography Homography 3x3 matrix data
* @return Pose estimate
*/
wpi::math::Transform3d EstimateHomography(std::span<const double, 9> homography) const;
wpi::math::Transform3d EstimateHomography(
std::span<const double, 9> homography) const;
/**
* Estimates the pose of the tag. This returns one or two possible poses for
@@ -135,7 +137,7 @@ class WPILIB_DLLEXPORT AprilTagPoseEstimator {
* @return Pose estimate
*/
wpi::math::Transform3d Estimate(std::span<const double, 9> homography,
std::span<const double, 8> corners) const;
std::span<const double, 8> corners) const;
private:
Config m_config;

7
apriltag/src/test/native/cpp/AprilTagJsonTest.cpp
View File

@@ -15,9 +15,10 @@ using namespace wpi::apriltag;
TEST(AprilTagJsonTest, DeserializeMatches) {
auto layout = AprilTagFieldLayout{
std::vector{
AprilTag{1, wpi::math::Pose3d{}},
AprilTag{3, wpi::math::Pose3d{0_m, 1_m, 0_m, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}}},
std::vector{AprilTag{1, wpi::math::Pose3d{}},
AprilTag{3, wpi::math::Pose3d{0_m, 1_m, 0_m,
wpi::math::Rotation3d{
0_deg, 0_deg, 0_deg}}}},
54_ft, 27_ft};
AprilTagFieldLayout deserialized;

15
apriltag/src/test/native/cpp/AprilTagPoseSetOriginTest.cpp
View File

@@ -16,18 +16,21 @@ using namespace wpi::apriltag;
TEST(AprilTagPoseSetOriginTest, TransformationMatches) {
auto layout = AprilTagFieldLayout{
std::vector<AprilTag>{
AprilTag{1,
wpi::math::Pose3d{0_ft, 0_ft, 0_ft, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}},
AprilTag{
2, wpi::math::Pose3d{4_ft, 4_ft, 4_ft, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}}}},
1, wpi::math::Pose3d{0_ft, 0_ft, 0_ft,
wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}}},
AprilTag{2, wpi::math::Pose3d{4_ft, 4_ft, 4_ft,
wpi::math::Rotation3d{0_deg, 0_deg,
180_deg}}}},
54_ft, 27_ft};
layout.SetOrigin(
AprilTagFieldLayout::OriginPosition::kRedAllianceWallRightSide);
auto mirrorPose =
wpi::math::Pose3d{54_ft, 27_ft, 0_ft, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}};
auto mirrorPose = wpi::math::Pose3d{
54_ft, 27_ft, 0_ft, wpi::math::Rotation3d{0_deg, 0_deg, 180_deg}};
EXPECT_EQ(mirrorPose, *layout.GetTagPose(1));
mirrorPose = wpi::math::Pose3d{50_ft, 23_ft, 4_ft, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}};
mirrorPose = wpi::math::Pose3d{50_ft, 23_ft, 4_ft,
wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}};
EXPECT_EQ(mirrorPose, *layout.GetTagPose(2));
}

9
apriltag/src/test/native/cpp/LoadConfigTest.cpp
View File

@@ -27,20 +27,23 @@ TEST(AprilTagFieldsTest, TestLoad2022RapidReact) {
// Blue Hangar Truss - Hub
auto expectedPose =
wpi::math::Pose3d{127.272_in, 216.01_in, 67.932_in, wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}};
wpi::math::Pose3d{127.272_in, 216.01_in, 67.932_in,
wpi::math::Rotation3d{0_deg, 0_deg, 0_deg}};
auto maybePose = layout.GetTagPose(1);
EXPECT_TRUE(maybePose);
EXPECT_EQ(expectedPose, *maybePose);
// Blue Terminal Near Station
expectedPose = wpi::math::Pose3d{4.768_in, 67.631_in, 35.063_in,
expectedPose =
wpi::math::Pose3d{4.768_in, 67.631_in, 35.063_in,
wpi::math::Rotation3d{0_deg, 0_deg, 46.25_deg}};
maybePose = layout.GetTagPose(5);
EXPECT_TRUE(maybePose);
EXPECT_EQ(expectedPose, *maybePose);
// Upper Hub Blue-Near
expectedPose = wpi::math::Pose3d{332.321_in, 183.676_in, 95.186_in,
expectedPose =
wpi::math::Pose3d{332.321_in, 183.676_in, 95.186_in,
wpi::math::Rotation3d{0_deg, 26.75_deg, 69_deg}};
maybePose = layout.GetTagPose(53);
EXPECT_TRUE(maybePose);