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 c901ad65b4..34740a40e7 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp @@ -19,12 +19,12 @@ namespace photon { PhotonPipelineResult::PhotonPipelineResult( - units::second_t latency, std::span targets) + units::millisecond_t latency, std::span targets) : latency(latency), targets(targets.data(), targets.data() + targets.size()) {} PhotonPipelineResult::PhotonPipelineResult( - units::second_t latency, std::span targets, + units::millisecond_t latency, std::span targets, MultiTargetPNPResult multitagResult) : latency(latency), targets(targets.data(), targets.data() + targets.size()), @@ -37,7 +37,7 @@ bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const { Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { // Encode latency and number of targets. - packet << result.latency.value() * 1000 << result.multitagResult + packet << result.latency.value() << result.multitagResult << static_cast(result.targets.size()); // Encode the information of each target. @@ -52,7 +52,7 @@ Packet& operator>>(Packet& packet, PhotonPipelineResult& result) { double latencyMillis = 0; int8_t targetCount = 0; packet >> latencyMillis >> result.multitagResult >> targetCount; - result.latency = units::second_t(latencyMillis / 1000.0); + result.latency = units::millisecond_t(latencyMillis); result.targets.clear(); diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp index 922d66f554..4289dbf3c7 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/proto/MultiTargetPNPResultProto.cpp @@ -18,6 +18,7 @@ #include "photon/targeting/proto/MultiTargetPNPResultProto.h" #include "photon.pb.h" +#include "photon/targeting/proto/PNPResultProto.h" google::protobuf::Message* wpi::Protobuf::New( google::protobuf::Arena* arena) { diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PNPResultProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PNPResultProto.cpp index b51e88ffc7..464bd7c479 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PNPResultProto.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PNPResultProto.cpp @@ -33,10 +33,12 @@ photon::PNPResult wpi::Protobuf::Unpack( return photon::PNPResult(); } - return photon::PNPResult{wpi::UnpackProtobuf(m->best()), + return photon::PNPResult{true, + wpi::UnpackProtobuf(m->best()), m->best_reproj_err(), wpi::UnpackProtobuf(m->alt()), - m->alt_reproj_err(), m->ambiguity()}; + m->alt_reproj_err(), + m->ambiguity()}; } void wpi::Protobuf::Pack(google::protobuf::Message* msg, diff --git a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp index c48f1f316d..3605ae5946 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/proto/PhotonPipelineResultProto.cpp @@ -18,6 +18,8 @@ #include "photon/targeting/proto/PhotonPipelineResultProto.h" #include "photon.pb.h" +#include "photon/targeting/proto/MultiTargetPNPResultProto.h" +#include "photon/targeting/proto/PhotonTrackedTargetProto.h" google::protobuf::Message* wpi::Protobuf::New( google::protobuf::Arena* arena) { diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h index 16aa6d5f6e..c0965fbd9c 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -44,7 +44,7 @@ class PhotonPipelineResult { * @param latency The latency in the pipeline. * @param targets The list of targets identified by the pipeline. */ - PhotonPipelineResult(units::second_t latency, + PhotonPipelineResult(units::millisecond_t latency, std::span targets); /** @@ -53,7 +53,7 @@ class PhotonPipelineResult { * @param targets The list of targets identified by the pipeline. * @param multitagResult The multitarget result */ - PhotonPipelineResult(units::second_t latency, + PhotonPipelineResult(units::millisecond_t latency, std::span targets, MultiTargetPNPResult multitagResult); @@ -81,7 +81,7 @@ class PhotonPipelineResult { * Returns the latency in the pipeline. * @return The latency in the pipeline. */ - units::second_t GetLatency() const { return latency; } + units::millisecond_t GetLatency() const { return latency; } /** * Returns the estimated time the frame was taken, @@ -125,7 +125,7 @@ class PhotonPipelineResult { friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result); friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result); - units::second_t latency = 0_s; + units::millisecond_t latency = 0_s; units::second_t timestamp = -1_s; wpi::SmallVector targets; MultiTargetPNPResult multitagResult; diff --git a/photon-targeting/src/test/native/cpp/targeting/proto/MultiTargetPNPResultProtoTest.cpp b/photon-targeting/src/test/native/cpp/targeting/proto/MultiTargetPNPResultProtoTest.cpp index 77caa1304f..df630ec5c5 100644 --- a/photon-targeting/src/test/native/cpp/targeting/proto/MultiTargetPNPResultProtoTest.cpp +++ b/photon-targeting/src/test/native/cpp/targeting/proto/MultiTargetPNPResultProtoTest.cpp @@ -18,6 +18,7 @@ #include "gtest/gtest.h" #include "photon.pb.h" #include "photon/targeting/MultiTargetPNPResult.h" +#include "photon/targeting/proto/MultiTargetPNPResultProto.h" TEST(MultiTargetPNPResultTest, Roundtrip) { photon::MultiTargetPNPResult result; @@ -33,12 +34,14 @@ TEST(MultiTargetPNPResultTest, Roundtrip) { EXPECT_EQ(result, unpacked_data); photon::PNPResult pnpRes{ + true, frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), frc::Rotation3d(1_rad, 2_rad, 3_rad)), 0.1, frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), frc::Rotation3d(1_rad, 2_rad, 3_rad)), - 0.1, 0}; + 0.1, + 0}; photon::MultiTargetPNPResult result1{pnpRes, {1, 2, 3, 4}}; diff --git a/photon-targeting/src/test/native/cpp/targeting/proto/PNPResultProtoTest.cpp b/photon-targeting/src/test/native/cpp/targeting/proto/PNPResultProtoTest.cpp index 33ddc7a712..2b6cb2fcc0 100644 --- a/photon-targeting/src/test/native/cpp/targeting/proto/PNPResultProtoTest.cpp +++ b/photon-targeting/src/test/native/cpp/targeting/proto/PNPResultProtoTest.cpp @@ -18,6 +18,7 @@ #include "gtest/gtest.h" #include "photon.pb.h" #include "photon/targeting/PNPResult.h" +#include "photon/targeting/proto/PNPResultProto.h" TEST(PNPResultTest, Roundtrip) { photon::PNPResult result; @@ -33,12 +34,14 @@ TEST(PNPResultTest, Roundtrip) { EXPECT_EQ(result, unpacked_data); photon::PNPResult result1{ + true, frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), frc::Rotation3d(1_rad, 2_rad, 3_rad)), 0.1, frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), frc::Rotation3d(1_rad, 2_rad, 3_rad)), - 0.1, 0}; + 0.1, + 0}; proto = wpi::Protobuf::New(&arena); wpi::Protobuf::Pack(proto, result1); diff --git a/photon-targeting/src/test/native/cpp/targeting/proto/PhotonPipelineResultProtoTest.cpp b/photon-targeting/src/test/native/cpp/targeting/proto/PhotonPipelineResultProtoTest.cpp index 4a400091bf..65c60f0d13 100644 --- a/photon-targeting/src/test/native/cpp/targeting/proto/PhotonPipelineResultProtoTest.cpp +++ b/photon-targeting/src/test/native/cpp/targeting/proto/PhotonPipelineResultProtoTest.cpp @@ -18,6 +18,7 @@ #include "gtest/gtest.h" #include "photon.pb.h" #include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/proto/PhotonPipelineResultProto.h" TEST(PhotonPipelineResultTest, Roundtrip) { photon::PhotonPipelineResult result{12_ms, {}}; @@ -72,12 +73,14 @@ TEST(PhotonPipelineResultTest, Roundtrip) { EXPECT_EQ(result2, unpacked_data2); photon::PNPResult pnpRes{ + true, frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), frc::Rotation3d(1_rad, 2_rad, 3_rad)), 0.1, frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), frc::Rotation3d(1_rad, 2_rad, 3_rad)), - 0.1, 0}; + 0.1, + 0}; photon::MultiTargetPNPResult multitagRes{pnpRes, {1, 2, 3, 4}}; diff --git a/photon-targeting/src/test/native/cpp/targeting/proto/PhotonTrackedTargetProtoTest.cpp b/photon-targeting/src/test/native/cpp/targeting/proto/PhotonTrackedTargetProtoTest.cpp index 39c28773a3..73ef41f77c 100644 --- a/photon-targeting/src/test/native/cpp/targeting/proto/PhotonTrackedTargetProtoTest.cpp +++ b/photon-targeting/src/test/native/cpp/targeting/proto/PhotonTrackedTargetProtoTest.cpp @@ -18,6 +18,7 @@ #include "gtest/gtest.h" #include "photon.pb.h" #include "photon/targeting/PhotonTrackedTarget.h" +#include "photon/targeting/proto/PhotonTrackedTargetProto.h" TEST(PhotonTrackedTargetTest, Roundtrip) { photon::PhotonTrackedTarget target{