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

[apriltag] Split AprilTag functionality to a separate library (#4578)

Browse Source 
Add AprilTagFieldLayout JSON file and move class to edu.wpi.first.apriltag.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
PJ Reiniger
2022-11-17 17:29:29 -05:00
committed by GitHub
parent 2e88a496c2
commit 53904e7cf4
23 changed files with 887 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
wpilibc/src/main/native/cpp/apriltag/AprilTag.cpp
View File

@@ -1,26 +0,0 @@
// 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 "frc/apriltag/AprilTag.h"
#include <wpi/json.h>
using namespace frc;
bool AprilTag::operator==(const AprilTag& other) const {
return ID == other.ID && pose == other.pose;
}
bool AprilTag::operator!=(const AprilTag& other) const {
return !operator==(other);
}
void frc::to_json(wpi::json& json, const AprilTag& apriltag) {
json = wpi::json{{"ID", apriltag.ID}, {"pose", apriltag.pose}};
}
void frc::from_json(const wpi::json& json, AprilTag& apriltag) {
apriltag.ID = json.at("ID").get<int>();
apriltag.pose = json.at("pose").get<Pose3d>();
}

108
wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp
View File

@@ -1,108 +0,0 @@
// 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 "frc/apriltag/AprilTagFieldLayout.h"
#include <system_error>
#include <units/angle.h>
#include <units/length.h>
#include <wpi/json.h>
#include <wpi/raw_istream.h>
#include <wpi/raw_ostream.h>
using namespace frc;
AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) {
std::error_code error_code;
wpi::raw_fd_istream input{path, error_code};
if (error_code) {
throw std::runtime_error(fmt::format("Cannot open file: {}", path));
}
wpi::json json;
input >> json;
for (const auto& tag : json.at("tags").get<std::vector<AprilTag>>()) {
m_apriltags[tag.ID] = tag;
}
m_fieldWidth = units::meter_t{json.at("field").at("width").get<double>()};
m_fieldLength = units::meter_t{json.at("field").at("height").get<double>()};
}
AprilTagFieldLayout::AprilTagFieldLayout(std::vector<AprilTag> apriltags,
units::meter_t fieldLength,
units::meter_t fieldWidth)
: m_fieldLength(std::move(fieldLength)),
m_fieldWidth(std::move(fieldWidth)) {
for (const auto& tag : apriltags) {
m_apriltags[tag.ID] = tag;
}
}
void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) {
m_mirror = alliance == DriverStation::Alliance::kRed;
}
std::optional<frc::Pose3d> AprilTagFieldLayout::GetTagPose(int ID) const {
const auto& it = m_apriltags.find(ID);
if (it == m_apriltags.end()) {
return std::nullopt;
}
Pose3d returnPose = it->second.pose;
if (m_mirror) {
returnPose = returnPose.RelativeTo(Pose3d{
m_fieldLength, m_fieldWidth, 0_m, Rotation3d{0_deg, 0_deg, 180_deg}});
}
return returnPose;
}
void AprilTagFieldLayout::Serialize(std::string_view path) {
std::error_code error_code;
wpi::raw_fd_ostream output{path, error_code};
if (error_code) {
throw std::runtime_error(fmt::format("Cannot open file: {}", path));
}
wpi::json json = *this;
output << json;
output.flush();
}
bool AprilTagFieldLayout::operator==(const AprilTagFieldLayout& other) const {
return m_apriltags == other.m_apriltags && m_mirror == other.m_mirror &&
m_fieldLength == other.m_fieldLength &&
m_fieldWidth == other.m_fieldWidth;
}
bool AprilTagFieldLayout::operator!=(const AprilTagFieldLayout& other) const {
return !operator==(other);
}
void frc::to_json(wpi::json& json, const AprilTagFieldLayout& layout) {
std::vector<AprilTag> tagVector;
tagVector.reserve(layout.m_apriltags.size());
for (const auto& pair : layout.m_apriltags) {
tagVector.push_back(pair.second);
}
json = wpi::json{{"field",
{{"length", layout.m_fieldLength.value()},
{"width", layout.m_fieldWidth.value()}}},
{"tags", tagVector}};
}
void frc::from_json(const wpi::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;
}
layout.m_fieldLength =
units::meter_t{json.at("field").at("length").get<double>()};
layout.m_fieldWidth =
units::meter_t{json.at("field").at("width").get<double>()};
}

