From 81c911122fbf1168ff7d932609bfec2471b6f484 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sat, 18 Nov 2023 01:42:23 -0500 Subject: [PATCH] think i added packet --- photon-targeting/build.gradle | 4 +- .../photon/targeting/MultiTargetPNPResult.cpp | 31 +++++++ .../native/cpp/photon/targeting/PNPResult.cpp | 51 ++++++++++++ .../photon/targeting/PhotonPipelineResult.cpp | 30 +++++++ .../photon/targeting/PhotonTrackedTarget.cpp | 83 +++++++++++++++++++ .../src/test/native/cpp/PacketTest.cpp | 1 - 6 files changed, 196 insertions(+), 4 deletions(-) diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index ac19365651..08bf6099d7 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -17,12 +17,10 @@ model { cpp { source { srcDirs 'src/main/native/cpp' - srcDirs "build/generated/source/proto/main/cpp" - include '**/*.cpp', "**/*.cc" + include '**/*.cpp' } exportedHeaders { srcDirs 'src/main/native/include' - srcDirs "build/generated/source/proto/main/cpp" include "**/*.h" } } diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp index b894272b11..6b8b97c4c4 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp @@ -26,3 +26,34 @@ photon::MultiTargetPNPResult::MultiTargetPNPResult( bool MultiTargetPNPResult::operator==(const MultiTargetPNPResult& other) const { return other.result == result && other.fiducialIdsUsed == fiducialIdsUsed; } + +Packet& operator<<(Packet& packet, const MultiTargetPNPResult& target) { + packet << target.result; + + size_t i; + for (i = 0; i < target.fiducialIdsUsed.capacity(); i++) { + if (i < target.fiducialIdsUsed.size()) { + packet << static_cast(target.fiducialIdsUsed[i]); + } else { + packet << static_cast(-1); + } + } + + return packet; +} + +Packet& operator>>(Packet& packet, MultiTargetPNPResult& target) { + packet >> target.result; + + target.fiducialIdsUsed.clear(); + for (size_t i = 0; i < target.fiducialIdsUsed.capacity(); i++) { + int16_t id = 0; + packet >> id; + + if (id > -1) { + target.fiducialIdsUsed.push_back(id); + } + } + + return packet; +} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp index f604c0465d..8d56ec612b 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp @@ -35,3 +35,54 @@ bool PNPResult::operator==(const PNPResult& other) const { other.altReprojErr == 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; +} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp index 989887e41a..0dcc6ca435 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp @@ -37,3 +37,33 @@ bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const { return latency == other.latency && targets == other.targets && multitagResult == other.multitagResult; } + +Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { + // Encode latency and number of targets. + packet << result.latency.value() * 1000 << result.multitagResult + << static_cast(result.targets.size()); + + // Encode the information of each target. + for (auto& target : result.targets) packet << target; + + // Return the packet + return packet; +} + +Packet& operator>>(Packet& packet, PhotonPipelineResult& result) { + // Decode latency, existence of targets, and number of targets. + double latencyMillis = 0; + int8_t targetCount = 0; + packet >> latencyMillis >> result.multitagResult >> targetCount; + result.latency = units::second_t(latencyMillis / 1000.0); + + result.targets.clear(); + + // Decode the information of each target. + for (int i = 0; i < targetCount; ++i) { + PhotonTrackedTarget target; + packet >> target; + result.targets.push_back(target); + } + return packet; +} diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp index d1939fb2f5..c0dd341eac 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp @@ -49,3 +49,86 @@ bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const { other.skew == skew && other.bestCameraToTarget == bestCameraToTarget && other.minAreaRectCorners == minAreaRectCorners; } + +Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target) { + packet << target.yaw << target.pitch << target.area << target.skew + << target.fiducialId + << 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(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; + + // 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; +} diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp index 85c56f5216..8a4923fbda 100644 --- a/photon-targeting/src/test/native/cpp/PacketTest.cpp +++ b/photon-targeting/src/test/native/cpp/PacketTest.cpp @@ -20,7 +20,6 @@ #include #include "gtest/gtest.h" - #include "photon/dataflow/structures/Packet.h" #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PNPResult.h"