Multi-tag pnp in robot code (#787)

---------

Co-authored-by: Banks Troutman <btrout.dhrs@gmail.com>
Co-authored-by: Joseph Farkas <16584585+MrRedness@users.noreply.github.com>

photon-targeting/src/main/java/org/photonvision/common/dataflow/structures/Packet.java

@@ -182,4 +182,18 @@ public class Packet {
         }
         return packetData[readPos++] == 1;
     }
+
+    public void encode(double[] data) {
+        for (double d : data) {
+            encode(d);
+        }
+    }
+
+    public double[] decodeDoubleArray(int len) {
+        double[] ret = new double[len];
+        for (int i = 0; i < len; i++) {
+            ret[i] = decodeDouble();
+        }
+        return ret;
+    }
 }

photon-targeting/src/main/java/org/photonvision/common/networktables/NTTopicSet.java

@@ -64,6 +64,10 @@ public class NTTopicSet {
     public IntegerTopic heartbeatTopic;
     public IntegerPublisher heartbeatPublisher;
 
+    // Camera Calibration
+    public DoubleArrayPublisher cameraIntrinsicsPublisher;
+    public DoubleArrayPublisher cameraDistortionPublisher;
+
     public void updateEntries() {
         rawBytesEntry =
                 subTable
@@ -93,6 +97,9 @@ public class NTTopicSet {
 
         heartbeatTopic = subTable.getIntegerTopic("heartbeat");
         heartbeatPublisher = heartbeatTopic.publish();
+
+        cameraIntrinsicsPublisher = subTable.getDoubleArrayTopic("cameraIntrinsics").publish();
+        cameraDistortionPublisher = subTable.getDoubleArrayTopic("cameraDistortion").publish();
     }
 
     @SuppressWarnings("DuplicatedCode")
@@ -113,5 +120,10 @@ public class NTTopicSet {
         if (targetSkewEntry != null) targetSkewEntry.close();
         if (bestTargetPosX != null) bestTargetPosX.close();
         if (bestTargetPosY != null) bestTargetPosY.close();
+
+        if (heartbeatPublisher != null) heartbeatPublisher.close();
+
+        if (cameraIntrinsicsPublisher != null) cameraIntrinsicsPublisher.close();
+        if (cameraDistortionPublisher != null) cameraDistortionPublisher.close();
     }
 }

photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java

@@ -19,7 +19,6 @@ package org.photonvision.targeting;
 
 import java.util.ArrayList;
 import java.util.List;
-import java.util.Objects;
 import org.photonvision.common.dataflow.structures.Packet;
 
 /** Represents a pipeline result from a PhotonCamera. */
@@ -123,21 +122,6 @@ public class PhotonPipelineResult {
         return new ArrayList<>(targets);
     }
 
-    @Override
-    public boolean equals(Object o) {
-        if (this == o) return true;
-        if (o == null || getClass() != o.getClass()) return false;
-        PhotonPipelineResult that = (PhotonPipelineResult) o;
-        boolean latencyMatch = Double.compare(that.latencyMillis, latencyMillis) == 0;
-        boolean targetsMatch = that.targets.equals(targets);
-        return latencyMatch && targetsMatch;
-    }
-
-    @Override
-    public int hashCode() {
-        return Objects.hash(latencyMillis, targets);
-    }
-
     /**
      * Populates the fields of the pipeline result from the packet.
      *
@@ -178,4 +162,33 @@ public class PhotonPipelineResult {
         // Return the packet.
         return packet;
     }
+
+    @Override
+    public int hashCode() {
+        final int prime = 31;
+        int result = 1;
+        result = prime * result + ((targets == null) ? 0 : targets.hashCode());
+        long temp;
+        temp = Double.doubleToLongBits(latencyMillis);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        temp = Double.doubleToLongBits(timestampSeconds);
+        result = prime * result + (int) (temp ^ (temp >>> 32));
+        return result;
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (obj == null) return false;
+        if (getClass() != obj.getClass()) return false;
+        PhotonPipelineResult other = (PhotonPipelineResult) obj;
+        if (targets == null) {
+            if (other.targets != null) return false;
+        } else if (!targets.equals(other.targets)) return false;
+        if (Double.doubleToLongBits(latencyMillis) != Double.doubleToLongBits(other.latencyMillis))
+            return false;
+        if (Double.doubleToLongBits(timestampSeconds)
+                != Double.doubleToLongBits(other.timestampSeconds)) return false;
+        return true;
+    }
 }