Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-27 02:01:40 +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

38
photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp
View File

@@ -17,40 +17,4 @@
#include "photon/targeting/MultiTargetPNPResult.h"
namespace photon {
bool MultiTargetPNPResult::operator==(const MultiTargetPNPResult& other) const {
return other.result == result && other.fiducialIdsUsed == fiducialIdsUsed;
}
Packet& operator<<(Packet& packet, const MultiTargetPNPResult& result) {
packet << result.result;
size_t i;
for (i = 0; i < result.fiducialIdsUsed.capacity(); i++) {
if (i < result.fiducialIdsUsed.size()) {
packet << static_cast<int16_t>(result.fiducialIdsUsed[i]);
} else {
packet << static_cast<int16_t>(-1);
}
}
return packet;
}
Packet& operator>>(Packet& packet, MultiTargetPNPResult& result) {
packet >> result.result;
result.fiducialIdsUsed.clear();
for (size_t i = 0; i < result.fiducialIdsUsed.capacity(); i++) {
int16_t id = 0;
packet >> id;
if (id > -1) {
result.fiducialIdsUsed.push_back(id);
}
}
return packet;
}
} // namespace photon
namespace photon {} // namespace photon

77
photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp
View File

@@ -1,77 +0,0 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include "photon/targeting/PNPResult.h"
namespace photon {
bool PNPResult::operator==(const PNPResult& other) const {
return other.isPresent == isPresent && other.best == best &&
other.bestReprojErr == bestReprojErr && other.alt == alt &&
other.altReprojErr == altReprojErr && other.ambiguity == ambiguity;
}
// Encode a transform3d
Packet& operator<<(Packet& packet, const frc::Transform3d& transform) {
packet << transform.Translation().X().value()
<< transform.Translation().Y().value()
<< transform.Translation().Z().value()
<< transform.Rotation().GetQuaternion().W()
<< transform.Rotation().GetQuaternion().X()
<< transform.Rotation().GetQuaternion().Y()
<< transform.Rotation().GetQuaternion().Z();
return packet;
}
// Decode a transform3d
Packet& operator>>(Packet& packet, frc::Transform3d& transform) {
frc::Transform3d ret;
// We use these for best and alt transforms below
double x = 0;
double y = 0;
double z = 0;
double w = 0;
// decode and unitify translation
packet >> x >> y >> z;
const auto translation = frc::Translation3d(
units::meter_t(x), units::meter_t(y), units::meter_t(z));
// decode and add units to rotation
packet >> w >> x >> y >> z;
const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z));
transform = frc::Transform3d(translation, rotation);
return packet;
}
Packet& operator<<(Packet& packet, PNPResult const& result) {
packet << result.isPresent << result.best << result.alt
<< result.bestReprojErr << result.altReprojErr << result.ambiguity;
return packet;
}
Packet& operator>>(Packet& packet, PNPResult& result) {
packet >> result.isPresent >> result.best >> result.alt >>
result.bestReprojErr >> result.altReprojErr >> result.ambiguity;
return packet;
}
} // namespace photon

72
photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp
View File

@@ -17,74 +17,4 @@
#include "photon/targeting/PhotonPipelineResult.h"
#include <vector>
namespace photon {
PhotonPipelineResult::PhotonPipelineResult(
int64_t sequenceID, units::microsecond_t captureTimestamp,
units::microsecond_t publishTimestamp,
std::span<const PhotonTrackedTarget> targets,
MultiTargetPNPResult multitagResult)
: sequenceID(sequenceID),
captureTimestamp(captureTimestamp),
publishTimestamp(publishTimestamp),
targets(targets.data(), targets.data() + targets.size()),
multitagResult(multitagResult) {}
bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const {
return sequenceID == other.sequenceID &&
captureTimestamp == other.captureTimestamp &&
publishTimestamp == other.publishTimestamp &&
ntRecieveTimestamp == other.ntRecieveTimestamp &&
targets == other.targets && multitagResult == other.multitagResult;
}
Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) {
// Encode latency and number of targets.
packet << result.sequenceID
<< static_cast<int64_t>(result.captureTimestamp.value())
<< static_cast<int64_t>(result.publishTimestamp.value())
<< static_cast<int8_t>(result.targets.size());
// Encode the information of each target.
for (auto& target : result.targets) packet << target;
packet << result.multitagResult;
// Return the packet
return packet;
}
Packet& operator>>(Packet& packet, PhotonPipelineResult& result) {
// Decode latency, existence of targets, and number of targets.
int64_t sequenceID = 0;
int64_t capTS = 0;
int64_t pubTS = 0;
int8_t targetCount = 0;
std::vector<PhotonTrackedTarget> targets;
MultiTargetPNPResult multitagResult;
packet >> sequenceID >> capTS >> pubTS >> targetCount;
targets.clear();
targets.reserve(targetCount);
// Decode the information of each target.
for (int i = 0; i < targetCount; ++i) {
PhotonTrackedTarget target;
packet >> target;
targets.push_back(target);
}
packet >> multitagResult;
units::microsecond_t captureTS =
units::microsecond_t{static_cast<double>(capTS)};
units::microsecond_t publishTS =
units::microsecond_t{static_cast<double>(pubTS)};
result = PhotonPipelineResult{sequenceID, captureTS, publishTS, targets,
multitagResult};
return packet;
}
} // namespace photon
namespace photon {} // namespace photon

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