45
wpilibc/src/main/native/include/frc/apriltag/AprilTag.h
View File

@@ -1,45 +0,0 @@
// 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 <wpi/SymbolExports.h>
#include "frc/geometry/Pose3d.h"
namespace wpi {
class json;
} // namespace wpi
namespace frc {
struct WPILIB_DLLEXPORT AprilTag {
int ID;
Pose3d pose;
/**
* Checks equality between this AprilTag and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const AprilTag& other) const;
/**
* Checks inequality between this AprilTag and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const AprilTag& other) const;
};
WPILIB_DLLEXPORT
void to_json(wpi::json& json, const AprilTag& apriltag);
WPILIB_DLLEXPORT
void from_json(const wpi::json& json, AprilTag& apriltag);
} // namespace frc

123
wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h
View File

@@ -1,123 +0,0 @@
// 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 <optional>
#include <string_view>
#include <unordered_map>
#include <vector>
#include <units/length.h>
#include <wpi/SymbolExports.h>
#include "frc/DriverStation.h"
#include "frc/apriltag/AprilTag.h"
#include "frc/geometry/Pose3d.h"
namespace wpi {
class json;
} // namespace wpi
namespace frc {
/**
* Class for representing a layout of AprilTags on a field and reading them from
* a JSON format.
*
* 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 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 mirroring the poses.
*
* Pose3ds are assumed to be measured from the bottom-left corner of the field,
* when the blue alliance is at the left. Pose3ds will automatically be returned
* as passed in when calling GetTagPose(int). Setting an alliance color via
* SetAlliance(DriverStation::Alliance) will mirror the poses returned from
* GetTagPose(int) to be correct relative to the other alliance.
*/
class WPILIB_DLLEXPORT AprilTagFieldLayout {
public:
AprilTagFieldLayout() = default;
/**
* Construct a new AprilTagFieldLayout with values imported from a JSON file.
*
* @param path Path of the JSON file to import from.
*/
explicit AprilTagFieldLayout(std::string_view path);
/**
* Construct a new AprilTagFieldLayout from a vector of AprilTag objects.
*
* @param apriltags Vector of AprilTags.
* @param fieldLength Length of field the layout of representing.
* @param fieldWidth Width of field the layout is representing.
*/
AprilTagFieldLayout(std::vector<AprilTag> apriltags,
units::meter_t fieldLength, units::meter_t fieldWidth);
/**
* Set the alliance that your team is on.
*
* This changes the GetTagPose(int) method to return the correct pose for your
* alliance.
*
* @param alliance The alliance to mirror poses for.
*/
void SetAlliance(DriverStation::Alliance alliance);
/**
* Gets an AprilTag pose by its ID.
*
* @param ID The ID of the tag.
* @return The pose corresponding to the ID that was passed in or an empty
* optional if a tag with that ID is not found.
*/
std::optional<Pose3d> GetTagPose(int ID) const;
/**
* Serializes an AprilTagFieldLayout to a JSON file.
*
* @param path The path to write the JSON file to.
*/
void Serialize(std::string_view path);
/*
* Checks equality between this AprilTagFieldLayout and another object.
*
* @param other The other object.
* @return Whether the two objects are equal.
*/
bool operator==(const AprilTagFieldLayout& other) const;
/**
* Checks inequality between this AprilTagFieldLayout and another object.
*
* @param other The other object.
* @return Whether the two objects are not equal.
*/
bool operator!=(const AprilTagFieldLayout& other) const;
private:
std::unordered_map<int, AprilTag> m_apriltags;
units::meter_t m_fieldLength;
units::meter_t m_fieldWidth;
bool m_mirror = false;
friend WPILIB_DLLEXPORT void to_json(wpi::json& json,
const AprilTagFieldLayout& layout);
friend WPILIB_DLLEXPORT void from_json(const wpi::json& json,
AprilTagFieldLayout& layout);
};
WPILIB_DLLEXPORT
void to_json(wpi::json& json, const AprilTagFieldLayout& layout);
WPILIB_DLLEXPORT
void from_json(const wpi::json& json, AprilTagFieldLayout& layout);
} // namespace frc