Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-05 03:21:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpical] Add JSON Combiner (#7640)

Browse Source 
This new feature allows users to combine multiple Apriltag layouts. This can be useful for fields where the apriltags are split into two or more sections: (red/blue side, grouped together by task, etc.)
This commit is contained in:
Elliot Scher
2025-01-10 02:30:17 -05:00
committed by GitHub
parent e37c35746a
commit 6125227836
7 changed files with 373 additions and 186 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
wpical/src/main/native/include/fieldmap.h
View File

@@ -5,7 +5,7 @@
#pragma once
#include <cmath>
#include <vector>
#include <map>
#include <tagpose.h>
#include <wpi/json.h>
@@ -19,6 +19,7 @@ class Fieldmap {
double field_width_meters =
static_cast<double>(json.at("field").at("width"));
for (const auto& tag : json.at("tags").items()) {
double tag_id = static_cast<int>(tag.value().at("ID"));
double tagXPos =
static_cast<double>(tag.value().at("pose").at("translation").at("x"));
double tagYPos =
@@ -34,15 +35,30 @@ class Fieldmap {
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);
tagMap.emplace(
tag_id, tag::Pose(tag_id, tagXPos, tagYPos, tagZPos, tagWQuat,
tagXQuat, tagYQuat, tagZQuat, field_length_meters,
field_width_meters));
}
fieldLength = field_length_meters;
fieldWidth = field_width_meters;
}
const tag::Pose& getTag(size_t tag) const { return tagVec[tag - 1]; }
const tag::Pose& getTag(size_t tag) const { return tagMap.at(tag); }
int getNumTags() const { return tagVec.size(); }
int getNumTags() const { return tagMap.size(); }
bool hasTag(int tag) { return tagMap.find(tag) != tagMap.end(); }
wpi::json toJson() {
wpi::json json;
for (auto& [key, val] : tagMap) {
json["tags"].push_back(val.toJson());
}
json["field"]["length"] = fieldLength;
json["field"]["width"] = fieldWidth;
return json;
}
static double minimizeAngle(double angle) {
angle = std::fmod(angle, 360);
@@ -54,6 +70,13 @@ class Fieldmap {
return angle;
}
void replaceTag(int tag_id, tag::Pose pose) {
tagMap.erase(tag_id);
tagMap.emplace(tag_id, pose);
}
private:
std::vector<tag::Pose> tagVec;
double fieldLength;
double fieldWidth;
std::map<int, tag::Pose> tagMap;
};