From 5be9b8be2c1032454cc9dd9f94e4a25d25822428 Mon Sep 17 00:00:00 2001 From: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com> Date: Sun, 24 Dec 2023 19:56:08 -0500 Subject: [PATCH] Add PacketSerde interface and expand PacketUtils for more wpimath classes (#1058) Follows a similar system to the current Protobuf implementation that helps make code more readable and expandable. Also wraps the NT topic to be more useful. Impl stuff is hidden so it can't be extended. Optimizes AT-specific classes by only serializing data when needed, won't save on size but will on time. closes #1003 --- .../networktables/NTDataPublisher.java | 5 +- .../java/org/photonvision/PhotonCamera.java | 29 +- .../simulation/PhotonCameraSim.java | 5 +- .../simulation/SimPhotonCamera.java | 6 +- .../java/org/photonvision/PacketTest.java | 190 ---------- .../org/photonvision/PhotonCameraTest.java | 2 +- .../dataflow/structures/PacketSerde.java | 26 ++ .../common/networktables/NTTopicSet.java | 11 +- .../common/networktables/PacketPublisher.java | 47 +++ .../networktables/PacketSubscriber.java | 50 +++ .../targeting/MultiTargetPNPResult.java | 59 +-- .../org/photonvision/targeting/PNPResult.java | 64 ++-- .../targeting/PhotonPipelineResult.java | 83 ++--- .../targeting/PhotonTrackedTarget.java | 176 +++++---- .../photonvision/targeting/TargetCorner.java | 22 ++ .../org/photonvision/utils/PacketUtils.java | 113 ++++-- .../java/org/photonvision/PacketTest.java | 337 ++++++++++++++++++ 17 files changed, 792 insertions(+), 433 deletions(-) delete mode 100644 photon-lib/src/test/java/org/photonvision/PacketTest.java create mode 100644 photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/PacketSerde.java create mode 100644 photon-targeting/src/main/java/org/photonvision/common/networktables/PacketPublisher.java create mode 100644 photon-targeting/src/main/java/org/photonvision/common/networktables/PacketSubscriber.java create mode 100644 photon-targeting/src/test/java/org/photonvision/PacketTest.java 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 d985c710f..0f668a417 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.BooleanSupplier; 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 class NTDataPublisher implements CVPipelineResultConsumer { 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 faa03ebc5..c5a704460 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.MultiSubscriber; 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 class PhotonCamera implements AutoCloseable { VERSION_CHECK_ENABLED = enabled; } - Packet packet = new Packet(1); - /** * Constructs a PhotonCamera from a root table. * @@ -130,11 +127,14 @@ public class PhotonCamera implements AutoCloseable { 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 class PhotonCamera implements AutoCloseable { 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 b602b0194..e3efe971c 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.core.Size; 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 class PhotonCameraSim implements AutoCloseable { 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 5eb73b35e..15df48ac9 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.Arrays; 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 class SimPhotonCamera { 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 a2f98a27c..000000000 --- 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 8a3fb5fad..241fdc82f 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 @@ class PhotonCameraTest { 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 000000000..cb2ae2ec7 --- /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 5a0d31eae..1fdf01347 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.IntegerSubscriber; 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 @@ import edu.wpi.first.networktables.RawPublisher; */ 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 class NTTopicSet { @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 000000000..135726d89 --- /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 000000000..8cc9f4454 --- /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 a3ddaaa6f..03a45cb56 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 @@ package org.photonvision.targeting; 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 class MultiTargetPNPResult { 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 class MultiTargetPNPResult { + 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 ab7aa5f46..6e4b595c4 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 @@ package org.photonvision.targeting; 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 class 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 class PNPResult { + 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 7047ee46a..d55d4813e 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 @@ package org.photonvision.targeting; 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 class 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 class PhotonPipelineResult { 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 class PhotonPipelineResult { + 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 392a42900..560fafd41 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 edu.wpi.first.math.geometry.Transform3d; 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 class PhotonTrackedTarget { 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 class PhotonTrackedTarget { + 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 9808079a5..38cc48959 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 class TargetCorner { 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 43a0dd4cf..b90de1353 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 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 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 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 000000000..3885f3810 --- /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))); + } +}