Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +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

12
wpimath/src/main/native/cpp/geometry/Pose3d.cpp
View File

@@ -6,6 +6,8 @@
#include <cmath>
#include <wpi/json.h>
using namespace frc;
namespace {
@@ -149,3 +151,13 @@ Twist3d Pose3d::Log(const Pose3d& end) const {
Pose2d Pose3d::ToPose2d() const {
return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
}
void frc::to_json(wpi::json& json, const Pose3d& pose) {
json = wpi::json{{"translation", pose.Translation()},
{"rotation", pose.Rotation()}};
}
void frc::from_json(const wpi::json& json, Pose3d& pose) {
pose = Pose3d{json.at("translation").get<Translation3d>(),
json.at("rotation").get<Rotation3d>()};
}

15
wpimath/src/main/native/cpp/geometry/Quaternion.cpp
View File

@@ -4,6 +4,8 @@
#include "frc/geometry/Quaternion.h"
#include <wpi/json.h>
using namespace frc;
Quaternion::Quaternion(double w, double x, double y, double z)
@@ -81,3 +83,16 @@ Eigen::Vector3d Quaternion::ToRotationVector() const {
}
}
}
void frc::to_json(wpi::json& json, const Quaternion& quaternion) {
json = wpi::json{{"W", quaternion.W()},
{"X", quaternion.X()},
{"Y", quaternion.Y()},
{"Z", quaternion.Z()}};
}
void frc::from_json(const wpi::json& json, Quaternion& quaternion) {
quaternion =
Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
json.at("Y").get<double>(), json.at("Z").get<double>()};
}

10
wpimath/src/main/native/cpp/geometry/Rotation3d.cpp
View File

@@ -7,6 +7,8 @@
#include <cmath>
#include <numbers>
#include <wpi/json.h>
#include "Eigen/Core"
#include "Eigen/LU"
#include "Eigen/QR"
@@ -238,3 +240,11 @@ units::radian_t Rotation3d::Angle() const {
Rotation2d Rotation3d::ToRotation2d() const {
return Rotation2d{Z()};
}
void frc::to_json(wpi::json& json, const Rotation3d& rotation) {
json = wpi::json{{"quaternion", rotation.GetQuaternion()}};
}
void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
}

17
wpimath/src/main/native/cpp/geometry/Translation3d.cpp
View File

@@ -4,6 +4,11 @@
#include "frc/geometry/Translation3d.h"
#include <wpi/json.h>
#include "units/length.h"
#include "units/math.h"
using namespace frc;
Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
@@ -39,3 +44,15 @@ bool Translation3d::operator==(const Translation3d& other) const {
bool Translation3d::operator!=(const Translation3d& other) const {
return !operator==(other);
}
void frc::to_json(wpi::json& json, const Translation3d& translation) {
json = wpi::json{{"x", translation.X().value()},
{"y", translation.Y().value()},
{"z", translation.Z().value()}};
}
void frc::from_json(const wpi::json& json, Translation3d& translation) {
translation = Translation3d{units::meter_t{json.at("x").get<double>()},
units::meter_t{json.at("y").get<double>()},
units::meter_t{json.at("z").get<double>()}};
}