Add PacketSerde interface and expand PacketUtils for more wpimath classes

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());

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<PhotonPipelineResult> 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;

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);

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);

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);
                 });
     }
 }

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 <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.dataflow.structures;
+
+public interface PacketSerde<T> {
+    public int getMaxByteSize();
+
+    void pack(Packet packet, T value);
+
+    T unpack(Packet packet);
+}