Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add AprilTag and AprilTagFieldLayout (#4421)

Browse Source 
This is an API for looking up a Pose3d from a tag id, and includes functionality to load that map from a JSON file.

This also adds JSON support to Pose3d, Rotation3d. Translation3d, and Quaternion.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: AMereBagatelle <themerebagatelle@gmail.com>
This commit is contained in:
Brennen Puth
2022-11-04 12:56:22 -04:00
committed by GitHub
parent 7aab8fa93a
commit b2b473b24a
22 changed files with 813 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp Normal file
View File

@@ -0,0 +1,27 @@
// 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.
#include <vector>
#include <wpi/json.h>
#include "frc/apriltag/AprilTag.h"
#include "frc/apriltag/AprilTagFieldLayout.h"
#include "frc/geometry/Pose3d.h"
#include "gtest/gtest.h"
using namespace frc;
TEST(AprilTagJsonTest, DeserializeMatches) {
auto layout = AprilTagFieldLayout{
std::vector{
AprilTag{1, Pose3d{}},
AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}},
54_ft, 27_ft};
AprilTagFieldLayout deserialized;
wpi::json json = layout;
EXPECT_NO_THROW(deserialized = json.get<AprilTagFieldLayout>());
EXPECT_EQ(layout, deserialized);
}

32
wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp Normal file
View File

@@ -0,0 +1,32 @@
// 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.
#include <vector>
#include <wpi/json.h>
#include "frc/apriltag/AprilTag.h"
#include "frc/apriltag/AprilTagFieldLayout.h"
#include "frc/geometry/Pose3d.h"
#include "gtest/gtest.h"
using namespace frc;
TEST(AprilTagPoseMirroringTest, MirroringMatches) {
auto layout = AprilTagFieldLayout{
std::vector<AprilTag>{
AprilTag{1,
Pose3d{0_ft, 0_ft, 0_ft, Rotation3d{0_deg, 0_deg, 0_deg}}},
AprilTag{
2, Pose3d{4_ft, 4_ft, 4_ft, Rotation3d{0_deg, 0_deg, 180_deg}}}},
54_ft, 27_ft};
layout.SetAlliance(DriverStation::Alliance::kRed);
auto mirrorPose =
Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}};
EXPECT_EQ(mirrorPose, *layout.GetTagPose(1));
mirrorPose = Pose3d{50_ft, 23_ft, 4_ft, Rotation3d{0_deg, 0_deg, 0_deg}};
EXPECT_EQ(mirrorPose, *layout.GetTagPose(2));
}