diff --git a/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp b/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp deleted file mode 100644 index 91d6ffd6c3..0000000000 --- a/photon-lib/src/main/native/cpp/photonlib/MultiTargetPNPResult.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "photonlib/MultiTargetPNPResult.h" - -namespace photonlib { - -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; -} - -// 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, PNPResults const& result) { - packet << result.isValid << result.best << result.alt - << result.bestReprojectionErr << result.altReprojectionErr - << result.ambiguity; - - return packet; -} - -Packet& operator>>(Packet& packet, PNPResults& result) { - packet >> result.isValid >> result.best >> result.alt >> - result.bestReprojectionErr >> result.altReprojectionErr >> - result.ambiguity; - - return packet; -} - -} // namespace photonlib diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp index 4d45b6d47e..e1eb754d66 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonCamera.cpp @@ -29,7 +29,6 @@ #include #include "PhotonVersion.h" -#include "photonlib/Packet.h" namespace photonlib { @@ -82,9 +81,6 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { // Prints warning if not connected VerifyVersion(); - // Clear the current packet. - packet.Clear(); - // Create the new result; PhotonPipelineResult result = pipelineResultsSubscriber.Get(); diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp index 6efc660058..3fbe8fd52d 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPipelineResult.cpp @@ -40,36 +40,6 @@ bool PhotonPipelineResult::operator!=(const PhotonPipelineResult& other) const { return !operator==(other); } -Packet& operator<<(Packet& packet, const PhotonPipelineResult& result) { - // Encode latency and number of targets. - packet << result.latency.value() * 1000 << result.m_pnpResults - << 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.m_pnpResults >> 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; -} - } // namespace photonlib google::protobuf::Message* wpi::Protobuf::New( diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp index 25b1ea2dfd..720b8965b0 100644 --- a/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp +++ b/photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp @@ -63,89 +63,6 @@ bool PhotonTrackedTarget::operator!=(const PhotonTrackedTarget& other) const { return !operator==(other); } -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; -} - } // namespace photonlib google::protobuf::Message* wpi::Protobuf::New( @@ -162,15 +79,22 @@ wpi::Protobuf::Unpack( auto m = static_cast(&msg); + wpi::SmallVector, 4> minAreaRectCorners; + for (const auto& t : m->minarearectcorners()) { + minAreaRectCorners.emplace_back(t.x(), t.y()); + } + + std::vector> detectedCorners; + detectedCorners.reserve(m->detectedcorners_size()); + for (const auto& t : m->detectedcorners()) { + detectedCorners.emplace_back(t.x(), t.y()); + } + return photonlib::PhotonTrackedTarget( m->yaw(), m->pitch(), m->area(), m->skew(), m->fiducialid(), wpi::UnpackProtobuf(m->bestcameratotarget()), wpi::UnpackProtobuf(m->altcameratotarget()), - m->poseambiguity(), - // corners - {}, - // detected corners - {}); + m->poseambiguity(), minAreaRectCorners, detectedCorners); } void wpi::Protobuf::Pack( diff --git a/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h b/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h index 96681a0307..74da796203 100644 --- a/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h +++ b/photon-lib/src/main/native/include/photonlib/MultiTargetPNPResult.h @@ -27,8 +27,6 @@ #include #include -#include "photonlib/Packet.h" - namespace photonlib { class PNPResults { @@ -44,18 +42,12 @@ class PNPResults { double altReprojectionErr; double ambiguity; - - friend Packet& operator<<(Packet& packet, const PNPResults& result); - friend Packet& operator>>(Packet& packet, PNPResults& result); }; class MultiTargetPnpResult { public: PNPResults result; wpi::SmallVector fiducialIdsUsed; - - friend Packet& operator<<(Packet& packet, const MultiTargetPnpResult& result); - friend Packet& operator>>(Packet& packet, MultiTargetPnpResult& result); }; } // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/Packet.h b/photon-lib/src/main/native/include/photonlib/Packet.h deleted file mode 100644 index 59ae40b462..0000000000 --- a/photon-lib/src/main/native/include/photonlib/Packet.h +++ /dev/null @@ -1,130 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#pragma once - -#include -#include -#include - -#include - -namespace photonlib { - -/** - * A packet that holds byte-packed data to be sent over NetworkTables. - */ -class Packet { - public: - /** - * Constructs an empty packet. - */ - Packet() = default; - - /** - * Constructs a packet with the given data. - * @param data The packet data. - */ - explicit Packet(std::vector data) : packetData(data) {} - - /** - * Clears the packet and resets the read and write positions. - */ - void Clear() { - packetData.clear(); - readPos = 0; - writePos = 0; - } - - /** - * Returns the packet data. - * @return The packet data. - */ - const std::vector& GetData() { return packetData; } - - /** - * Returns the number of bytes in the data. - * @return The number of bytes in the data. - */ - size_t GetDataSize() const { return packetData.size(); } - - /** - * Adds a value to the data buffer. This should only be used with PODs. - * @tparam T The data type. - * @param src The data source. - * @return A reference to the current object. - */ - template - Packet& operator<<(T src) { - packetData.resize(packetData.size() + sizeof(T)); - std::memcpy(packetData.data() + writePos, &src, sizeof(T)); - - if constexpr (wpi::support::endian::system_endianness() == - wpi::support::endianness::little) { - // Reverse to big endian for network conventions. - std::reverse(packetData.data() + writePos, - packetData.data() + writePos + sizeof(T)); - } - - writePos += sizeof(T); - return *this; - } - - /** - * Extracts a value to the provided destination. - * @tparam T The type of value to extract. - * @param value The value to extract. - * @return A reference to the current object. - */ - template - Packet& operator>>(T& value) { - if (!packetData.empty()) { - std::memcpy(&value, packetData.data() + readPos, sizeof(T)); - - if constexpr (wpi::support::endian::system_endianness() == - wpi::support::endianness::little) { - // Reverse to little endian for host. - uint8_t& raw = reinterpret_cast(value); - std::reverse(&raw, &raw + sizeof(T)); - } - } - - readPos += sizeof(T); - return *this; - } - - bool operator==(const Packet& right) const { - return packetData == right.packetData; - } - bool operator!=(const Packet& right) const { return !operator==(right); } - - private: - // Data stored in the packet - std::vector packetData; - - size_t readPos = 0; - size_t writePos = 0; -}; - -} // namespace photonlib diff --git a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h index 8acef300cf..59027ed140 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonCamera.h @@ -198,8 +198,6 @@ class PhotonCamera { std::string path; std::string m_cameraName; - mutable Packet packet; - private: units::second_t lastVersionCheckTime = 0_s; inline static bool VERSION_CHECK_ENABLED = true; diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h index 9a76fce034..08825eca8e 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonPipelineResult.h @@ -33,7 +33,6 @@ #include #include "photonlib/MultiTargetPNPResult.h" -#include "photonlib/Packet.h" #include "photonlib/PhotonTrackedTarget.h" namespace photonlib { @@ -121,9 +120,6 @@ class PhotonPipelineResult { bool operator==(const PhotonPipelineResult& other) const; bool operator!=(const PhotonPipelineResult& other) const; - friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result); - friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result); - private: units::second_t latency = 0_s; units::second_t timestamp = -1_s; diff --git a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h index 154cbb9924..933b0da2aa 100644 --- a/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h +++ b/photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h @@ -32,8 +32,6 @@ #include #include -#include "photonlib/Packet.h" - namespace photonlib { /** * Represents a tracked target within a pipeline. @@ -144,9 +142,6 @@ class PhotonTrackedTarget { bool operator==(const PhotonTrackedTarget& other) const; bool operator!=(const PhotonTrackedTarget& other) const; - friend Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target); - friend Packet& operator>>(Packet& packet, PhotonTrackedTarget& target); - private: double yaw = 0; double pitch = 0; diff --git a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h index ff96bbd5c9..9972b06e67 100644 --- a/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h +++ b/photon-lib/src/main/native/include/photonlib/SimPhotonCamera.h @@ -48,8 +48,9 @@ class SimPhotonCamera : public PhotonCamera { targetAreaEntry = rootTable->GetEntry("targetAreaEntry"); targetSkewEntry = rootTable->GetEntry("targetSkewEntry"); targetPoseEntry = rootTable->GetEntry("targetPoseEntry"); - // rawBytesPublisher = - // rootTable->GetRawTopic("rawBytes").Publish("rawBytes"); + pipelineResultsPublisher = + rootTable->GetProtobufTopic("result_proto") + .Publish({.periodic = 0.01, .sendAll = true}); versionEntry = instance.GetTable("photonvision")->GetEntry("version"); } @@ -86,11 +87,8 @@ class SimPhotonCamera : public PhotonCamera { std::sort(targetList.begin(), targetList.end(), [&](auto lhs, auto rhs) { return sortMode(lhs, rhs); }); PhotonPipelineResult newResult{latency, targetList}; - Packet packet{}; - packet << newResult; - // rawBytesPublisher.Set( - // std::span{packet.GetData().data(), packet.GetDataSize()}); + pipelineResultsPublisher.Set(newResult); bool hasTargets = newResult.HasTargets(); hasTargetEntry.SetBoolean(hasTargets); diff --git a/photon-lib/src/test/native/cpp/PacketTest.cpp b/photon-lib/src/test/native/cpp/PacketTest.cpp deleted file mode 100644 index fe53cd85c1..0000000000 --- a/photon-lib/src/test/native/cpp/PacketTest.cpp +++ /dev/null @@ -1,108 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include - -#include - -#include "gtest/gtest.h" -#include "photonlib/PhotonPipelineResult.h" -#include "photonlib/PhotonTrackedTarget.h" - -TEST(PacketTest, PhotonTrackedTarget) { - photonlib::PhotonTrackedTarget target{ - 3.0, - 4.0, - 9.0, - -5.0, - -1, - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - -1, - {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, - {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}; - - photonlib::Packet p; - p << target; - - photonlib::PhotonTrackedTarget b; - p >> b; - - for (auto& c : p.GetData()) { - std::cout << static_cast(c) << ","; - } - - EXPECT_EQ(target, b); -} - -TEST(PacketTest, PhotonPipelineResult) { - photonlib::PhotonPipelineResult result{1_s, {}}; - photonlib::Packet p; - p << result; - - photonlib::PhotonPipelineResult b; - p >> b; - - EXPECT_EQ(result, b); - - wpi::SmallVector targets{ - photonlib::PhotonTrackedTarget{ - 3.0, - -4.0, - 9.0, - 4.0, - 1, - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - -1, - {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, - {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}}, - photonlib::PhotonTrackedTarget{ - 3.0, - -4.0, - 9.1, - 6.7, - -1, - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m), - frc::Rotation3d(1_rad, 2_rad, 3_rad)), - -1, - {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}, - {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, - std::pair{7, 8}}}}; - - photonlib::PhotonPipelineResult result2{2_s, targets}; - photonlib::Packet p2; - p2 << result2; - - photonlib::PhotonPipelineResult b2; - p2 >> b2; - - EXPECT_EQ(result2, b2); -}