mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
60 lines
2.0 KiB
C
60 lines
2.0 KiB
C
|
// 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 <cmath>
|
||
|
|
#include <vector>
|
||
|
|
|
||
|
|
#include <tagpose.h>
|
||
|
|
#include <wpi/json.h>
|
||
|
|
|
||
|
|
class Fieldmap {
|
||
|
|
public:
|
||
|
|
Fieldmap() = default;
|
||
|
|
explicit Fieldmap(const wpi::json& json) {
|
||
|
|
double field_length_meters =
|
||
|
|
static_cast<double>(json.at("field").at("length"));
|
||
|
|
double field_width_meters =
|
||
|
|
static_cast<double>(json.at("field").at("width"));
|
||
|
|
for (const auto& tag : json.at("tags").items()) {
|
||
|
|
double tagXPos =
|
||
|
|
static_cast<double>(tag.value().at("pose").at("translation").at("x"));
|
||
|
|
double tagYPos =
|
||
|
|
static_cast<double>(tag.value().at("pose").at("translation").at("y"));
|
||
|
|
double tagZPos =
|
||
|
|
static_cast<double>(tag.value().at("pose").at("translation").at("z"));
|
||
|
|
double tagWQuat = static_cast<double>(
|
||
|
|
tag.value().at("pose").at("rotation").at("quaternion").at("W"));
|
||
|
|
double tagXQuat = static_cast<double>(
|
||
|
|
tag.value().at("pose").at("rotation").at("quaternion").at("X"));
|
||
|
|
double tagYQuat = static_cast<double>(
|
||
|
|
tag.value().at("pose").at("rotation").at("quaternion").at("Y"));
|
||
|
|
double tagZQuat = static_cast<double>(
|
||
|
|
tag.value().at("pose").at("rotation").at("quaternion").at("Z"));
|
||
|
|
|
||
|
|
tagVec.emplace_back(tagXPos, tagYPos, tagZPos, tagWQuat, tagXQuat,
|
||
|
|
tagYQuat, tagZQuat, field_length_meters,
|
||
|
|
field_width_meters);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
const tag::Pose& getTag(size_t tag) const { return tagVec[tag - 1]; }
|
||
|
|
|
||
|
|
int getNumTags() const { return tagVec.size(); }
|
||
|
|
|
||
|
|
static double minimizeAngle(double angle) {
|
||
|
|
angle = std::fmod(angle, 360);
|
||
|
|
if (angle > 180) {
|
||
|
|
return angle - 360;
|
||
|
|
} else if (angle < -180) {
|
||
|
|
return angle + 360;
|
||
|
|
}
|
||
|
|
return angle;
|
||
|
|
}
|
||
|
|
|
||
|
|
private:
|
||
|
|
std::vector<tag::Pose> tagVec;
|
||
|
|
};
|