Only encode positive IDs

PhotonVision · Aug 30, 2023 · e5a1eea · e5a1eea
1 parent 1d22a23
commit e5a1eea
Show file tree

Hide file tree

Showing 2 changed files with 48 additions and 1 deletion.
diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java
@@ -40,7 +40,8 @@ public static MultiTargetPNPResults createFromPacket(Packet packet) {
         var results = PNPResults.createFromPacket(packet);
         var ids = new ArrayList<Integer>(MAX_IDS);
         for (int i = 0; i < MAX_IDS; i++) {
-            ids.add((int) packet.decodeByte());
+            int targetId = (int) packet.decodeByte();
+            if (targetId > Byte.MIN_VALUE) ids.add(targetId);
         }
         return new MultiTargetPNPResults(results, ids);
     }
@@ -55,4 +56,37 @@ public void populatePacket(Packet packet) {
             }
         }
     }
+
+    @Override
+    public int hashCode() {
+        final int prime = 31;
+        int result = 1;
+        result = prime * result + ((estimatedPose == null) ? 0 : estimatedPose.hashCode());
+        result = prime * result + ((fiducialIDsUsed == null) ? 0 : fiducialIDsUsed.hashCode());
+        return result;
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (obj == null) return false;
+        if (getClass() != obj.getClass()) return false;
+        MultiTargetPNPResults other = (MultiTargetPNPResults) obj;
+        if (estimatedPose == null) {
+            if (other.estimatedPose != null) return false;
+        } else if (!estimatedPose.equals(other.estimatedPose)) return false;
+        if (fiducialIDsUsed == null) {
+            if (other.fiducialIDsUsed != null) return false;
+        } else if (!fiducialIDsUsed.equals(other.fiducialIDsUsed)) return false;
+        return true;
+    }
+
+    @Override
+    public String toString() {
+        return "MultiTargetPNPResults [estimatedPose="
+                + estimatedPose
+                + ", fiducialIDsUsed="
+                + fiducialIDsUsed
+                + "]";
+    }
 }
diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java
@@ -225,4 +225,17 @@ public boolean equals(Object obj) {
         } else if (!multiTagResult.equals(other.multiTagResult)) return false;
         return true;
     }
+
+    @Override
+    public String toString() {
+        return "PhotonPipelineResult [targets="
+                + targets
+                + ", latencyMillis="
+                + latencyMillis
+                + ", timestampSeconds="
+                + timestampSeconds
+                + ", multiTagResult="
+                + multiTagResult
+                + "]";
+    }
 }