diff --git a/build.gradle b/build.gradle index f772c89e32..b4e3f50576 100644 --- a/build.gradle +++ b/build.gradle @@ -1,9 +1,10 @@ plugins { id "com.diffplug.spotless" version "6.22.0" - id "edu.wpi.first.NativeUtils" version "2024.3.2" apply false + id "edu.wpi.first.NativeUtils" version "2024.4.0" apply false id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2" id "edu.wpi.first.GradleRIO" version "2024.1.1-beta-3" id 'edu.wpi.first.WpilibTools' version '1.3.0' + id 'com.google.protobuf' version '0.9.4' apply false } allprojects { @@ -20,7 +21,7 @@ allprojects { apply from: "versioningHelper.gradle" ext { - wpilibVersion = "2024.1.1-beta-3" + wpilibVersion = "2024.1.1-beta-3-52-g9280054" wpimathVersion = wpilibVersion openCVversion = "4.8.0-2" joglVersion = "2.4.0-rc-20200307" diff --git a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java index d985c710f5..24f5d0ad8d 100644 --- a/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java +++ b/photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java @@ -23,7 +23,6 @@ import java.util.function.Consumer; import java.util.function.Supplier; import org.photonvision.common.dataflow.CVPipelineResultConsumer; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.logging.LogGroup; import org.photonvision.common.logging.Logger; import org.photonvision.common.networktables.NTTopicSet; @@ -129,15 +128,13 @@ public void updateCameraNickname(String newCameraNickname) { @Override public void accept(CVPipelineResult result) { - var simplified = + var res = new PhotonPipelineResult( result.getLatencyMillis(), TrackedTarget.simpleFromTrackedTargets(result.targets), result.multiTagResult); - Packet packet = new Packet(simplified.getPacketSize()); - simplified.populatePacket(packet); - ts.rawBytesEntry.set(packet.getData()); + ts.pipelineResultsPublisher.set(res); ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get()); ts.driverModePublisher.set(driverModeSupplier.getAsBoolean()); diff --git a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java index 15a6d0b9e0..4dc788a0a7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java +++ b/photon-core/src/main/java/org/photonvision/vision/calibration/JsonMat.java @@ -26,7 +26,6 @@ import org.opencv.core.CvType; import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.vision.opencv.Releasable; public class JsonMat implements Releasable { @@ -121,9 +120,4 @@ public Matrix getAsWpilibMat() { public void release() { getAsMat().release(); } - - public Packet populatePacket(Packet packet) { - packet.encode(this.data); - return packet; - } } diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 099308f470..ec468c1f21 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -60,7 +60,35 @@ task writeCurrentVersion { } build.mustRunAfter writeCurrentVersion +cppHeadersZip.dependsOn writeCurrentVersion +// Building photon-lib requires photon-targeting to generate its proto files. This technically shouldn't be required but is needed for it to build. +model { + components { + all { + it.sources.each { + it.exportedHeaders { + srcDirs "src/main/native/include" + srcDirs "src/generate/native/include" + } + } + it.binaries.all { + it.tasks.withType(CppCompile) { + it.dependsOn ":photon-targeting:generateProto" + } + } + } + } + testSuites { + all { + it.binaries.all { + it.tasks.withType(CppCompile) { + it.dependsOn ":photon-targeting:generateProto" + } + } + } + } +} def vendorJson = artifacts.add('archives', file("$photonlibFileOutput")) model { diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 8b46a2c372..f7bf613fb9 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -39,14 +39,13 @@ import edu.wpi.first.networktables.MultiSubscriber; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.ProtobufSubscriber; import edu.wpi.first.networktables.PubSubOption; -import edu.wpi.first.networktables.RawSubscriber; import edu.wpi.first.networktables.StringSubscriber; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import java.util.Optional; import java.util.Set; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.hardware.VisionLEDMode; import org.photonvision.targeting.PhotonPipelineResult; @@ -55,7 +54,7 @@ public class PhotonCamera implements AutoCloseable { public static final String kTableName = "photonvision"; private final NetworkTable cameraTable; - RawSubscriber rawBytesEntry; + ProtobufSubscriber pipelineResultsSubscriber; BooleanPublisher driverModePublisher; BooleanSubscriber driverModeSubscriber; DoublePublisher latencyMillisEntry; @@ -75,7 +74,7 @@ public class PhotonCamera implements AutoCloseable { @Override public void close() { - rawBytesEntry.close(); + pipelineResultsSubscriber.close(); driverModePublisher.close(); driverModeSubscriber.close(); latencyMillisEntry.close(); @@ -112,8 +111,6 @@ public static void setVersionCheckEnabled(boolean enabled) { VERSION_CHECK_ENABLED = enabled; } - Packet packet = new Packet(1); - /** * Constructs a PhotonCamera from a root table. * @@ -127,11 +124,14 @@ public PhotonCamera(NetworkTableInstance instance, String cameraName) { var photonvision_root_table = instance.getTable(kTableName); this.cameraTable = photonvision_root_table.getSubTable(cameraName); path = cameraTable.getPath(); - rawBytesEntry = + pipelineResultsSubscriber = cameraTable - .getRawTopic("rawBytes") + .getProtobufTopic("result_proto", PhotonPipelineResult.proto) .subscribe( - "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); + new PhotonPipelineResult(), + PubSubOption.periodic(0.01), + PubSubOption.sendAll(true)); + driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish(); driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false); inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0); @@ -171,21 +171,12 @@ public PhotonCamera(String cameraName) { public PhotonPipelineResult getLatestResult() { verifyVersion(); - // Clear the packet. - packet.clear(); - - // Create latest result. - var ret = new PhotonPipelineResult(); - - // Populate packet and create result. - packet.setData(rawBytesEntry.get(new byte[] {})); - - if (packet.getSize() < 1) return ret; - ret.createFromPacket(packet); + var ret = pipelineResultsSubscriber.get(); // Set the timestamp of the result. // getLatestChange returns in microseconds, so we divide by 1e6 to convert to seconds. - ret.setTimestampSeconds((rawBytesEntry.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3); + ret.setTimestampSeconds( + (pipelineResultsSubscriber.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3); // Return result. return ret; @@ -321,14 +312,14 @@ public boolean isConnected() { public Optional> getCameraMatrix() { var cameraMatrix = cameraIntrinsicsSubscriber.get(); if (cameraMatrix != null && cameraMatrix.length == 9) { - return Optional.of(new MatBuilder<>(Nat.N3(), Nat.N3()).fill(cameraMatrix)); + return Optional.of(MatBuilder.fill(Nat.N3(), Nat.N3(), cameraMatrix)); } else return Optional.empty(); } public Optional> getDistCoeffs() { var distCoeffs = cameraDistortionSubscriber.get(); if (distCoeffs != null && distCoeffs.length == 5) { - return Optional.of(new MatBuilder<>(Nat.N5(), Nat.N1()).fill(distCoeffs)); + return Optional.of(MatBuilder.fill(Nat.N5(), Nat.N1(), distCoeffs)); } else return Optional.empty(); } diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index b602b0194d..90dc3e1211 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -47,7 +47,6 @@ import org.opencv.imgproc.Imgproc; import org.photonvision.PhotonCamera; import org.photonvision.PhotonTargetSortMode; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.networktables.NTTopicSet; import org.photonvision.estimation.CameraTargetRelation; import org.photonvision.estimation.OpenCVHelp; @@ -562,11 +561,9 @@ public void submitProcessedFrame(PhotonPipelineResult result) { * @param receiveTimestamp The (sim) timestamp when this result was read by NT in microseconds */ public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimestamp) { - ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp); + ts.pipelineResultsPublisher.set(result, receiveTimestamp); - var newPacket = new Packet(result.getPacketSize()); - result.populatePacket(newPacket); - ts.rawBytesEntry.set(newPacket.getData(), receiveTimestamp); + ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp); boolean hasTargets = result.hasTargets(); ts.hasTargetEntry.set(hasTargets, receiveTimestamp); diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java index 6d157fb252..1e2183fac8 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimCameraProperties.java @@ -25,6 +25,7 @@ package org.photonvision.simulation; import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; @@ -146,8 +147,8 @@ public SimCameraProperties(Path path, int width, int height) throws IOException setCalibration( jsonWidth, jsonHeight, - Matrix.mat(Nat.N3(), Nat.N3()).fill(jsonIntrinsics), - Matrix.mat(Nat.N5(), Nat.N1()).fill(jsonDistortion)); + MatBuilder.fill(Nat.N3(), Nat.N3(), jsonIntrinsics), + MatBuilder.fill(Nat.N5(), Nat.N1(), jsonDistortion)); setCalibError(jsonAvgError, jsonErrorStdDev); success = true; } @@ -184,7 +185,7 @@ public void setCalibration(int resWidth, int resHeight, Rotation2d fovDiag) { double fy = cy / Math.tan(fovHeight.getRadians() / 2.0); // create camera intrinsics matrix - var camIntrinsics = Matrix.mat(Nat.N3(), Nat.N3()).fill(fx, 0, cx, 0, fy, cy, 0, 0, 1); + var camIntrinsics = MatBuilder.fill(Nat.N3(), Nat.N3(), fx, 0, cx, 0, fy, cy, 0, 0, 1); setCalibration(resWidth, resHeight, camIntrinsics, distCoeff); } @@ -597,17 +598,19 @@ public static SimCameraProperties PI4_LIFECAM_320_240() { prop.setCalibration( 320, 240, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 328.2733242048587, - 0.0, - 164.8190261141906, - 0.0, - 318.0609794305216, - 123.8633838438093, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 328.2733242048587, + 0.0, + 164.8190261141906, + 0.0, + 318.0609794305216, + 123.8633838438093, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.09957946553445934, -0.9166265114485799, @@ -626,17 +629,19 @@ public static SimCameraProperties PI4_LIFECAM_640_480() { prop.setCalibration( 640, 480, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 669.1428078983059, - 0.0, - 322.53377249329213, - 0.0, - 646.9843137061716, - 241.26567383784163, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 669.1428078983059, + 0.0, + 322.53377249329213, + 0.0, + 646.9843137061716, + 241.26567383784163, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.12788470750464645, -1.2350335805796528, @@ -655,17 +660,19 @@ public static SimCameraProperties LL2_640_480() { prop.setCalibration( 640, 480, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 511.22843367007755, - 0.0, - 323.62049380211096, - 0.0, - 514.5452336723849, - 261.8827920543568, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 511.22843367007755, + 0.0, + 323.62049380211096, + 0.0, + 514.5452336723849, + 261.8827920543568, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.1917469998873756, -0.5142936883324216, @@ -684,17 +691,19 @@ public static SimCameraProperties LL2_960_720() { prop.setCalibration( 960, 720, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 769.6873145148892, - 0.0, - 486.1096609458122, - 0.0, - 773.8164483705323, - 384.66071662358354, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 769.6873145148892, + 0.0, + 486.1096609458122, + 0.0, + 773.8164483705323, + 384.66071662358354, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.189462064814501, -0.49903003669627627, @@ -713,17 +722,19 @@ public static SimCameraProperties LL2_1280_720() { prop.setCalibration( 1280, 720, - Matrix.mat(Nat.N3(), Nat.N3()) - .fill( // intrinsic - 1011.3749416937393, - 0.0, - 645.4955139388737, - 0.0, - 1008.5391755084075, - 508.32877656020196, - 0.0, - 0.0, - 1.0), + MatBuilder.fill( + Nat.N3(), + Nat.N3(), + // intrinsic + 1011.3749416937393, + 0.0, + 645.4955139388737, + 0.0, + 1008.5391755084075, + 508.32877656020196, + 0.0, + 0.0, + 1.0), VecBuilder.fill( // distort 0.13730101577061535, -0.2904345656989261, diff --git a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java index 5eb73b35eb..fc5d155de4 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/SimPhotonCamera.java @@ -33,7 +33,6 @@ import java.util.List; import org.photonvision.PhotonCamera; import org.photonvision.PhotonTargetSortMode; -import org.photonvision.common.dataflow.structures.Packet; import org.photonvision.common.networktables.NTTopicSet; import org.photonvision.targeting.MultiTargetPNPResult; import org.photonvision.targeting.PhotonPipelineResult; @@ -143,9 +142,7 @@ public void submitProcessedFrame( PhotonPipelineResult newResult = new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResult()); - var newPacket = new Packet(newResult.getPacketSize()); - newResult.populatePacket(newPacket); - ts.rawBytesEntry.set(newPacket.getData()); + ts.pipelineResultsPublisher.set(newResult); boolean hasTargets = newResult.hasTargets(); ts.hasTargetEntry.set(hasTargets); diff --git a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp index 95eecb444c..981165c055 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonCamera.cpp @@ -29,7 +29,6 @@ #include #include "PhotonVersion.h" -#include "photon/dataflow/structures/Packet.h" namespace photon { @@ -40,9 +39,10 @@ PhotonCamera::PhotonCamera(nt::NetworkTableInstance instance, const std::string_view cameraName) : mainTable(instance.GetTable("photonvision")), rootTable(mainTable->GetSubTable(cameraName)), - rawBytesEntry( - rootTable->GetRawTopic("rawBytes") - .Subscribe("rawBytes", {}, {.periodic = 0.01, .sendAll = true})), + pipelineResultsSubscriber( + rootTable->GetProtobufTopic("result_proto") + .Subscribe(PhotonPipelineResult(), + {.periodic = 0.01, .sendAll = true})), inputSaveImgEntry( rootTable->GetIntegerTopic("inputSaveImgCmd").Publish()), inputSaveImgSubscriber( @@ -81,22 +81,12 @@ PhotonPipelineResult PhotonCamera::GetLatestResult() { // Prints warning if not connected VerifyVersion(); - // Clear the current packet. - packet.Clear(); - // Create the new result; - PhotonPipelineResult result; - - // Fill the packet with latest data and populate result. - const auto value = rawBytesEntry.Get(); - if (!value.size()) return result; - - photon::Packet packet{value}; - - packet >> result; + PhotonPipelineResult result = pipelineResultsSubscriber.Get(); - result.SetTimestamp(units::microsecond_t(rawBytesEntry.GetLastChange()) - - result.GetLatency()); + result.SetTimestamp( + units::microsecond_t(pipelineResultsSubscriber.GetLastChange()) - + result.GetLatency()); return result; } diff --git a/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp b/photon-lib/src/main/native/include/geometry3d.pb.h similarity index 83% rename from photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp rename to photon-lib/src/main/native/include/geometry3d.pb.h index 95a14e259e..2bfe3ddb1d 100644 --- a/photon-lib/src/test/native/cpp/PhotonUtilsTest.cpp +++ b/photon-lib/src/main/native/include/geometry3d.pb.h @@ -22,7 +22,8 @@ * SOFTWARE. */ -#include "gtest/gtest.h" -#include "photon/PhotonUtils.h" - -TEST(PhotonUtilsTest, Include) {} +#pragma once +// So wpilib publishes protbufs here at wpimath/protobuf. but generated code +// assumes that the protobuf includes are on your include path. So we need this +// stupid shim +#include "wpimath/protobuf/geometry3d.pb.h" diff --git a/photon-lib/src/main/native/include/photon/PhotonCamera.h b/photon-lib/src/main/native/include/photon/PhotonCamera.h index 2a1418f1b5..12adb6ad54 100644 --- a/photon-lib/src/main/native/include/photon/PhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/PhotonCamera.h @@ -34,12 +34,11 @@ #include #include #include -#include +#include #include #include #include -#include "photon/dataflow/structures/Packet.h" #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" @@ -179,7 +178,7 @@ class PhotonCamera { protected: std::shared_ptr mainTable; std::shared_ptr rootTable; - nt::RawSubscriber rawBytesEntry; + nt::ProtobufSubscriber pipelineResultsSubscriber; nt::IntegerPublisher inputSaveImgEntry; nt::IntegerSubscriber inputSaveImgSubscriber; nt::IntegerPublisher outputSaveImgEntry; @@ -202,8 +201,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/photon/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h index cdb60f6cee..b2d495be80 100644 --- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h +++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h @@ -31,7 +31,6 @@ #include #include "photon/PhotonCamera.h" -#include "photon/dataflow/structures/Packet.h" #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" diff --git a/photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h b/photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h index c8ad53c39f..bbe70d960b 100644 --- a/photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h +++ b/photon-lib/src/main/native/include/photon/PhotonTargetSortMode.h @@ -26,7 +26,6 @@ #include -#include "photon/dataflow/structures/Packet.h" #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" diff --git a/photon-lib/src/main/native/include/photon/PhotonUtils.h b/photon-lib/src/main/native/include/photon/PhotonUtils.h index 14076a5cd3..de18ae8125 100644 --- a/photon-lib/src/main/native/include/photon/PhotonUtils.h +++ b/photon-lib/src/main/native/include/photon/PhotonUtils.h @@ -32,7 +32,6 @@ #include #include -#include "photon/dataflow/structures/Packet.h" #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" diff --git a/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h b/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h index 365efebd59..dbd3270dfa 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimPhotonCamera.h @@ -34,7 +34,6 @@ #include "photon/PhotonCamera.h" #include "photon/PhotonTargetSortMode.h" -#include "photon/dataflow/structures/Packet.h" #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" @@ -53,9 +52,10 @@ 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"); - // versionEntry.SetString(PhotonVersion.versionString); } explicit SimPhotonCamera(const std::string& cameraName) @@ -91,11 +91,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); @@ -132,6 +129,6 @@ class SimPhotonCamera : public PhotonCamera { nt::NetworkTableEntry targetSkewEntry; nt::NetworkTableEntry targetPoseEntry; nt::NetworkTableEntry versionEntry; - nt::RawPublisher rawBytesPublisher; + nt::ProtobufPublisher pipelineResultsPublisher; }; } // namespace photon diff --git a/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h b/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h index 759efb3adc..dd1b39f194 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimVisionSystem.h @@ -34,7 +34,6 @@ #include "SimPhotonCamera.h" #include "SimVisionTarget.h" -#include "photon/dataflow/structures/Packet.h" #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" diff --git a/photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h b/photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h index 33a9adfee4..5d1975cd1a 100644 --- a/photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h +++ b/photon-lib/src/main/native/include/photon/simulation/SimVisionTarget.h @@ -27,7 +27,6 @@ #include #include -#include "photon/dataflow/structures/Packet.h" #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" diff --git a/photon-lib/src/test/java/org/photonvision/PacketTest.java b/photon-lib/src/test/java/org/photonvision/PacketTest.java deleted file mode 100644 index a2f98a27c2..0000000000 --- a/photon-lib/src/test/java/org/photonvision/PacketTest.java +++ /dev/null @@ -1,190 +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. - */ - -package org.photonvision; - -import edu.wpi.first.math.geometry.*; -import java.util.ArrayList; -import java.util.List; -import org.junit.jupiter.api.Assertions; -import org.junit.jupiter.api.Test; -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.targeting.MultiTargetPNPResult; -import org.photonvision.targeting.PNPResult; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; - -class PacketTest { - @Test - void testSimpleTrackedTarget() { - var target = - new PhotonTrackedTarget( - 3.0, - 4.0, - 9.0, - -5.0, - -1, - new Transform3d(new Translation3d(), new Rotation3d()), - new Transform3d(new Translation3d(), new Rotation3d()), - 0.25, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))); - var p = new Packet(PhotonTrackedTarget.PACK_SIZE_BYTES); - target.populatePacket(p); - - var b = new PhotonTrackedTarget(); - b.createFromPacket(p); - - Assertions.assertEquals(target, b); - } - - @Test - void testSimplePipelineResult() { - var result = new PhotonPipelineResult(1, new ArrayList<>()); - var p = new Packet(result.getPacketSize()); - result.populatePacket(p); - - var b = new PhotonPipelineResult(); - b.createFromPacket(p); - - Assertions.assertEquals(result, b); - - var result2 = - new PhotonPipelineResult( - 2, - List.of( - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.0, - 4.0, - 2, - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - 0.25, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.1, - 6.7, - 3, - new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), - new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), - 0.25, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))))); - var p2 = new Packet(result2.getPacketSize()); - result2.populatePacket(p2); - - var b2 = new PhotonPipelineResult(); - b2.createFromPacket(p2); - - Assertions.assertEquals(result2, b2); - } - - @Test - public void testMultiTargetSerde() { - var result = - new PhotonPipelineResult( - 2, - List.of( - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.0, - 4.0, - 2, - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), - 0.25, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8))), - new PhotonTrackedTarget( - 3.0, - -4.0, - 9.1, - 6.7, - 3, - new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), - new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), - 0.25, - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)), - List.of( - new TargetCorner(1, 2), - new TargetCorner(3, 4), - new TargetCorner(5, 6), - new TargetCorner(7, 8)))), - new MultiTargetPNPResult( - new PNPResult( - new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), - List.of(1, 2, 3))); - - Packet packet = new Packet(result.getPacketSize()); - result.populatePacket(packet); - - var result_deserialized = new PhotonPipelineResult(); - result_deserialized.createFromPacket(packet); - - Assertions.assertEquals(result, result_deserialized); - } -} diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java deleted file mode 100644 index 8a3fb5fad8..0000000000 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ /dev/null @@ -1,46 +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. - */ - -package org.photonvision; - -import org.junit.jupiter.api.Assertions; -import org.junit.jupiter.api.Test; -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.targeting.PhotonPipelineResult; - -class PhotonCameraTest { - @Test - public void testEmpty() { - Assertions.assertDoesNotThrow( - () -> { - var packet = new Packet(1); - var ret = new PhotonPipelineResult(); - packet.setData(new byte[0]); - if (packet.getSize() < 1) { - return; - } - ret.createFromPacket(packet); - }); - } -} diff --git a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp index aed60ff2fc..6c78f9c0fc 100644 --- a/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp +++ b/photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp @@ -37,7 +37,6 @@ #include "gtest/gtest.h" #include "photon/PhotonCamera.h" #include "photon/PhotonPoseEstimator.h" -#include "photon/dataflow/structures/Packet.h" #include "photon/targeting/MultiTargetPNPResult.h" #include "photon/targeting/PNPResult.h" #include "photon/targeting/PhotonPipelineResult.h" diff --git a/photon-targeting/build.gradle b/photon-targeting/build.gradle index 4238e951e6..5eb2d058a8 100644 --- a/photon-targeting/build.gradle +++ b/photon-targeting/build.gradle @@ -1,4 +1,5 @@ plugins { + id 'java' id 'edu.wpi.first.WpilibTools' version '1.3.0' } diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java deleted file mode 100644 index e97e3f1658..0000000000 --- a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java +++ /dev/null @@ -1,216 +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 . - */ - -package org.photonvision.common.dataflow.structures; - -/** A packet that holds byte-packed data to be sent over NetworkTables. */ -public class Packet { - // Size of the packet. - int size; - // Data stored in the packet. - byte[] packetData; - // Read and write positions. - int readPos, writePos; - - /** - * Constructs an empty packet. - * - * @param size The size of the packet buffer. - */ - public Packet(int size) { - this.size = size; - packetData = new byte[size]; - } - - /** - * Constructs a packet with the given data. - * - * @param data The packet data. - */ - public Packet(byte[] data) { - packetData = data; - size = packetData.length; - } - - /** Clears the packet and resets the read and write positions. */ - public void clear() { - packetData = new byte[size]; - readPos = 0; - writePos = 0; - } - - public int getSize() { - return size; - } - - /** - * Returns the packet data. - * - * @return The packet data. - */ - public byte[] getData() { - return packetData; - } - - /** - * Sets the packet data. - * - * @param data The packet data. - */ - public void setData(byte[] data) { - packetData = data; - size = data.length; - } - - /** - * Encodes the byte into the packet. - * - * @param src The byte to encode. - */ - public void encode(byte src) { - packetData[writePos++] = src; - } - - /** - * Encodes the short into the packet. - * - * @param src The short to encode. - */ - public void encode(short src) { - packetData[writePos++] = (byte) (src >>> 8); - packetData[writePos++] = (byte) src; - } - - /** - * Encodes the integer into the packet. - * - * @param src The integer to encode. - */ - public void encode(int src) { - packetData[writePos++] = (byte) (src >>> 24); - packetData[writePos++] = (byte) (src >>> 16); - packetData[writePos++] = (byte) (src >>> 8); - packetData[writePos++] = (byte) src; - } - - /** - * Encodes the double into the packet. - * - * @param src The double to encode. - */ - public void encode(double src) { - long data = Double.doubleToRawLongBits(src); - packetData[writePos++] = (byte) ((data >> 56) & 0xff); - packetData[writePos++] = (byte) ((data >> 48) & 0xff); - packetData[writePos++] = (byte) ((data >> 40) & 0xff); - packetData[writePos++] = (byte) ((data >> 32) & 0xff); - packetData[writePos++] = (byte) ((data >> 24) & 0xff); - packetData[writePos++] = (byte) ((data >> 16) & 0xff); - packetData[writePos++] = (byte) ((data >> 8) & 0xff); - packetData[writePos++] = (byte) (data & 0xff); - } - - /** - * Encodes the boolean into the packet. - * - * @param src The boolean to encode. - */ - public void encode(boolean src) { - packetData[writePos++] = src ? (byte) 1 : (byte) 0; - } - - /** - * Returns a decoded byte from the packet. - * - * @return A decoded byte from the packet. - */ - public byte decodeByte() { - if (packetData.length < readPos) { - return '\0'; - } - return packetData[readPos++]; - } - - /** - * Returns a decoded int from the packet. - * - * @return A decoded int from the packet. - */ - public int decodeInt() { - if (packetData.length < readPos + 3) { - return 0; - } - return (0xff & packetData[readPos++]) << 24 - | (0xff & packetData[readPos++]) << 16 - | (0xff & packetData[readPos++]) << 8 - | (0xff & packetData[readPos++]); - } - - /** - * Returns a decoded double from the packet. - * - * @return A decoded double from the packet. - */ - public double decodeDouble() { - if (packetData.length < (readPos + 7)) { - return 0; - } - long data = - (long) (0xff & packetData[readPos++]) << 56 - | (long) (0xff & packetData[readPos++]) << 48 - | (long) (0xff & packetData[readPos++]) << 40 - | (long) (0xff & packetData[readPos++]) << 32 - | (long) (0xff & packetData[readPos++]) << 24 - | (long) (0xff & packetData[readPos++]) << 16 - | (long) (0xff & packetData[readPos++]) << 8 - | (long) (0xff & packetData[readPos++]); - return Double.longBitsToDouble(data); - } - - /** - * Returns a decoded boolean from the packet. - * - * @return A decoded boolean from the packet. - */ - public boolean decodeBoolean() { - if (packetData.length < readPos) { - return false; - } - return packetData[readPos++] == 1; - } - - public void encode(double[] data) { - for (double d : data) { - encode(d); - } - } - - public double[] decodeDoubleArray(int len) { - double[] ret = new double[len]; - for (int i = 0; i < len; i++) { - ret[i] = decodeDouble(); - } - return ret; - } - - public short decodeShort() { - if (packetData.length < readPos + 1) { - return 0; - } - return (short) ((0xff & packetData[readPos++]) << 8 | (0xff & packetData[readPos++])); - } -} diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java index 5a0d31eaed..5e995f2c32 100644 --- a/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java @@ -26,8 +26,9 @@ import edu.wpi.first.networktables.IntegerSubscriber; import edu.wpi.first.networktables.IntegerTopic; import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.ProtobufPublisher; import edu.wpi.first.networktables.PubSubOption; -import edu.wpi.first.networktables.RawPublisher; +import org.photonvision.targeting.PhotonPipelineResult; /** * This class is a wrapper around all per-pipeline NT topics that PhotonVision should be publishing @@ -39,7 +40,7 @@ */ public class NTTopicSet { public NetworkTable subTable; - public RawPublisher rawBytesEntry; + public ProtobufPublisher pipelineResultsPublisher; public IntegerPublisher pipelineIndexPublisher; public IntegerSubscriber pipelineIndexRequestSub; @@ -69,10 +70,10 @@ public class NTTopicSet { public DoubleArrayPublisher cameraDistortionPublisher; public void updateEntries() { - rawBytesEntry = + pipelineResultsPublisher = subTable - .getRawTopic("rawBytes") - .publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); + .getProtobufTopic("result_proto", PhotonPipelineResult.proto) + .publish(PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish(); pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0); @@ -104,7 +105,7 @@ public void updateEntries() { @SuppressWarnings("DuplicatedCode") public void removeEntries() { - if (rawBytesEntry != null) rawBytesEntry.close(); + if (pipelineResultsPublisher != null) pipelineResultsPublisher.close(); if (pipelineIndexPublisher != null) pipelineIndexPublisher.close(); if (pipelineIndexRequestSub != null) pipelineIndexRequestSub.close(); diff --git a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java index ed53a17b80..e2413a4d22 100644 --- a/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java +++ b/photon-targeting/src/main/java/org/photonvision/estimation/OpenCVHelp.java @@ -18,6 +18,7 @@ package org.photonvision.estimation; import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.Num; @@ -63,8 +64,8 @@ public static void forceLoadOpenCV() { } static { - NWU_TO_EDN = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, -1, 0, 0, 0, -1, 1, 0, 0)); - EDN_TO_NWU = new Rotation3d(Matrix.mat(Nat.N3(), Nat.N3()).fill(0, 0, 1, -1, 0, 0, 0, -1, 0)); + NWU_TO_EDN = new Rotation3d(MatBuilder.fill(Nat.N3(), Nat.N3(), 0, -1, 0, 0, 0, -1, 1, 0, 0)); + EDN_TO_NWU = new Rotation3d(MatBuilder.fill(Nat.N3(), Nat.N3(), 0, 0, 1, -1, 0, 0, 0, -1, 0)); } public static Mat matrixToMat(SimpleMatrix matrix) { diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java index a3ddaaa6f0..144a514fd4 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java @@ -17,16 +17,15 @@ package org.photonvision.targeting; -import java.util.ArrayList; +import edu.wpi.first.util.protobuf.Protobuf; +import java.util.Arrays; import java.util.List; -import org.photonvision.common.dataflow.structures.Packet; +import java.util.stream.Collectors; +import org.photonvision.proto.Photon.ProtobufMultiTargetPNPResult; +import us.hebi.quickbuf.Descriptors.Descriptor; +import us.hebi.quickbuf.RepeatedInt; public class MultiTargetPNPResult { - // Seeing 32 apriltags at once seems like a sane limit - private static final int MAX_IDS = 32; - // pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally) - public static final int PACK_SIZE_BYTES = PNPResult.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS); - public PNPResult estimatedPose = new PNPResult(); public List fiducialIDsUsed = List.of(); @@ -37,27 +36,6 @@ public MultiTargetPNPResult(PNPResult results, List ids) { fiducialIDsUsed = ids; } - public static MultiTargetPNPResult createFromPacket(Packet packet) { - var results = PNPResult.createFromPacket(packet); - var ids = new ArrayList(MAX_IDS); - for (int i = 0; i < MAX_IDS; i++) { - int targetId = packet.decodeShort(); - if (targetId > -1) ids.add(targetId); - } - return new MultiTargetPNPResult(results, ids); - } - - public void populatePacket(Packet packet) { - estimatedPose.populatePacket(packet); - for (int i = 0; i < MAX_IDS; i++) { - if (i < fiducialIDsUsed.size()) { - packet.encode((short) fiducialIDsUsed.get(i).byteValue()); - } else { - packet.encode((short) -1); - } - } - } - @Override public int hashCode() { final int prime = 31; @@ -90,4 +68,47 @@ public String toString() { + fiducialIDsUsed + "]"; } + + public static final class AProto + implements Protobuf { + @Override + public Class getTypeClass() { + return MultiTargetPNPResult.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufMultiTargetPNPResult.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {PNPResult.proto}; + } + + @Override + public ProtobufMultiTargetPNPResult createMessage() { + return ProtobufMultiTargetPNPResult.newInstance(); + } + + @Override + public MultiTargetPNPResult unpack(ProtobufMultiTargetPNPResult msg) { + return new MultiTargetPNPResult( + PNPResult.proto.unpack(msg.getEstimatedPose()), + // TODO better way of doing this + Arrays.stream(msg.getFiducialIdsUsed().array()).boxed().collect(Collectors.toList())); + } + + @Override + public void pack(ProtobufMultiTargetPNPResult msg, MultiTargetPNPResult value) { + PNPResult.proto.pack(msg.getMutableEstimatedPose(), value.estimatedPose); + + RepeatedInt idsUsed = msg.getMutableFiducialIdsUsed().reserve(value.fiducialIDsUsed.size()); + for (int i = 0; i < value.fiducialIDsUsed.size(); i++) { + idsUsed.add(value.fiducialIDsUsed.get(i)); + } + } + } + + public static final AProto proto = new AProto(); } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java index ab7aa5f46f..09be52941f 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java @@ -18,8 +18,9 @@ package org.photonvision.targeting; import edu.wpi.first.math.geometry.Transform3d; -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.utils.PacketUtils; +import edu.wpi.first.util.protobuf.Protobuf; +import org.photonvision.proto.Photon.ProtobufPNPResult; +import us.hebi.quickbuf.Descriptors.Descriptor; /** * The best estimated transformation from solvePnP, and possibly an alternate transformation @@ -86,31 +87,6 @@ public PNPResult( this.altReprojErr = altReprojErr; } - public static final int PACK_SIZE_BYTES = 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3); - - public static PNPResult createFromPacket(Packet packet) { - var present = packet.decodeBoolean(); - var best = PacketUtils.decodeTransform(packet); - var alt = PacketUtils.decodeTransform(packet); - var bestEr = packet.decodeDouble(); - var altEr = packet.decodeDouble(); - var ambiguity = packet.decodeDouble(); - if (present) { - return new PNPResult(best, alt, ambiguity, bestEr, altEr); - } else { - return new PNPResult(); - } - } - - public void populatePacket(Packet packet) { - packet.encode(isPresent); - PacketUtils.encodeTransform(packet, best); - PacketUtils.encodeTransform(packet, alt); - packet.encode(bestReprojErr); - packet.encode(altReprojErr); - packet.encode(ambiguity); - } - @Override public int hashCode() { final int prime = 31; @@ -166,4 +142,52 @@ public String toString() { + ambiguity + "]"; } + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return PNPResult.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPNPResult.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Transform3d.proto}; + } + + @Override + public ProtobufPNPResult createMessage() { + return ProtobufPNPResult.newInstance(); + } + + @Override + public PNPResult unpack(ProtobufPNPResult msg) { + if (!msg.getIsPresent()) { + return new PNPResult(); + } + + return new PNPResult( + Transform3d.proto.unpack(msg.getBest()), + Transform3d.proto.unpack(msg.getAlt()), + msg.getAmbiguity(), + msg.getBestReprojErr(), + msg.getAltReprojErr()); + } + + @Override + public void pack(ProtobufPNPResult msg, PNPResult value) { + Transform3d.proto.pack(msg.getMutableBest(), value.best); + Transform3d.proto.pack(msg.getMutableAlt(), value.alt); + msg.setAmbiguity(value.ambiguity) + .setBestReprojErr(value.bestReprojErr) + .setAltReprojErr(value.altReprojErr) + .setIsPresent(value.isPresent); + } + } + + public static final AProto proto = new AProto(); } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 9a8ce65db9..6759237a73 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -17,9 +17,11 @@ package org.photonvision.targeting; +import edu.wpi.first.util.protobuf.Protobuf; import java.util.ArrayList; import java.util.List; -import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.proto.Photon.ProtobufPhotonPipelineResult; +import us.hebi.quickbuf.Descriptors.Descriptor; /** Represents a pipeline result from a PhotonCamera. */ public class PhotonPipelineResult { @@ -65,18 +67,6 @@ public PhotonPipelineResult( this.multiTagResult = result; } - /** - * Returns the size of the packet needed to store this pipeline result. - * - * @return The size of the packet needed to store this pipeline result. - */ - public int getPacketSize() { - return targets.size() * PhotonTrackedTarget.PACK_SIZE_BYTES - + 8 // latency - + MultiTargetPNPResult.PACK_SIZE_BYTES - + 1; // target count - } - /** * Returns the best target in this pipeline result. If there are no targets, this method will * return null. The best target is determined by the target sort mode in the PhotonVision UI. @@ -150,49 +140,6 @@ public MultiTargetPNPResult getMultiTagResult() { return multiTagResult; } - /** - * Populates the fields of the pipeline result from the packet. - * - * @param packet The incoming packet. - * @return The incoming packet. - */ - public Packet createFromPacket(Packet packet) { - // Decode latency, existence of targets, and number of targets. - latencyMillis = packet.decodeDouble(); - this.multiTagResult = MultiTargetPNPResult.createFromPacket(packet); - byte targetCount = packet.decodeByte(); - - targets.clear(); - - // Decode the information of each target. - for (int i = 0; i < (int) targetCount; ++i) { - var target = new PhotonTrackedTarget(); - target.createFromPacket(packet); - targets.add(target); - } - - return packet; - } - - /** - * Populates the outgoing packet with information from this pipeline result. - * - * @param packet The outgoing packet. - * @return The outgoing packet. - */ - public Packet populatePacket(Packet packet) { - // Encode latency, existence of targets, and number of targets. - packet.encode(latencyMillis); - multiTagResult.populatePacket(packet); - packet.encode((byte) targets.size()); - - // Encode the information of each target. - for (var target : targets) target.populatePacket(packet); - - // Return the packet. - return packet; - } - @Override public int hashCode() { final int prime = 31; @@ -236,4 +183,45 @@ public String toString() { + multiTagResult + "]"; } + + public static final class AProto + implements Protobuf { + @Override + public Class getTypeClass() { + return PhotonPipelineResult.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPhotonPipelineResult.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {PhotonTrackedTarget.proto, MultiTargetPNPResult.proto}; + } + + @Override + public ProtobufPhotonPipelineResult createMessage() { + return ProtobufPhotonPipelineResult.newInstance(); + } + + @Override + public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) { + return new PhotonPipelineResult( + msg.getLatencyMs(), + PhotonTrackedTarget.proto.unpack(msg.getTargets()), + MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult())); + } + + @Override + public void pack(ProtobufPhotonPipelineResult msg, PhotonPipelineResult value) { + PhotonTrackedTarget.proto.pack(msg.getMutableTargets(), value.targets); + MultiTargetPNPResult.proto.pack(msg.getMutableMultiTargetResult(), value.multiTagResult); + + msg.setLatencyMs(value.getLatencyMillis()); + } + } + + public static final AProto proto = new AProto(); } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java index 392a429006..631ded96c3 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -18,32 +18,29 @@ package org.photonvision.targeting; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.util.protobuf.Protobuf; import java.util.ArrayList; import java.util.List; -import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.utils.PacketUtils; +import org.photonvision.proto.Photon.ProtobufPhotonTrackedTarget; +import us.hebi.quickbuf.Descriptors.Descriptor; +import us.hebi.quickbuf.RepeatedMessage; public class PhotonTrackedTarget { private static final int MAX_CORNERS = 8; - public static final int PACK_SIZE_BYTES = - Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS); - - private double yaw; - private double pitch; - private double area; - private double skew; - private int fiducialId; - private Transform3d bestCameraToTarget = new Transform3d(); - private Transform3d altCameraToTarget = new Transform3d(); - private double poseAmbiguity; - // Corners from the min-area rectangle bounding the target - private List minAreaRectCorners; + private final double yaw; + private final double pitch; + private final double area; + private final double skew; + private final int fiducialId; + private final Transform3d bestCameraToTarget; + private final Transform3d altCameraToTarget; + private final double poseAmbiguity; + // Corners from the min-area rectangle bounding the target + private final List minAreaRectCorners; // Corners from whatever corner detection method was used - private List detectedCorners; - - public PhotonTrackedTarget() {} + private final List detectedCorners; /** Construct a tracked target, given exactly 4 corners */ public PhotonTrackedTarget( @@ -197,81 +194,6 @@ public boolean equals(Object obj) { return true; } - private static void encodeList(Packet packet, List list) { - packet.encode((byte) Math.min(list.size(), Byte.MAX_VALUE)); - for (TargetCorner targetCorner : list) { - packet.encode(targetCorner.x); - packet.encode(targetCorner.y); - } - } - - private static List decodeList(Packet p) { - byte len = p.decodeByte(); - var ret = new ArrayList(); - for (int i = 0; i < len; i++) { - double cx = p.decodeDouble(); - double cy = p.decodeDouble(); - ret.add(new TargetCorner(cx, cy)); - } - return ret; - } - - /** - * Populates the fields of this class with information from the incoming packet. - * - * @param packet The incoming packet. - * @return The incoming packet. - */ - public Packet createFromPacket(Packet packet) { - this.yaw = packet.decodeDouble(); - this.pitch = packet.decodeDouble(); - this.area = packet.decodeDouble(); - this.skew = packet.decodeDouble(); - this.fiducialId = packet.decodeInt(); - - this.bestCameraToTarget = PacketUtils.decodeTransform(packet); - this.altCameraToTarget = PacketUtils.decodeTransform(packet); - - this.poseAmbiguity = packet.decodeDouble(); - - this.minAreaRectCorners = new ArrayList<>(4); - for (int i = 0; i < 4; i++) { - double cx = packet.decodeDouble(); - double cy = packet.decodeDouble(); - minAreaRectCorners.add(new TargetCorner(cx, cy)); - } - - detectedCorners = decodeList(packet); - - return packet; - } - - /** - * Populates the outgoing packet with information from the current target. - * - * @param packet The outgoing packet. - * @return The outgoing packet. - */ - public Packet populatePacket(Packet packet) { - packet.encode(yaw); - packet.encode(pitch); - packet.encode(area); - packet.encode(skew); - packet.encode(fiducialId); - PacketUtils.encodeTransform(packet, bestCameraToTarget); - PacketUtils.encodeTransform(packet, altCameraToTarget); - packet.encode(poseAmbiguity); - - for (int i = 0; i < 4; i++) { - packet.encode(minAreaRectCorners.get(i).x); - packet.encode(minAreaRectCorners.get(i).y); - } - - encodeList(packet, detectedCorners); - - return packet; - } - @Override public String toString() { return "PhotonTrackedTarget{" @@ -291,4 +213,77 @@ public String toString() { + minAreaRectCorners + '}'; } + + public static final class AProto + implements Protobuf { + @Override + public Class getTypeClass() { + return PhotonTrackedTarget.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufPhotonTrackedTarget.getDescriptor(); + } + + @Override + public Protobuf[] getNested() { + return new Protobuf[] {Transform3d.proto, TargetCorner.proto}; + } + + @Override + public ProtobufPhotonTrackedTarget createMessage() { + return ProtobufPhotonTrackedTarget.newInstance(); + } + + @Override + public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) { + return new PhotonTrackedTarget( + msg.getYaw(), + msg.getPitch(), + msg.getArea(), + msg.getSkew(), + msg.getFiducialId(), + Transform3d.proto.unpack(msg.getBestCameraToTarget()), + Transform3d.proto.unpack(msg.getAltCameraToTarget()), + msg.getPoseAmbiguity(), + TargetCorner.proto.unpack(msg.getMinAreaRectCorners()), + TargetCorner.proto.unpack(msg.getDetectedCorners())); + } + + public List unpack(RepeatedMessage msg) { + ArrayList targets = new ArrayList<>(msg.length()); + for (ProtobufPhotonTrackedTarget target : msg) { + targets.add(unpack(target)); + } + return targets; + } + + @Override + public void pack(ProtobufPhotonTrackedTarget msg, PhotonTrackedTarget value) { + msg.setYaw(value.getYaw()) + .setPitch(value.getPitch()) + .setSkew(value.getSkew()) + .setArea(value.getArea()) + .setFiducialId(value.getFiducialId()) + .setPoseAmbiguity(value.getPoseAmbiguity()); + + Transform3d.proto.pack(msg.getMutableBestCameraToTarget(), value.bestCameraToTarget); + Transform3d.proto.pack(msg.getMutableAltCameraToTarget(), value.altCameraToTarget); + + TargetCorner.proto.pack(msg.getMutableMinAreaRectCorners(), value.getMinAreaRectCorners()); + TargetCorner.proto.pack(msg.getMutableDetectedCorners(), value.getDetectedCorners()); + } + + public void pack( + RepeatedMessage msg, List value) { + var targets = msg.reserve(value.size()); + for (PhotonTrackedTarget trackedTarget : value) { + var target = targets.next(); + pack(target, trackedTarget); + } + } + } + + public static final AProto proto = new AProto(); } diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java index 9808079a5d..ccfa41713a 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -17,7 +17,13 @@ package org.photonvision.targeting; +import edu.wpi.first.util.protobuf.Protobuf; +import java.util.ArrayList; +import java.util.List; import java.util.Objects; +import org.photonvision.proto.Photon.ProtobufTargetCorner; +import us.hebi.quickbuf.Descriptors.Descriptor; +import us.hebi.quickbuf.RepeatedMessage; /** * Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels. @@ -49,4 +55,49 @@ public int hashCode() { public String toString() { return "(" + x + "," + y + ')'; } + + public static final class AProto implements Protobuf { + @Override + public Class getTypeClass() { + return TargetCorner.class; + } + + @Override + public Descriptor getDescriptor() { + return ProtobufTargetCorner.getDescriptor(); + } + + @Override + public ProtobufTargetCorner createMessage() { + return ProtobufTargetCorner.newInstance(); + } + + @Override + public TargetCorner unpack(ProtobufTargetCorner msg) { + return new TargetCorner(msg.getX(), msg.getY()); + } + + public List unpack(RepeatedMessage msg) { + ArrayList corners = new ArrayList<>(msg.length()); + for (ProtobufTargetCorner corner : msg) { + corners.add(unpack(corner)); + } + return corners; + } + + @Override + public void pack(ProtobufTargetCorner msg, TargetCorner value) { + msg.setX(value.x).setY(value.y); + } + + public void pack(RepeatedMessage msg, List value) { + var corners = msg.reserve(value.size()); + for (TargetCorner targetCorner : value) { + var corner = corners.next(); + pack(corner, targetCorner); + } + } + } + + public static final AProto proto = new AProto(); } diff --git a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java deleted file mode 100644 index 43a0dd4cf6..0000000000 --- a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java +++ /dev/null @@ -1,49 +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 . - */ - -package org.photonvision.utils; - -import edu.wpi.first.math.geometry.Quaternion; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import org.photonvision.common.dataflow.structures.Packet; - -public class PacketUtils { - public static Transform3d decodeTransform(Packet packet) { - double x = packet.decodeDouble(); - double y = packet.decodeDouble(); - double z = packet.decodeDouble(); - var translation = new Translation3d(x, y, z); - double w = packet.decodeDouble(); - x = packet.decodeDouble(); - y = packet.decodeDouble(); - z = packet.decodeDouble(); - var rotation = new Rotation3d(new Quaternion(w, x, y, z)); - return new Transform3d(translation, rotation); - } - - public static void encodeTransform(Packet packet, Transform3d transform) { - packet.encode(transform.getTranslation().getX()); - packet.encode(transform.getTranslation().getY()); - packet.encode(transform.getTranslation().getZ()); - packet.encode(transform.getRotation().getQuaternion().getW()); - packet.encode(transform.getRotation().getQuaternion().getX()); - packet.encode(transform.getRotation().getQuaternion().getY()); - packet.encode(transform.getRotation().getQuaternion().getZ()); - } -} 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 3cfdbfe510..dee7cc2b9d 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/MultiTargetPNPResult.cpp @@ -17,40 +17,46 @@ #include "photon/targeting/MultiTargetPNPResult.h" +#include "photon.pb.h" + namespace photon { bool MultiTargetPNPResult::operator==(const MultiTargetPNPResult& other) const { return other.result == result && other.fiducialIdsUsed == fiducialIdsUsed; } +} // namespace photon + +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufMultiTargetPNPResult>(arena); +} -Packet& operator<<(Packet& packet, const MultiTargetPNPResult& result) { - packet << result.result; +photon::MultiTargetPNPResult +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast( + &msg); - size_t i; - for (i = 0; i < result.fiducialIdsUsed.capacity(); i++) { - if (i < result.fiducialIdsUsed.size()) { - packet << static_cast(result.fiducialIdsUsed[i]); - } else { - packet << static_cast(-1); - } + wpi::SmallVector fiducialIdsUsed; + for (int i = 0; i < m->fiducial_ids_used_size(); i++) { + fiducialIdsUsed.push_back(m->fiducial_ids_used(i)); } - return packet; + return photon::MultiTargetPNPResult{ + wpi::UnpackProtobuf(m->estimated_pose()), + fiducialIdsUsed}; } -Packet& operator>>(Packet& packet, MultiTargetPNPResult& result) { - packet >> result.result; +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const photon::MultiTargetPNPResult& value) { + auto m = static_cast(msg); - result.fiducialIdsUsed.clear(); - for (size_t i = 0; i < result.fiducialIdsUsed.capacity(); i++) { - int16_t id = 0; - packet >> id; + wpi::PackProtobuf(m->mutable_estimated_pose(), value.result); - if (id > -1) { - result.fiducialIdsUsed.push_back(id); - } + m->clear_fiducial_ids_used(); + for (const auto& t : value.fiducialIdsUsed) { + m->add_fiducial_ids_used(t); } - - return packet; } -} // namespace photon 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 b43e43481d..96d3b3d23b 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PNPResult.cpp @@ -17,61 +17,44 @@ #include "photon/targeting/PNPResult.h" +#include "photon.pb.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; } +} // namespace photon -// 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; +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufPNPResult>(arena); } -// 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)); +photon::PNPResult wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast(&msg); - // decode and add units to rotation - packet >> w >> x >> y >> z; - const auto rotation = frc::Rotation3d(frc::Quaternion(w, x, y, z)); + if (!m->is_present()) { + return photon::PNPResult(); + } - transform = frc::Transform3d(translation, rotation); - - return packet; + return photon::PNPResult{wpi::UnpackProtobuf(m->best()), + m->best_reproj_err(), + wpi::UnpackProtobuf(m->alt()), + m->alt_reproj_err(), m->ambiguity()}; } -Packet& operator<<(Packet& packet, PNPResult const& result) { - packet << result.isPresent << result.best << result.alt - << result.bestReprojErr << result.altReprojErr << result.ambiguity; +void wpi::Protobuf::Pack(google::protobuf::Message* msg, + const photon::PNPResult& value) { + auto m = static_cast(msg); - return packet; + 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); + m->set_alt_reproj_err(value.altReprojErr); + m->set_ambiguity(value.ambiguity); } - -Packet& operator>>(Packet& packet, PNPResult& result) { - packet >> result.isPresent >> result.best >> result.alt >> - result.bestReprojErr >> result.altReprojErr >> result.ambiguity; - - return packet; -} -} // namespace photon 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..64378dd3e7 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonPipelineResult.cpp @@ -17,51 +17,50 @@ #include "photon/targeting/PhotonPipelineResult.h" -namespace photon { -PhotonPipelineResult::PhotonPipelineResult( - units::second_t latency, std::span targets) - : latency(latency), - targets(targets.data(), targets.data() + targets.size()) {} - -PhotonPipelineResult::PhotonPipelineResult( - units::second_t latency, std::span targets, - MultiTargetPNPResult multitagResult) - : latency(latency), - targets(targets.data(), targets.data() + targets.size()), - multitagResult(multitagResult) {} +#include "photon.pb.h" +namespace photon { bool PhotonPipelineResult::operator==(const PhotonPipelineResult& other) const { return latency == other.latency && targets == other.targets && multitagResult == other.multitagResult; } +} // namespace photon -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()); +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufPhotonPipelineResult>(arena); +} - // Encode the information of each target. - for (auto& target : result.targets) packet << target; +photon::PhotonPipelineResult +wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = + static_cast( + &msg); - // Return the packet - return packet; + std::vector targets; + targets.reserve(m->targets_size()); + for (const auto& t : m->targets()) { + targets.emplace_back(wpi::UnpackProtobuf(t)); + } + + return photon::PhotonPipelineResult{ + units::millisecond_t{m->latency_ms()}, targets, + wpi::UnpackProtobuf( + m->multi_target_result())}; } -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); +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const photon::PhotonPipelineResult& value) { + auto m = static_cast(msg); - result.targets.clear(); + m->set_latency_ms(value.latency.value()); - // Decode the information of each target. - for (int i = 0; i < targetCount; ++i) { - PhotonTrackedTarget target; - packet >> target; - result.targets.push_back(target); + m->clear_targets(); + for (const auto& t : value.GetTargets()) { + wpi::PackProtobuf(m->add_targets(), t); } - return packet; + + wpi::PackProtobuf(m->mutable_multi_target_result(), value.multitagResult); } -} // namespace photon 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 3252528efd..7b4909ca9c 100644 --- a/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp +++ b/photon-targeting/src/main/native/cpp/photon/targeting/PhotonTrackedTarget.cpp @@ -23,113 +23,78 @@ #include #include +#include "photon.pb.h" + static constexpr const uint8_t MAX_CORNERS = 8; namespace photon { - -PhotonTrackedTarget::PhotonTrackedTarget( - double yaw, double pitch, double area, double skew, int id, - const frc::Transform3d& pose, const frc::Transform3d& alternatePose, - double ambiguity, - const wpi::SmallVector, 4> minAreaRectCorners, - const std::vector> detectedCorners) - : yaw(yaw), - pitch(pitch), - area(area), - skew(skew), - fiducialId(id), - 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.minAreaRectCorners == minAreaRectCorners; } +} // namespace photon -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; +google::protobuf::Message* wpi::Protobuf::New( + google::protobuf::Arena* arena) { + return google::protobuf::Arena::CreateMessage< + photonvision::proto::ProtobufPhotonTrackedTarget>(arena); +} + +photon::PhotonTrackedTarget wpi::Protobuf::Unpack( + const google::protobuf::Message& msg) { + auto m = static_cast( + &msg); - for (int i = 0; i < 4; i++) { - packet << target.minAreaRectCorners[i].first - << target.minAreaRectCorners[i].second; + wpi::SmallVector, 4> minAreaRectCorners; + for (const auto& t : m->min_area_rect_corners()) { + minAreaRectCorners.emplace_back(t.x(), t.y()); } - 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; + std::vector> detectedCorners; + detectedCorners.reserve(m->detected_corners_size()); + for (const auto& t : m->detected_corners()) { + detectedCorners.emplace_back(t.x(), t.y()); } - return packet; + return photon::PhotonTrackedTarget{ + m->yaw(), + m->pitch(), + m->area(), + m->skew(), + m->fiducial_id(), + wpi::UnpackProtobuf(m->best_camera_to_target()), + wpi::UnpackProtobuf(m->alt_camera_to_target()), + m->pose_ambiguity(), + minAreaRectCorners, + detectedCorners}; } -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); +void wpi::Protobuf::Pack( + google::protobuf::Message* msg, const photon::PhotonTrackedTarget& value) { + auto m = static_cast(msg); + + m->set_yaw(value.yaw); + m->set_pitch(value.pitch); + m->set_area(value.area); + m->set_skew(value.skew); + m->set_fiducial_id(value.fiducialId); + wpi::PackProtobuf(m->mutable_best_camera_to_target(), + value.bestCameraToTarget); + wpi::PackProtobuf(m->mutable_alt_camera_to_target(), value.altCameraToTarget); + m->set_pose_ambiguity(value.poseAmbiguity); + + 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); } - 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); + 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); } - - return packet; } -} // namespace photon diff --git a/photon-targeting/src/main/native/include/geometry3d.pb.h b/photon-targeting/src/main/native/include/geometry3d.pb.h new file mode 100644 index 0000000000..3cff26d7e0 --- /dev/null +++ b/photon-targeting/src/main/native/include/geometry3d.pb.h @@ -0,0 +1,22 @@ +/* + * 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 . + */ + +#pragma once +// So wpilib publishes protbufs here at wpimath/protobuf. but generated code +// assumes that the protobuf includes are on your include path. So we need this +// stupid shim +#include "wpimath/protobuf/geometry3d.pb.h" diff --git a/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h b/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h deleted file mode 100644 index c2b16c7821..0000000000 --- a/photon-targeting/src/main/native/include/photon/dataflow/structures/Packet.h +++ /dev/null @@ -1,122 +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 . - */ - -#pragma once - -#include -#include -#include - -#include - -namespace photon { - -/** - * 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 photon diff --git a/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h index 76a323e39a..cf5f58741a 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/MultiTargetPNPResult.h @@ -19,9 +19,9 @@ #include #include +#include #include "PNPResult.h" -#include "photon/dataflow/structures/Packet.h" namespace photon { class MultiTargetPNPResult { @@ -29,9 +29,21 @@ class MultiTargetPNPResult { PNPResult result; wpi::SmallVector fiducialIdsUsed; - bool operator==(const MultiTargetPNPResult& other) const; + MultiTargetPNPResult() = default; + + MultiTargetPNPResult(PNPResult result, + wpi::SmallVector fiducialIdsUsed) + : result(result), fiducialIdsUsed(fiducialIdsUsed) {} - friend Packet& operator<<(Packet& packet, const MultiTargetPNPResult& result); - friend Packet& operator>>(Packet& packet, MultiTargetPNPResult& result); + bool operator==(const MultiTargetPNPResult& other) const; }; } // namespace photon + +template <> +struct wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static photon::MultiTargetPNPResult Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const photon::MultiTargetPNPResult& value); +}; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h index e97f6c8d71..f200a3de04 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PNPResult.h @@ -18,8 +18,7 @@ #pragma once #include - -#include "photon/dataflow/structures/Packet.h" +#include namespace photon { @@ -37,9 +36,34 @@ class PNPResult { double ambiguity; - bool operator==(const PNPResult& other) const; + // Apparently this can't be default-constructed? Things seem to have garbadge + // with the defaulted empty constructor anyhow + PNPResult() + : isPresent{false}, + best{frc::Transform3d{}}, + bestReprojErr{0}, + alt{frc::Transform3d()}, + altReprojErr{0}, + ambiguity{0} {} + + PNPResult(frc::Transform3d best, double bestReprojErr, frc::Transform3d alt, + double altReprojErr, double ambiguity) + : best(best), + bestReprojErr(bestReprojErr), + alt(alt), + altReprojErr(altReprojErr), + ambiguity(ambiguity) { + this->isPresent = true; + } - friend Packet& operator<<(Packet& packet, const PNPResult& target); - friend Packet& operator>>(Packet& packet, PNPResult& target); + bool operator==(const PNPResult& other) const; }; } // namespace photon + +template <> +struct wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static photon::PNPResult Unpack(const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const photon::PNPResult& value); +}; 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..8c1abeb965 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonPipelineResult.h @@ -23,10 +23,10 @@ #include #include #include +#include #include "MultiTargetPNPResult.h" #include "PhotonTrackedTarget.h" -#include "photon/dataflow/structures/Packet.h" namespace photon { /** @@ -34,6 +34,12 @@ namespace photon { */ class PhotonPipelineResult { public: + units::millisecond_t latency = 0_s; + units::second_t timestamp = -1_s; + wpi::SmallVector targets; + MultiTargetPNPResult multitagResult; + inline static bool HAS_WARNED = false; + /** * Constructs an empty pipeline result */ @@ -44,8 +50,10 @@ class PhotonPipelineResult { * @param latency The latency in the pipeline. * @param targets The list of targets identified by the pipeline. */ - PhotonPipelineResult(units::second_t latency, - std::span targets); + PhotonPipelineResult(units::millisecond_t latency, + std::span targets) + : latency(latency), + targets(targets.data(), targets.data() + targets.size()) {} /** * Constructs a pipeline result. @@ -53,9 +61,12 @@ 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); + MultiTargetPNPResult multitagResult) + : latency(latency), + targets(targets.data(), targets.data() + targets.size()), + multitagResult(multitagResult) {} /** * Returns the best target in this pipeline result. If there are no targets, @@ -121,14 +132,14 @@ class PhotonPipelineResult { } bool operator==(const PhotonPipelineResult& other) const; - - friend Packet& operator<<(Packet& packet, const PhotonPipelineResult& result); - friend Packet& operator>>(Packet& packet, PhotonPipelineResult& result); - - units::second_t latency = 0_s; - units::second_t timestamp = -1_s; - wpi::SmallVector targets; - MultiTargetPNPResult multitagResult; - inline static bool HAS_WARNED = false; }; } // namespace photon + +template <> +struct wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static photon::PhotonPipelineResult Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const photon::PhotonPipelineResult& value); +}; diff --git a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h index feba565910..80ede3dc0c 100644 --- a/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h +++ b/photon-targeting/src/main/native/include/photon/targeting/PhotonTrackedTarget.h @@ -24,8 +24,7 @@ #include #include - -#include "photon/dataflow/structures/Packet.h" +#include namespace photon { /** @@ -33,6 +32,17 @@ namespace photon { */ class PhotonTrackedTarget { public: + double yaw = 0; + double pitch = 0; + double area = 0; + double skew = 0; + int fiducialId; + frc::Transform3d bestCameraToTarget; + frc::Transform3d altCameraToTarget; + double poseAmbiguity; + wpi::SmallVector, 4> minAreaRectCorners; + std::vector> detectedCorners; + /** * Constructs an empty target. */ @@ -50,11 +60,21 @@ class PhotonTrackedTarget { * @param detectedCorners All detected corners */ PhotonTrackedTarget( - double yaw, double pitch, double area, double skew, int fiducialID, - const frc::Transform3d& pose, const frc::Transform3d& alternatePose, + double yaw, double pitch, double area, double skew, int fiducialId, + const frc::Transform3d pose, const frc::Transform3d alternatePose, double ambiguity, const wpi::SmallVector, 4> minAreaRectCorners, - const std::vector> detectedCorners); + const std::vector> detectedCorners) + : yaw(yaw), + pitch(pitch), + area(area), + skew(skew), + fiducialId(fiducialId), + bestCameraToTarget(pose), + altCameraToTarget(alternatePose), + poseAmbiguity(ambiguity), + minAreaRectCorners(minAreaRectCorners), + detectedCorners(detectedCorners) {} /** * Returns the target yaw (positive-left). @@ -138,19 +158,14 @@ class PhotonTrackedTarget { } bool operator==(const PhotonTrackedTarget& other) const; - - friend Packet& operator<<(Packet& packet, const PhotonTrackedTarget& target); - friend Packet& operator>>(Packet& packet, PhotonTrackedTarget& target); - - double yaw = 0; - double pitch = 0; - double area = 0; - double skew = 0; - int fiducialId; - frc::Transform3d bestCameraToTarget; - frc::Transform3d altCameraToTarget; - double poseAmbiguity; - wpi::SmallVector, 4> minAreaRectCorners; - std::vector> detectedCorners; }; } // namespace photon + +template <> +struct wpi::Protobuf { + static google::protobuf::Message* New(google::protobuf::Arena* arena); + static photon::PhotonTrackedTarget Unpack( + const google::protobuf::Message& msg); + static void Pack(google::protobuf::Message* msg, + const photon::PhotonTrackedTarget& value); +}; diff --git a/photon-targeting/src/main/proto/photon.proto b/photon-targeting/src/main/proto/photon.proto new file mode 100644 index 0000000000..d5b75802fc --- /dev/null +++ b/photon-targeting/src/main/proto/photon.proto @@ -0,0 +1,63 @@ +/* + * 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 . + */ + +syntax = "proto3"; + +package photonvision.proto; + +import "geometry3d.proto"; + +option java_package = "org.photonvision.proto"; + +message ProtobufTargetCorner { + double x = 1; + double y = 2; +} + +message ProtobufPNPResult { + bool is_present = 1; + wpi.proto.ProtobufTransform3d best = 2; + double best_reproj_err = 3; + optional wpi.proto.ProtobufTransform3d alt = 4; + optional double alt_reproj_err = 5; + double ambiguity = 6; +} + +message ProtobufMultiTargetPNPResult { + ProtobufPNPResult estimated_pose = 1; + repeated int32 fiducial_ids_used = 2; +} + +message ProtobufPhotonTrackedTarget { + double yaw = 1; + double pitch = 2; + double area = 3; + double skew = 4; + int32 fiducial_id = 5; + wpi.proto.ProtobufTransform3d best_camera_to_target = 6; + wpi.proto.ProtobufTransform3d alt_camera_to_target = 7; + double pose_ambiguity = 8; + repeated ProtobufTargetCorner min_area_rect_corners = 9; + repeated ProtobufTargetCorner detected_corners = 10; +} + +message ProtobufPhotonPipelineResult { + double latency_ms = 1; + + repeated ProtobufPhotonTrackedTarget targets = 2; + ProtobufMultiTargetPNPResult multi_target_result = 3; +} diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java index 4e28237ac5..1d709c875d 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/MultiTargetPNPResultTest.java @@ -27,6 +27,25 @@ import org.junit.jupiter.api.Test; public class MultiTargetPNPResultTest { + @Test + public void protobufTest() { + var result = new MultiTargetPNPResult(); + var serializedResult = MultiTargetPNPResult.proto.createMessage(); + MultiTargetPNPResult.proto.pack(serializedResult, result); + var unpackedResult = MultiTargetPNPResult.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + + result = + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3)); + serializedResult = MultiTargetPNPResult.proto.createMessage(); + MultiTargetPNPResult.proto.pack(serializedResult, result); + unpackedResult = MultiTargetPNPResult.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + } + @Test public void equalityTest() { var a = new MultiTargetPNPResult(); diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java index 9f86473eaa..55e9d1966e 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PNPResultTest.java @@ -25,6 +25,21 @@ import org.junit.jupiter.api.Test; public class PNPResultTest { + @Test + public void protobufTest() { + var pnpRes = new PNPResult(); + var serializedPNPRes = PNPResult.proto.createMessage(); + PNPResult.proto.pack(serializedPNPRes, pnpRes); + var unpackedPNPRes = PNPResult.proto.unpack(serializedPNPRes); + assertEquals(pnpRes, unpackedPNPRes); + + pnpRes = new PNPResult(new Transform3d(1, 2, 3, new Rotation3d(1, 2, 3)), 0.1); + serializedPNPRes = PNPResult.proto.createMessage(); + PNPResult.proto.pack(serializedPNPRes, pnpRes); + unpackedPNPRes = PNPResult.proto.unpack(serializedPNPRes); + assertEquals(pnpRes, unpackedPNPRes); + } + @Test public void equalityTest() { var a = new PNPResult(); diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java index 72dacda168..be17e744d2 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonPipelineResultTest.java @@ -27,6 +27,116 @@ import org.junit.jupiter.api.Test; public class PhotonPipelineResultTest { + @Test + public void protobufTest() { + // Empty Result + var result = new PhotonPipelineResult(); + var serializedResult = PhotonPipelineResult.proto.createMessage(); + PhotonPipelineResult.proto.pack(serializedResult, result); + var unpackedResult = PhotonPipelineResult.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + + // non multitag result + result = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))))); + serializedResult = PhotonPipelineResult.proto.createMessage(); + PhotonPipelineResult.proto.pack(serializedResult, result); + unpackedResult = PhotonPipelineResult.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + + // multitag result + result = + new PhotonPipelineResult( + 2, + List.of( + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.0, + 4.0, + 2, + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 3.0, + -4.0, + 9.1, + 6.7, + 3, + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))), + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3))); + serializedResult = PhotonPipelineResult.proto.createMessage(); + PhotonPipelineResult.proto.pack(serializedResult, result); + unpackedResult = PhotonPipelineResult.proto.unpack(serializedResult); + assertEquals(result, unpackedResult); + } + @Test public void equalityTest() { var a = new PhotonPipelineResult(); diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java index 5d42a2b68f..c458bc4c17 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/PhotonTrackedTargetTest.java @@ -25,8 +25,93 @@ import edu.wpi.first.math.geometry.Translation3d; import java.util.List; import org.junit.jupiter.api.Test; +import org.photonvision.proto.Photon.ProtobufPhotonTrackedTarget; +import us.hebi.quickbuf.RepeatedMessage; public class PhotonTrackedTargetTest { + @Test + public void protobufTest() { + var target = + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))); + var serializedTarget = PhotonTrackedTarget.proto.createMessage(); + PhotonTrackedTarget.proto.pack(serializedTarget, target); + var unpackedTarget = PhotonTrackedTarget.proto.unpack(serializedTarget); + assertEquals(target, unpackedTarget); + } + + @Test + public void protobufListTest() { + List targets = List.of(); + var serializedTargets = + RepeatedMessage.newEmptyInstance(ProtobufPhotonTrackedTarget.getFactory()); + PhotonTrackedTarget.proto.pack(serializedTargets, targets); + var unpackedTargets = PhotonTrackedTarget.proto.unpack(serializedTargets); + assertEquals(targets, unpackedTargets); + + targets = + List.of( + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8))), + new PhotonTrackedTarget( + 7.0, + 2.0, + 1.0, + -9.0, + -1, + new Transform3d(new Translation3d(), new Rotation3d()), + new Transform3d(new Translation3d(), new Rotation3d()), + 0.25, + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)), + List.of( + new TargetCorner(1, 2), + new TargetCorner(3, 4), + new TargetCorner(5, 6), + new TargetCorner(7, 8)))); + serializedTargets = RepeatedMessage.newEmptyInstance(ProtobufPhotonTrackedTarget.getFactory()); + PhotonTrackedTarget.proto.pack(serializedTargets, targets); + unpackedTargets = PhotonTrackedTarget.proto.unpack(serializedTargets); + assertEquals(targets, unpackedTargets); + } + @Test public void equalityTest() { var a = diff --git a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java index b813fb7afb..53beaed614 100644 --- a/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java +++ b/photon-targeting/src/test/java/org/photonvision/targeting/TargetCornerTest.java @@ -20,9 +20,36 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; +import java.util.List; import org.junit.jupiter.api.Test; +import org.photonvision.proto.Photon.ProtobufTargetCorner; +import us.hebi.quickbuf.RepeatedMessage; public class TargetCornerTest { + @Test + public void protobufTest() { + var corner = new TargetCorner(0, 1); + var serializedCorner = TargetCorner.proto.createMessage(); + TargetCorner.proto.pack(serializedCorner, corner); + var unpackedCorner = TargetCorner.proto.unpack(serializedCorner); + assertEquals(corner, unpackedCorner); + } + + @Test + public void protobufListTest() { + List corners = List.of(); + var serializedCorners = RepeatedMessage.newEmptyInstance(ProtobufTargetCorner.getFactory()); + TargetCorner.proto.pack(serializedCorners, corners); + var unpackedCorners = TargetCorner.proto.unpack(serializedCorners); + assertEquals(corners, unpackedCorners); + + corners = List.of(new TargetCorner(0, 1), new TargetCorner(1, 2)); + serializedCorners = RepeatedMessage.newEmptyInstance(ProtobufTargetCorner.getFactory()); + TargetCorner.proto.pack(serializedCorners, corners); + unpackedCorners = TargetCorner.proto.unpack(serializedCorners); + assertEquals(corners, unpackedCorners); + } + @Test public void equalityTest() { var a = new TargetCorner(0, 1); diff --git a/photon-targeting/src/test/native/cpp/PacketTest.cpp b/photon-targeting/src/test/native/cpp/PacketTest.cpp deleted file mode 100644 index 1094a3783f..0000000000 --- a/photon-targeting/src/test/native/cpp/PacketTest.cpp +++ /dev/null @@ -1,120 +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 . - */ - -#include - -#include "gtest/gtest.h" -#include "photon/dataflow/structures/Packet.h" -#include "photon/targeting/MultiTargetPNPResult.h" -#include "photon/targeting/PNPResult.h" -#include "photon/targeting/PhotonPipelineResult.h" -#include "photon/targeting/PhotonTrackedTarget.h" - -TEST(PacketTest, PNPResult) { - photon::PNPResult result; - photon::Packet p; - p << result; - - photon::PNPResult b; - p >> b; - - EXPECT_EQ(result, b); -} - -TEST(PacketTest, MultiTargetPNPResult) { - photon::MultiTargetPNPResult result; - photon::Packet p; - p << result; - - photon::MultiTargetPNPResult b; - p >> b; - - EXPECT_EQ(result, b); -} - -TEST(PacketTest, PhotonTrackedTarget) { - photon::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}}}; - - photon::Packet p; - p << target; - - photon::PhotonTrackedTarget b; - p >> b; - - EXPECT_EQ(target, b); -} - -TEST(PacketTest, PhotonPipelineResult) { - photon::PhotonPipelineResult result{1_s, {}}; - photon::Packet p; - p << result; - - photon::PhotonPipelineResult b; - p >> b; - - EXPECT_EQ(result, b); - - wpi::SmallVector targets{ - photon::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}}}, - photon::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}}}}; - - photon::PhotonPipelineResult result2{2_s, targets}; - photon::Packet p2; - p2 << result2; - - photon::PhotonPipelineResult b2; - p2 >> b2; - - EXPECT_EQ(result2, b2); -} diff --git a/photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp b/photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp index 1add81e9f6..ffda261ca4 100644 --- a/photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp +++ b/photon-targeting/src/test/native/cpp/targeting/MultiTargetPNPResultTest.cpp @@ -16,10 +16,57 @@ */ #include "gtest/gtest.h" +#include "photon.pb.h" #include "photon/targeting/MultiTargetPNPResult.h" -// TODO -TEST(MultiTargetPNPResultTest, Equality) {} +TEST(MultiTargetPNPResultTest, Equality) { + photon::MultiTargetPNPResult a; + photon::MultiTargetPNPResult b; -// TODO -TEST(MultiTargetPNPResultTest, Inequality) {} + EXPECT_EQ(a, b); + + photon::PNPResult pnpRes{ + 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}; + + photon::MultiTargetPNPResult a1{pnpRes, {1, 2, 3, 4}}; + photon::MultiTargetPNPResult b1{pnpRes, {1, 2, 3, 4}}; + + EXPECT_EQ(a1, b1); +} + +TEST(MultiTargetPNPResultTest, Roundtrip) { + photon::MultiTargetPNPResult result; + + google::protobuf::Arena arena; + google::protobuf::Message* proto = + wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result); + + photon::MultiTargetPNPResult unpacked_data = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result, unpacked_data); + + photon::PNPResult pnpRes{ + 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}; + + photon::MultiTargetPNPResult result1{pnpRes, {1, 2, 3, 4}}; + + proto = wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result1); + + photon::MultiTargetPNPResult unpacked_data1 = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result1, unpacked_data1); +} diff --git a/photon-targeting/src/test/native/cpp/targeting/PNPResultTest.cpp b/photon-targeting/src/test/native/cpp/targeting/PNPResultTest.cpp index ac2c2adf11..c4313396ae 100644 --- a/photon-targeting/src/test/native/cpp/targeting/PNPResultTest.cpp +++ b/photon-targeting/src/test/native/cpp/targeting/PNPResultTest.cpp @@ -16,10 +16,57 @@ */ #include "gtest/gtest.h" +#include "photon.pb.h" #include "photon/targeting/PNPResult.h" -// TODO -TEST(PNPResultTest, Equality) {} +TEST(PNPResultTest, Equality) { + photon::PNPResult a; + photon::PNPResult b; -// TODO -TEST(PNPResultTest, Inequality) {} + EXPECT_EQ(a, b); + + photon::PNPResult a1{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}; + photon::PNPResult b1{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}; + + EXPECT_EQ(a1, b1); +} + +TEST(PNPResultTest, Roundtrip) { + photon::PNPResult result; + + google::protobuf::Arena arena; + google::protobuf::Message* proto = + wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result); + + photon::PNPResult unpacked_data = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result, unpacked_data); + + photon::PNPResult result1{ + 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}; + + proto = wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result1); + + photon::PNPResult unpacked_data2 = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result1, unpacked_data2); +} diff --git a/photon-targeting/src/test/native/cpp/targeting/PhotonPipelineResultTest.cpp b/photon-targeting/src/test/native/cpp/targeting/PhotonPipelineResultTest.cpp index 716334dcb3..73a4efcfdb 100644 --- a/photon-targeting/src/test/native/cpp/targeting/PhotonPipelineResultTest.cpp +++ b/photon-targeting/src/test/native/cpp/targeting/PhotonPipelineResultTest.cpp @@ -16,10 +16,137 @@ */ #include "gtest/gtest.h" +#include "photon.pb.h" #include "photon/targeting/PhotonPipelineResult.h" +#include "photon/targeting/PhotonTrackedTarget.h" // TODO -TEST(PhotonPipelineResultTest, Equality) {} +TEST(PhotonPipelineResultTest, Equality) { + photon::PhotonPipelineResult a{12_ms, {}}; + photon::PhotonPipelineResult b{12_ms, {}}; + + EXPECT_EQ(a, b); + + wpi::SmallVector targets{ + photon::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}}}, + photon::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}}}}; + + photon::PhotonPipelineResult a1{12_ms, targets}; + photon::PhotonPipelineResult b1{12_ms, targets}; + + EXPECT_EQ(a1, b1); + + photon::PNPResult pnpRes{ + 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}; + + photon::MultiTargetPNPResult multitagRes{pnpRes, {1, 2, 3, 4}}; + + photon::PhotonPipelineResult a2{12_ms, targets, multitagRes}; + photon::PhotonPipelineResult b2{12_ms, targets, multitagRes}; + + EXPECT_EQ(a2, b2); +} // TODO -TEST(PhotonPipelineResultTest, Inequality) {} +TEST(PhotonPipelineResultTest, Roundtrip) { + photon::PhotonPipelineResult result{12_ms, {}}; + + google::protobuf::Arena arena; + google::protobuf::Message* proto = + wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result); + + photon::PhotonPipelineResult unpacked_data = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result, unpacked_data); + + wpi::SmallVector targets{ + photon::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}}}, + photon::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}}}}; + + photon::PhotonPipelineResult result2{12_ms, targets}; + + proto = wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result2); + + photon::PhotonPipelineResult unpacked_data2 = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result2, unpacked_data2); + + photon::PNPResult pnpRes{ + 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}; + + photon::MultiTargetPNPResult multitagRes{pnpRes, {1, 2, 3, 4}}; + + photon::PhotonPipelineResult result3{12_ms, targets, multitagRes}; + + proto = wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, result3); + + photon::PhotonPipelineResult unpacked_data3 = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(result3, unpacked_data3); +} diff --git a/photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp b/photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp index 243c56ad6f..ea8d0d5de7 100644 --- a/photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp +++ b/photon-targeting/src/test/native/cpp/targeting/PhotonTrackedTargetTest.cpp @@ -16,10 +16,63 @@ */ #include "gtest/gtest.h" +#include "photon.pb.h" #include "photon/targeting/PhotonTrackedTarget.h" -// TODO -TEST(PhotonTrackedTargetTest, Equality) {} +TEST(PhotonTrackedTargetTest, Equality) { + photon::PhotonTrackedTarget a{ + 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}}}; -// TODO -TEST(PhotonTrackedTargetTest, Inequality) {} + photon::PhotonTrackedTarget b{ + 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}}}; + + EXPECT_EQ(a, b); +} + +TEST(PhotonTrackedTargetTest, Roundtrip) { + photon::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}}}; + + google::protobuf::Arena arena; + google::protobuf::Message* proto = + wpi::Protobuf::New(&arena); + wpi::Protobuf::Pack(proto, target); + + photon::PhotonTrackedTarget unpacked_data = + wpi::Protobuf::Unpack(*proto); + + EXPECT_EQ(target, unpacked_data); +} diff --git a/shared/common.gradle b/shared/common.gradle index faeaa1f694..7cbf858b22 100644 --- a/shared/common.gradle +++ b/shared/common.gradle @@ -32,6 +32,7 @@ dependencies { implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") + implementation wpilibTools.deps.wpilibJava("wpiunits") implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() diff --git a/shared/examples_common.gradle b/shared/examples_common.gradle index 5212307f0f..711f6c9fa8 100644 --- a/shared/examples_common.gradle +++ b/shared/examples_common.gradle @@ -18,3 +18,6 @@ task copyPhotonlib() { } outputs.upToDateWhen { false } } + +wpi.maven.useDevelopment = true +wpi.versions.wpilibVersion = "2024.1.1-beta-3-43-g25b7dca" diff --git a/shared/javacommon.gradle b/shared/javacommon.gradle index c4e0ad3b56..d1c17f5016 100644 --- a/shared/javacommon.gradle +++ b/shared/javacommon.gradle @@ -1,6 +1,7 @@ apply plugin: 'maven-publish' apply plugin: 'java-library' apply plugin: 'jacoco' +apply plugin: 'com.google.protobuf' java { sourceCompatibility = JavaVersion.VERSION_11 @@ -113,6 +114,7 @@ dependencies { implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("wpilibj") implementation wpilibTools.deps.wpilibJava("apriltag") + implementation wpilibTools.deps.wpilibJava("wpiunits") implementation wpilibTools.deps.wpilibOpenCvJava("frc" + wpi.frcYear.get(), wpi.versions.opencvVersion.get()) implementation group: "com.fasterxml.jackson.core", name: "jackson-annotations", version: wpi.versions.jacksonVersion.get() @@ -137,3 +139,27 @@ jacocoTestReport { html.required = true } } + +protobuf { + protoc { + artifact = 'com.google.protobuf:protoc:3.21.12' + } + plugins { + quickbuf { + artifact = 'us.hebi.quickbuf:protoc-gen-quickbuf:1.3.2' + } + } + generateProtoTasks { + all().configureEach { task -> + task.builtins { + cpp {} + remove java + } + task.plugins { + quickbuf { + option "gen_descriptors=true" + } + } + } + } +} diff --git a/shared/javacpp/setupBuild.gradle b/shared/javacpp/setupBuild.gradle index 66106c6057..a1f3c63711 100644 --- a/shared/javacpp/setupBuild.gradle +++ b/shared/javacpp/setupBuild.gradle @@ -38,11 +38,11 @@ model { sources { cpp { source { - srcDirs 'src/main/native/cpp' - include '**/*.cpp' + srcDirs 'src/main/native/cpp', "$buildDir/generated/source/proto/main/cpp" + include '**/*.cpp', '**/*.cc' } exportedHeaders { - srcDirs 'src/main/native/include' + srcDirs 'src/main/native/include', "$buildDir/generated/source/proto/main/cpp" if (project.hasProperty('generatedHeaders')) { srcDir generatedHeaders } @@ -51,8 +51,11 @@ model { } } - if(project.hasProperty('includePhotonTargeting')) { - binaries.all { + binaries.all { + it.tasks.withType(CppCompile) { + it.dependsOn generateProto + } + if(project.hasProperty('includePhotonTargeting')) { lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared' } } @@ -77,13 +80,16 @@ model { include '**/*.cpp' } exportedHeaders { - srcDirs 'src/test/native/include', 'src/main/native/cpp' + srcDirs 'src/test/native/include', "$buildDir/generated/source/proto/main/cpp" } } } - if(project.hasProperty('includePhotonTargeting')) { - binaries.all { + binaries.all { + it.tasks.withType(CppCompile) { + it.dependsOn generateProto + } + if(project.hasProperty('includePhotonTargeting')) { lib project: ':photon-targeting', library: 'photontargeting', linkage: 'shared' } }