20
photon-targeting/src/main/native/cpp/photon/targeting/PnpResult.cpp Normal file
View File

@@ -0,0 +1,20 @@
/*
* Copyright (C) Photon Vision.
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*/
#include "photon/targeting/PnpResult.h"
namespace photon {} // namespace photon

14
photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp
View File

@@ -33,24 +33,26 @@ wpi::Protobuf<photon::MultiTargetPNPResult>::Unpack(
static_cast<const photonvision::proto::ProtobufMultiTargetPNPResult*>(
&msg);
wpi::SmallVector<int16_t, 32> fiducialIdsUsed;
std::vector<int16_t> fiducialIdsUsed;
fiducialIdsUsed.reserve(32);
for (int i = 0; i < m->fiducial_ids_used_size(); i++) {
fiducialIdsUsed.push_back(m->fiducial_ids_used(i));
}
return photon::MultiTargetPNPResult{
wpi::UnpackProtobuf<photon::PNPResult>(m->estimated_pose()),
fiducialIdsUsed};
return photon::MultiTargetPNPResult{photon::MultiTargetPNPResult_PhotonStruct{
wpi::UnpackProtobuf<photon::PnpResult>(m->estimated_pose()),
fiducialIdsUsed}};
}
void wpi::Protobuf<photon::MultiTargetPNPResult>::Pack(
google::protobuf::Message* msg, const photon::MultiTargetPNPResult& value) {
auto m = static_cast<photonvision::proto::ProtobufMultiTargetPNPResult*>(msg);
wpi::PackProtobuf(m->mutable_estimated_pose(), value.result);
wpi::PackProtobuf(m->mutable_estimated_pose(), value.estimatedPose);
m->clear_fiducial_ids_used();
for (const auto& t : value.fiducialIdsUsed) {
for (const auto& t : value.fiducialIDsUsed) {
m->add_fiducial_ids_used(t);
}
}

24
photon-targeting/src/main/native/cpp/photon/targeting/proto/PNPResultProto.cpp
View File

@@ -19,33 +19,27 @@
#include "photon.pb.h"
google::protobuf::Message* wpi::Protobuf<photon::PNPResult>::New(
google::protobuf::Message* wpi::Protobuf<photon::PnpResult>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<
photonvision::proto::ProtobufPNPResult>(arena);
}
photon::PNPResult wpi::Protobuf<photon::PNPResult>::Unpack(
photon::PnpResult wpi::Protobuf<photon::PnpResult>::Unpack(
const google::protobuf::Message& msg) {
auto m = static_cast<const photonvision::proto::ProtobufPNPResult*>(&msg);
if (!m->is_present()) {
return photon::PNPResult();
}
return photon::PNPResult{true,
wpi::UnpackProtobuf<frc::Transform3d>(m->best()),
m->best_reproj_err(),
wpi::UnpackProtobuf<frc::Transform3d>(m->alt()),
m->alt_reproj_err(),
m->ambiguity()};
return photon::PnpResult{photon::PnpResult_PhotonStruct{
wpi::UnpackProtobuf<frc::Transform3d>(m->best()),
wpi::UnpackProtobuf<frc::Transform3d>(m->alt()), m->best_reproj_err(),
m->alt_reproj_err(), m->ambiguity()}};
}
void wpi::Protobuf<photon::PNPResult>::Pack(google::protobuf::Message* msg,
const photon::PNPResult& value) {
void wpi::Protobuf<photon::PnpResult>::Pack(google::protobuf::Message* msg,
const photon::PnpResult& value) {
auto m = static_cast<photonvision::proto::ProtobufPNPResult*>(msg);
m->set_is_present(value.isPresent);
// m->set_is_present(value.isPresent);
wpi::PackProtobuf(m->mutable_best(), value.best);
m->set_best_reproj_err(value.bestReprojErr);
wpi::PackProtobuf(m->mutable_alt(), value.alt);

33
photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp
View File

@@ -42,28 +42,39 @@ wpi::Protobuf<photon::PhotonPipelineResult>::Unpack(
targets.emplace_back(wpi::UnpackProtobuf<photon::PhotonTrackedTarget>(t));
}
return photon::PhotonPipelineResult{
m->sequence_id(),
units::microsecond_t{static_cast<double>(m->capture_timestamp_micros())},
units::microsecond_t{
static_cast<double>(m->nt_publish_timestamp_micros())},
return photon::PhotonPipelineResult{photon::PhotonPipelineResult_PhotonStruct{
photon::PhotonPipelineMetadata{
photon::PhotonPipelineMetadata_PhotonStruct{
m->sequence_id(),
m->capture_timestamp_micros(),
m->nt_publish_timestamp_micros(),
}},
targets,
wpi::UnpackProtobuf<photon::MultiTargetPNPResult>(
m->multi_target_result())};
// TODO need to pull this into an optional
m->has_multi_target_result()
? std::optional<photon::MultiTargetPNPResult>{wpi::UnpackProtobuf<
photon::MultiTargetPNPResult>(m->multi_target_result())}
: std::nullopt,
}};
}
void wpi::Protobuf<photon::PhotonPipelineResult>::Pack(
google::protobuf::Message* msg, const photon::PhotonPipelineResult& value) {
auto m = static_cast<photonvision::proto::ProtobufPhotonPipelineResult*>(msg);
m->set_sequence_id(value.sequenceID);
m->set_capture_timestamp_micros(value.captureTimestamp.value());
m->set_nt_publish_timestamp_micros(value.publishTimestamp.value());
m->set_sequence_id(value.metadata.sequenceID);
m->set_capture_timestamp_micros(value.metadata.captureTimestampMicros);
m->set_nt_publish_timestamp_micros(value.metadata.publishTimestampMicros);
m->clear_targets();
for (const auto& t : value.GetTargets()) {
wpi::PackProtobuf(m->add_targets(), t);
}
wpi::PackProtobuf(m->mutable_multi_target_result(), value.multitagResult);
// TODO this is dumb and bad
if (value.multitagResult) {
wpi::PackProtobuf(m->mutable_multi_target_result(), *value.multitagResult);
} else {
m->clear_multi_target_result();
}
}

37
photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonTrackedTargetProto.cpp
View File

@@ -22,6 +22,9 @@
#include "photon.pb.h"
using photon::TargetCorner;
using photon::TargetCorner_PhotonStruct;
google::protobuf::Message* wpi::Protobuf<photon::PhotonTrackedTarget>::New(
google::protobuf::Arena* arena) {
return google::protobuf::Arena::CreateMessage<
@@ -33,30 +36,26 @@ photon::PhotonTrackedTarget wpi::Protobuf<photon::PhotonTrackedTarget>::Unpack(
auto m = static_cast<const photonvision::proto::ProtobufPhotonTrackedTarget*>(
&msg);
wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners;
std::vector<photon::TargetCorner> minAreaRectCorners;
minAreaRectCorners.reserve(4);
for (const auto& t : m->min_area_rect_corners()) {
minAreaRectCorners.emplace_back(t.x(), t.y());
minAreaRectCorners.emplace_back(
TargetCorner{TargetCorner_PhotonStruct{t.x(), t.y()}});
}
std::vector<std::pair<double, double>> detectedCorners;
std::vector<photon::TargetCorner> detectedCorners;
detectedCorners.reserve(m->detected_corners_size());
for (const auto& t : m->detected_corners()) {
detectedCorners.emplace_back(t.x(), t.y());
minAreaRectCorners.emplace_back(
TargetCorner{TargetCorner_PhotonStruct{t.x(), t.y()}});
}
return photon::PhotonTrackedTarget{
m->yaw(),
m->pitch(),
m->area(),
m->skew(),
m->fiducial_id(),
m->obj_detection_id(),
m->obj_detection_conf(),
return photon::PhotonTrackedTarget{photon::PhotonTrackedTarget_PhotonStruct{
m->yaw(), m->pitch(), m->area(), m->skew(), m->fiducial_id(),
m->obj_detection_id(), m->obj_detection_conf(),
wpi::UnpackProtobuf<frc::Transform3d>(m->best_camera_to_target()),
wpi::UnpackProtobuf<frc::Transform3d>(m->alt_camera_to_target()),
m->pose_ambiguity(),
minAreaRectCorners,
detectedCorners};
m->pose_ambiguity(), minAreaRectCorners, detectedCorners}};
}
void wpi::Protobuf<photon::PhotonTrackedTarget>::Pack(
@@ -78,14 +77,14 @@ void wpi::Protobuf<photon::PhotonTrackedTarget>::Pack(
m->clear_min_area_rect_corners();
for (const auto& t : value.GetMinAreaRectCorners()) {
auto* corner = m->add_min_area_rect_corners();
corner->set_x(t.first);
corner->set_y(t.second);
corner->set_x(t.x);
corner->set_y(t.y);
}
m->clear_detected_corners();
for (const auto& t : value.GetDetectedCorners()) {
auto* corner = m->add_detected_corners();
corner->set_x(t.first);
corner->set_y(t.second);
corner->set_x(t.x);
corner->set_y(t.y);
}
}