Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-19 00:41:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Auto-generate packet dataclasses with Jinja (#1374)

Browse Source
This commit is contained in:
Matt
2024-08-31 13:44:19 -04:00
committed by GitHub
parent c19d54c633
commit 169595e56e
140 changed files with 4445 additions and 2097 deletions
Download Patch File Download Diff File Expand all files Collapse all files

113
photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp
View File

@@ -27,115 +27,4 @@
static constexpr const uint8_t MAX_CORNERS = 8;
namespace photon {
PhotonTrackedTarget::PhotonTrackedTarget(
double yaw, double pitch, double area, double skew, int id, int objdetid,
float objdetconf, const frc::Transform3d& pose,
const frc::Transform3d& alternatePose, double ambiguity,
const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners,
const std::vector<std::pair<double, double>> detectedCorners)
: yaw(yaw),
pitch(pitch),
area(area),
skew(skew),
fiducialId(id),
objDetectId(objdetid),
objDetectConf(objdetconf),
bestCameraToTarget(pose),
altCameraToTarget(alternatePose),
poseAmbiguity(ambiguity),
minAreaRectCorners(minAreaRectCorners),
detectedCorners(detectedCorners) {}
bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
return other.yaw == yaw && other.pitch == pitch && other.area == area &&
other.skew == skew && other.bestCameraToTarget == bestCameraToTarget &&
other.objDetectConf == objDetectConf &&
other.objDetectId == objDetectId &&
other.minAreaRectCorners == minAreaRectCorners;
}
Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) {
packet << target.yaw << target.pitch << target.area << target.skew
<< target.fiducialId << target.objDetectId << target.objDetectConf
<< target.bestCameraToTarget.Translation().X().value()
<< target.bestCameraToTarget.Translation().Y().value()
<< target.bestCameraToTarget.Translation().Z().value()
<< target.bestCameraToTarget.Rotation().GetQuaternion().W()
<< target.bestCameraToTarget.Rotation().GetQuaternion().X()
<< target.bestCameraToTarget.Rotation().GetQuaternion().Y()
<< target.bestCameraToTarget.Rotation().GetQuaternion().Z()
<< target.altCameraToTarget.Translation().X().value()
<< target.altCameraToTarget.Translation().Y().value()
<< target.altCameraToTarget.Translation().Z().value()
<< target.altCameraToTarget.Rotation().GetQuaternion().W()
<< target.altCameraToTarget.Rotation().GetQuaternion().X()
<< target.altCameraToTarget.Rotation().GetQuaternion().Y()
<< target.altCameraToTarget.Rotation().GetQuaternion().Z()
<< target.poseAmbiguity;
for (int i = 0; i < 4; i++) {
packet << target.minAreaRectCorners[i].first
<< target.minAreaRectCorners[i].second;
}
uint8_t num_corners =
std::min<uint8_t>(target.detectedCorners.size(), MAX_CORNERS);
packet << num_corners;
for (size_t i = 0; i < target.detectedCorners.size(); i++) {
packet << target.detectedCorners[i].first
<< target.detectedCorners[i].second;
}
return packet;
}
Packet& operator>>(Packet& packet, PhotonTrackedTarget& target) {
packet >> target.yaw >> target.pitch >> target.area >> target.skew >>
target.fiducialId >> target.objDetectId >> target.objDetectConf;
// We use these for best and alt transforms below
double x = 0;
double y = 0;
double z = 0;
double w = 0;
// First transform is the "best" pose
packet >> x >> y >> z;
const auto bestTranslation = frc::Translation3d(
units::meter_t(x), units::meter_t(y), units::meter_t(z));
packet >> w >> x >> y >> z;
const auto bestRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
target.bestCameraToTarget = frc::Transform3d(bestTranslation, bestRotation);
// Second transform is the "alternate" pose
packet >> x >> y >> z;
const auto altTranslation = frc::Translation3d(
units::meter_t(x), units::meter_t(y), units::meter_t(z));
packet >> w >> x >> y >> z;
const auto altRotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
target.altCameraToTarget = frc::Transform3d(altTranslation, altRotation);
packet >> target.poseAmbiguity;
target.minAreaRectCorners.clear();
double first = 0;
double second = 0;
for (int i = 0; i < 4; i++) {
packet >> first >> second;
target.minAreaRectCorners.emplace_back(first, second);
}
uint8_t numCorners = 0;
packet >> numCorners;
target.detectedCorners.clear();
target.detectedCorners.reserve(numCorners);
for (size_t i = 0; i < numCorners; i++) {
packet >> first >> second;
target.detectedCorners.emplace_back(first, second);
}
return packet;
}
} // namespace photon
namespace photon {} // namespace photon