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..0f668a417c 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; @@ -134,10 +133,8 @@ public void accept(CVPipelineResult result) { result.getLatencyMillis(), TrackedTarget.simpleFromTrackedTargets(result.targets), result.multiTagResult); - Packet packet = new Packet(simplified.getPacketSize()); - simplified.populatePacket(packet); - ts.rawBytesEntry.set(packet.getData()); + ts.resultPublisher.accept(simplified, simplified.getPacketSize()); ts.pipelineIndexPublisher.set(pipelineIndexSupplier.get()); ts.driverModePublisher.set(driverModeSupplier.getAsBoolean()); diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index faa03ebc54..c5a704460b 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -42,14 +42,13 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; 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.common.networktables.PacketSubscriber; import org.photonvision.targeting.PhotonPipelineResult; /** Represents a camera that is connected to PhotonVision. */ @@ -58,7 +57,7 @@ public class PhotonCamera implements AutoCloseable { public static final String kTableName = "photonvision"; private final NetworkTable cameraTable; - RawSubscriber rawBytesEntry; + PacketSubscriber resultSubscriber; BooleanPublisher driverModePublisher; BooleanSubscriber driverModeSubscriber; DoublePublisher latencyMillisEntry; @@ -78,7 +77,7 @@ public class PhotonCamera implements AutoCloseable { @Override public void close() { - rawBytesEntry.close(); + resultSubscriber.close(); driverModePublisher.close(); driverModeSubscriber.close(); latencyMillisEntry.close(); @@ -115,8 +114,6 @@ public static void setVersionCheckEnabled(boolean enabled) { VERSION_CHECK_ENABLED = enabled; } - Packet packet = new Packet(1); - /** * Constructs a PhotonCamera from a root table. * @@ -130,11 +127,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 = + var rawBytesEntry = cameraTable .getRawTopic("rawBytes") .subscribe( "rawBytes", new byte[] {}, PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); + resultSubscriber = + new PacketSubscriber<>( + rawBytesEntry, PhotonPipelineResult.serde, new PhotonPipelineResult()); driverModePublisher = cameraTable.getBooleanTopic("driverModeRequest").publish(); driverModeSubscriber = cameraTable.getBooleanTopic("driverMode").subscribe(false); inputSaveImgEntry = cameraTable.getIntegerTopic("inputSaveImgCmd").getEntry(0); @@ -177,21 +177,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 = resultSubscriber.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( + (resultSubscriber.subscriber.getLastChange() / 1e6) - ret.getLatencyMillis() / 1e3); // Return result. return ret; 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..e3efe971ca 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; @@ -564,9 +563,7 @@ public void submitProcessedFrame(PhotonPipelineResult result) { public void submitProcessedFrame(PhotonPipelineResult result, long receiveTimestamp) { ts.latencyMillisEntry.set(result.getLatencyMillis(), receiveTimestamp); - var newPacket = new Packet(result.getPacketSize()); - result.populatePacket(newPacket); - ts.rawBytesEntry.set(newPacket.getData(), receiveTimestamp); + ts.resultPublisher.accept(result, result.getPacketSize()); boolean hasTargets = result.hasTargets(); ts.hasTargetEntry.set(hasTargets, receiveTimestamp); 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..15df48ac90 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,8 @@ 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.resultPublisher.accept(newResult, newResult.getPacketSize()); boolean hasTargets = newResult.hasTargets(); ts.hasTargetEntry.set(hasTargets); 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 index 8a3fb5fad8..241fdc82f6 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java @@ -40,7 +40,7 @@ public void testEmpty() { if (packet.getSize() < 1) { return; } - ret.createFromPacket(packet); + PhotonPipelineResult.serde.pack(packet, ret); }); } } diff --git a/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java new file mode 100644 index 0000000000..cb2ae2ec76 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java @@ -0,0 +1,26 @@ +/* + * 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; + +public interface PacketSerde { + int getMaxByteSize(); + + void pack(Packet packet, T value); + + T unpack(Packet packet); +} 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..1fdf013477 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 @@ -27,7 +27,7 @@ import edu.wpi.first.networktables.IntegerTopic; import edu.wpi.first.networktables.NetworkTable; 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 +39,8 @@ */ public class NTTopicSet { public NetworkTable subTable; - public RawPublisher rawBytesEntry; + + public PacketPublisher resultPublisher; public IntegerPublisher pipelineIndexPublisher; public IntegerSubscriber pipelineIndexRequestSub; @@ -69,11 +70,13 @@ public class NTTopicSet { public DoubleArrayPublisher cameraDistortionPublisher; public void updateEntries() { - rawBytesEntry = + var rawBytesEntry = subTable .getRawTopic("rawBytes") .publish("rawBytes", PubSubOption.periodic(0.01), PubSubOption.sendAll(true)); + resultPublisher = new PacketPublisher<>(rawBytesEntry, PhotonPipelineResult.serde); + pipelineIndexPublisher = subTable.getIntegerTopic("pipelineIndexState").publish(); pipelineIndexRequestSub = subTable.getIntegerTopic("pipelineIndexRequest").subscribe(0); @@ -104,7 +107,7 @@ public void updateEntries() { @SuppressWarnings("DuplicatedCode") public void removeEntries() { - if (rawBytesEntry != null) rawBytesEntry.close(); + if (resultPublisher != null) resultPublisher.close(); if (pipelineIndexPublisher != null) pipelineIndexPublisher.close(); if (pipelineIndexRequestSub != null) pipelineIndexRequestSub.close(); diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java new file mode 100644 index 0000000000..135726d899 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java @@ -0,0 +1,47 @@ +/* + * 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.networktables; + +import edu.wpi.first.networktables.RawPublisher; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.dataflow.structures.PacketSerde; + +public class PacketPublisher implements AutoCloseable { + public final RawPublisher publisher; + private final PacketSerde serde; + + public PacketPublisher(RawPublisher publisher, PacketSerde serde) { + this.publisher = publisher; + this.serde = serde; + } + + public void accept(T value, int byteSize) { + var packet = new Packet(byteSize); + serde.pack(packet, value); + publisher.set(packet.getData()); + } + + public void accept(T value) { + accept(value, serde.getMaxByteSize()); + } + + @Override + public void close() { + publisher.close(); + } +} diff --git a/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java new file mode 100644 index 0000000000..8cc9f44545 --- /dev/null +++ b/photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java @@ -0,0 +1,50 @@ +/* + * 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.networktables; + +import edu.wpi.first.networktables.RawSubscriber; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.dataflow.structures.PacketSerde; + +public class PacketSubscriber implements AutoCloseable { + public final RawSubscriber subscriber; + private final PacketSerde serde; + private final T defaultValue; + + private final Packet packet = new Packet(1); + + public PacketSubscriber(RawSubscriber subscriber, PacketSerde serde, T defaultValue) { + this.subscriber = subscriber; + this.serde = serde; + this.defaultValue = defaultValue; + } + + public T get() { + packet.clear(); + + packet.setData(subscriber.get(new byte[] {})); + if (packet.getSize() < 1) return defaultValue; + + return serde.unpack(packet); + } + + @Override + public void close() { + subscriber.close(); + } +} 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..03a45cb562 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResult.java @@ -20,12 +20,11 @@ import java.util.ArrayList; import java.util.List; import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.dataflow.structures.PacketSerde; 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,39 @@ public String toString() { + fiducialIDsUsed + "]"; } + + public static final class APacketSerde implements PacketSerde { + @Override + public int getMaxByteSize() { + // PNPResult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, + // ideally) + return PNPResult.serde.getMaxByteSize() + (Short.BYTES * MAX_IDS); + } + + @Override + public void pack(Packet packet, MultiTargetPNPResult result) { + PNPResult.serde.pack(packet, result.estimatedPose); + + for (int i = 0; i < MAX_IDS; i++) { + if (i < result.fiducialIDsUsed.size()) { + packet.encode((short) result.fiducialIDsUsed.get(i).byteValue()); + } else { + packet.encode((short) -1); + } + } + } + + @Override + public MultiTargetPNPResult unpack(Packet packet) { + var results = PNPResult.serde.unpack(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 static final APacketSerde serde = new APacketSerde(); } 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..6e4b595c41 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResult.java @@ -19,6 +19,7 @@ import edu.wpi.first.math.geometry.Transform3d; import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.dataflow.structures.PacketSerde; import org.photonvision.utils.PacketUtils; /** @@ -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,42 @@ public String toString() { + ambiguity + "]"; } + + public static final class APacketSerde implements PacketSerde { + @Override + public int getMaxByteSize() { + return 1 + (Double.BYTES * 7 * 2) + (Double.BYTES * 3); + } + + @Override + public void pack(Packet packet, PNPResult value) { + packet.encode(value.isPresent); + + if (value.isPresent) { + PacketUtils.packTransform3d(packet, value.best); + PacketUtils.packTransform3d(packet, value.alt); + packet.encode(value.bestReprojErr); + packet.encode(value.altReprojErr); + packet.encode(value.ambiguity); + } + } + + @Override + public PNPResult unpack(Packet packet) { + var present = packet.decodeBoolean(); + + if (!present) { + return new PNPResult(); + } + + var best = PacketUtils.unpackTransform3d(packet); + var alt = PacketUtils.unpackTransform3d(packet); + var bestEr = packet.decodeDouble(); + var altEr = packet.decodeDouble(); + var ambiguity = packet.decodeDouble(); + return new PNPResult(best, alt, ambiguity, bestEr, altEr); + } + } + + public static final APacketSerde serde = new APacketSerde(); } 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 7047ee46aa..d55d4813e7 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -20,6 +20,7 @@ import java.util.ArrayList; import java.util.List; import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.dataflow.structures.PacketSerde; /** Represents a pipeline result from a PhotonCamera. */ public class PhotonPipelineResult { @@ -71,10 +72,10 @@ public PhotonPipelineResult( * @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 + return Double.BYTES // latency + + 1 // target count + + targets.size() * PhotonTrackedTarget.serde.getMaxByteSize() + + MultiTargetPNPResult.serde.getMaxByteSize(); } /** @@ -150,49 +151,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 +194,35 @@ public String toString() { + multiTagResult + "]"; } + + public static final class APacketSerde implements PacketSerde { + @Override + public int getMaxByteSize() { + // This uses dynamic packets so it doesn't matter + return -1; + } + + @Override + public void pack(Packet packet, PhotonPipelineResult value) { + packet.encode(value.latencyMillis); + packet.encode((byte) value.targets.size()); + for (var target : value.targets) PhotonTrackedTarget.serde.pack(packet, target); + MultiTargetPNPResult.serde.pack(packet, value.multiTagResult); + } + + @Override + public PhotonPipelineResult unpack(Packet packet) { + var latency = packet.decodeDouble(); + var len = packet.decodeByte(); + var targets = new ArrayList(len); + for (int i = 0; i < len; i++) { + targets.add(PhotonTrackedTarget.serde.unpack(packet)); + } + var result = MultiTargetPNPResult.serde.unpack(packet); + + return new PhotonPipelineResult(latency, targets, result); + } + } + + public static final APacketSerde serde = new APacketSerde(); } 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..560fafd417 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java @@ -21,29 +21,26 @@ import java.util.ArrayList; import java.util.List; import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.dataflow.structures.PacketSerde; import org.photonvision.utils.PacketUtils; 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; + 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 List minAreaRectCorners; + 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,80 @@ public String toString() { + minAreaRectCorners + '}'; } + + public static final class APacketSerde implements PacketSerde { + @Override + public int getMaxByteSize() { + return Double.BYTES * (5 + 7 + 2 * 4 + 1 + 7 + 2 * MAX_CORNERS); + } + + @Override + public void pack(Packet packet, PhotonTrackedTarget value) { + packet.encode(value.yaw); + packet.encode(value.pitch); + packet.encode(value.area); + packet.encode(value.skew); + packet.encode(value.fiducialId); + if (value.fiducialId != -1) { + PacketUtils.packTransform3d(packet, value.bestCameraToTarget); + PacketUtils.packTransform3d(packet, value.altCameraToTarget); + packet.encode(value.poseAmbiguity); + } + + for (int i = 0; i < 4; i++) { + TargetCorner.serde.pack(packet, value.minAreaRectCorners.get(i)); + } + + packet.encode((byte) Math.min(value.detectedCorners.size(), Byte.MAX_VALUE)); + for (TargetCorner targetCorner : value.detectedCorners) { + TargetCorner.serde.pack(packet, targetCorner); + } + } + + @Override + public PhotonTrackedTarget unpack(Packet packet) { + var yaw = packet.decodeDouble(); + var pitch = packet.decodeDouble(); + var area = packet.decodeDouble(); + var skew = packet.decodeDouble(); + var fiducialId = packet.decodeInt(); + Transform3d best; + Transform3d alt; + double ambiguity; + if (fiducialId != -1.0) { + best = PacketUtils.unpackTransform3d(packet); + alt = PacketUtils.unpackTransform3d(packet); + ambiguity = packet.decodeDouble(); + } else { + best = new Transform3d(); + alt = new Transform3d(); + ambiguity = -1.0; + } + + var minAreaRectCorners = new ArrayList(4); + for (int i = 0; i < 4; i++) { + minAreaRectCorners.add(TargetCorner.serde.unpack(packet)); + } + + var len = packet.decodeByte(); + var detectedCorners = new ArrayList(len); + for (int i = 0; i < len; i++) { + detectedCorners.add(TargetCorner.serde.unpack(packet)); + } + + return new PhotonTrackedTarget( + yaw, + pitch, + area, + skew, + fiducialId, + best, + alt, + ambiguity, + minAreaRectCorners, + detectedCorners); + } + } + + public static final APacketSerde serde = new APacketSerde(); } 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..38cc489594 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/TargetCorner.java @@ -18,6 +18,8 @@ package org.photonvision.targeting; import java.util.Objects; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.common.dataflow.structures.PacketSerde; /** * Represents a point in an image at the corner of the minimum-area bounding rectangle, in pixels. @@ -49,4 +51,24 @@ public int hashCode() { public String toString() { return "(" + x + "," + y + ')'; } + + public static final class APacketSerde implements PacketSerde { + @Override + public int getMaxByteSize() { + return Double.BYTES * 2; + } + + @Override + public void pack(Packet packet, TargetCorner corner) { + packet.encode(corner.x); + packet.encode(corner.y); + } + + @Override + public TargetCorner unpack(Packet packet) { + return new TargetCorner(packet.decodeDouble(), packet.decodeDouble()); + } + } + + public static final APacketSerde serde = new APacketSerde(); } diff --git a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java index 43a0dd4cf6..b90de13539 100644 --- a/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java +++ b/photon-targeting/src/main/java/org/photonvision/utils/PacketUtils.java @@ -17,33 +17,100 @@ 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 edu.wpi.first.math.geometry.*; 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()); + public static final int ROTATION2D_BYTE_SIZE = Double.BYTES; + public static final int QUATERNION_BYTE_SIZE = Double.BYTES * 4; + public static final int ROTATION3D_BYTE_SIZE = QUATERNION_BYTE_SIZE; + public static final int TRANSLATION2D_BYTE_SIZE = Double.BYTES * 2; + public static final int TRANSLATION3D_BYTE_SIZE = Double.BYTES * 3; + public static final int TRANSFORM2D_BYTE_SIZE = TRANSLATION2D_BYTE_SIZE + ROTATION2D_BYTE_SIZE; + public static final int TRANSFORM3D_BYTE_SIZE = TRANSLATION3D_BYTE_SIZE + ROTATION3D_BYTE_SIZE; + public static final int POSE2D_BYTE_SIZE = TRANSLATION2D_BYTE_SIZE + ROTATION2D_BYTE_SIZE; + public static final int POSE3D_BYTE_SIZE = TRANSLATION3D_BYTE_SIZE + ROTATION3D_BYTE_SIZE; + + public static void packRotation2d(Packet packet, Rotation2d rotation) { + packet.encode(rotation.getRadians()); + } + + public static Rotation2d unpackRotation2d(Packet packet) { + return new Rotation2d(packet.decodeDouble()); + } + + public static void packQuaternion(Packet packet, Quaternion quaternion) { + packet.encode(quaternion.getW()); + packet.encode(quaternion.getX()); + packet.encode(quaternion.getY()); + packet.encode(quaternion.getZ()); + } + + public static Quaternion unpackQuaternion(Packet packet) { + return new Quaternion( + packet.decodeDouble(), packet.decodeDouble(), packet.decodeDouble(), packet.decodeDouble()); + } + + public static void packRotation3d(Packet packet, Rotation3d rotation) { + packQuaternion(packet, rotation.getQuaternion()); + } + + public static Rotation3d unpackRotation3d(Packet packet) { + return new Rotation3d(unpackQuaternion(packet)); + } + + public static void packTranslation2d(Packet packet, Translation2d translation) { + packet.encode(translation.getX()); + packet.encode(translation.getY()); + } + + public static Translation2d unpackTranslation2d(Packet packet) { + return new Translation2d(packet.decodeDouble(), packet.decodeDouble()); + } + + public static void packTranslation3d(Packet packet, Translation3d translation) { + packet.encode(translation.getX()); + packet.encode(translation.getY()); + packet.encode(translation.getZ()); + } + + public static Translation3d unpackTranslation3d(Packet packet) { + return new Translation3d(packet.decodeDouble(), packet.decodeDouble(), packet.decodeDouble()); + } + + public static void packTransform2d(Packet packet, Transform2d transform) { + packTranslation2d(packet, transform.getTranslation()); + packRotation2d(packet, transform.getRotation()); + } + + public static Transform2d unpackTransform2d(Packet packet) { + return new Transform2d(unpackTranslation2d(packet), unpackRotation2d(packet)); + } + + public static void packTransform3d(Packet packet, Transform3d transform) { + packTranslation3d(packet, transform.getTranslation()); + packRotation3d(packet, transform.getRotation()); + } + + public static Transform3d unpackTransform3d(Packet packet) { + return new Transform3d(unpackTranslation3d(packet), unpackRotation3d(packet)); + } + + public static void packPose2d(Packet packet, Pose2d pose) { + packTranslation2d(packet, pose.getTranslation()); + packRotation2d(packet, pose.getRotation()); + } + + public static Pose2d unpackPose2d(Packet packet) { + return new Pose2d(unpackTranslation2d(packet), unpackRotation2d(packet)); + } + + public static void packPose3d(Packet packet, Pose3d pose) { + packTranslation3d(packet, pose.getTranslation()); + packRotation3d(packet, pose.getRotation()); + } + + public static Pose3d unpackPose3d(Packet packet) { + return new Pose3d(unpackTranslation3d(packet), unpackRotation3d(packet)); } } diff --git a/photon-targeting/src/test/java/org/photonvision/PacketTest.java b/photon-targeting/src/test/java/org/photonvision/PacketTest.java new file mode 100644 index 0000000000..3885f38101 --- /dev/null +++ b/photon-targeting/src/test/java/org/photonvision/PacketTest.java @@ -0,0 +1,337 @@ +/* + * 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; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.geometry.*; +import java.util.List; +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; +import org.photonvision.utils.PacketUtils; + +class PacketTest { + @Test + void rotation2dSerde() { + var packet = new Packet(PacketUtils.ROTATION2D_BYTE_SIZE); + var ret = new Rotation2d(); + PacketUtils.packRotation2d(packet, ret); + var unpacked = PacketUtils.unpackRotation2d(packet); + assertEquals(ret, unpacked); + } + + @Test + void quaternionSerde() { + var packet = new Packet(PacketUtils.QUATERNION_BYTE_SIZE); + var ret = new Quaternion(); + PacketUtils.packQuaternion(packet, ret); + var unpacked = PacketUtils.unpackQuaternion(packet); + assertEquals(ret, unpacked); + } + + @Test + void rotation3dSerde() { + var packet = new Packet(PacketUtils.ROTATION3D_BYTE_SIZE); + var ret = new Rotation3d(); + PacketUtils.packRotation3d(packet, ret); + var unpacked = PacketUtils.unpackRotation3d(packet); + assertEquals(ret, unpacked); + } + + @Test + void translation2dSerde() { + var packet = new Packet(PacketUtils.TRANSLATION2D_BYTE_SIZE); + var ret = new Translation2d(); + PacketUtils.packTranslation2d(packet, ret); + var unpacked = PacketUtils.unpackTranslation2d(packet); + assertEquals(ret, unpacked); + } + + @Test + void translation3dSerde() { + var packet = new Packet(PacketUtils.TRANSLATION3D_BYTE_SIZE); + var ret = new Translation3d(); + PacketUtils.packTranslation3d(packet, ret); + var unpacked = PacketUtils.unpackTranslation3d(packet); + assertEquals(ret, unpacked); + } + + @Test + void transform2dSerde() { + var packet = new Packet(PacketUtils.TRANSFORM2D_BYTE_SIZE); + var ret = new Transform2d(); + PacketUtils.packTransform2d(packet, ret); + var unpacked = PacketUtils.unpackTransform2d(packet); + assertEquals(ret, unpacked); + } + + @Test + void transform3dSerde() { + var packet = new Packet(PacketUtils.TRANSFORM3D_BYTE_SIZE); + var ret = new Transform3d(); + PacketUtils.packTransform3d(packet, ret); + var unpacked = PacketUtils.unpackTransform3d(packet); + assertEquals(ret, unpacked); + } + + @Test + void pose2dSerde() { + var packet = new Packet(PacketUtils.POSE2D_BYTE_SIZE); + var ret = new Pose2d(); + PacketUtils.packPose2d(packet, ret); + var unpacked = PacketUtils.unpackPose2d(packet); + assertEquals(ret, unpacked); + } + + @Test + void pose3dSerde() { + var packet = new Packet(PacketUtils.POSE3D_BYTE_SIZE); + var ret = new Pose3d(); + PacketUtils.packPose3d(packet, ret); + var unpacked = PacketUtils.unpackPose3d(packet); + assertEquals(ret, unpacked); + } + + @Test + void targetCornerSerde() { + var packet = new Packet(TargetCorner.serde.getMaxByteSize()); + var ret = new TargetCorner(0.0, 1.0); + TargetCorner.serde.pack(packet, ret); + var unpacked = TargetCorner.serde.unpack(packet); + assertEquals(ret, unpacked); + } + + @Test + void pnpResultSerde() { + var packet = new Packet(PNPResult.serde.getMaxByteSize()); + var ret = new PNPResult(); + PNPResult.serde.pack(packet, ret); + var unpackedRet = PNPResult.serde.unpack(packet); + assertEquals(ret, unpackedRet); + + var packet1 = new Packet(PNPResult.serde.getMaxByteSize()); + var ret1 = + new PNPResult(new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1); + PNPResult.serde.pack(packet1, ret1); + var unpackedRet1 = PNPResult.serde.unpack(packet1); + assertEquals(ret1, unpackedRet1); + } + + @Test + void multitagResultSerde() { + var packet = new Packet(MultiTargetPNPResult.serde.getMaxByteSize()); + var ret = + new MultiTargetPNPResult( + new PNPResult( + new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)), 0.1), + List.of(1, 2, 3)); + MultiTargetPNPResult.serde.pack(packet, ret); + var unpackedRet = MultiTargetPNPResult.serde.unpack(packet); + assertEquals(ret, unpackedRet); + } + + @Test + void trackedTargetSerde() { + var packet = new Packet(PhotonTrackedTarget.serde.getMaxByteSize()); + var ret = + new PhotonTrackedTarget( + 3.0, + 4.0, + 9.0, + -5.0, + -1, + new Transform3d(), + new Transform3d(), + -1, + 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))); + PhotonTrackedTarget.serde.pack(packet, ret); + var unpacked = PhotonTrackedTarget.serde.unpack(packet); + assertEquals(ret, unpacked); + } + + @Test + void pipelineResultSerde() { + var ret1 = new PhotonPipelineResult(1, List.of()); + var p1 = new Packet(ret1.getPacketSize()); + PhotonPipelineResult.serde.pack(p1, ret1); + var unpackedRet1 = PhotonPipelineResult.serde.unpack(p1); + assertEquals(ret1, unpackedRet1); + + var ret2 = + 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(ret2.getPacketSize()); + PhotonPipelineResult.serde.pack(p2, ret2); + var unpackedRet2 = PhotonPipelineResult.serde.unpack(p2); + assertEquals(ret2, unpackedRet2); + + var ret3 = + 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))); + var p3 = new Packet(ret3.getPacketSize()); + PhotonPipelineResult.serde.pack(p3, ret3); + var unpackedRet3 = PhotonPipelineResult.serde.unpack(p3); + assertEquals(ret3, unpackedRet3); + } + + @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))); + } +}