diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java index 7f068409dc..e1bfaf8140 100644 --- 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(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 index 056fe3877a..6d85487789 100644 --- 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 + + "]"; + } }