Fix bugs with packing and unpacking

PhotonVision · Nov 6, 2023 · 7246a81 · 7246a81
1 parent d9c32de
commit 7246a81
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 8 deletions.
diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java
@@ -25,6 +25,7 @@
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.proto.PhotonTypes.ProtobufMultiTargetPNPResults;
 import us.hebi.quickbuf.Descriptors.Descriptor;
+import us.hebi.quickbuf.RepeatedInt;
 
 public class MultiTargetPNPResults {
     // Seeing 32 apriltags at once seems like a sane limit
@@ -125,12 +126,10 @@ public MultiTargetPNPResults unpack(ProtobufMultiTargetPNPResults msg) {
         public void pack(ProtobufMultiTargetPNPResults msg, MultiTargetPNPResults value) {
             PNPResults.proto.pack(msg.getMutableEstimatedPose(), value.estimatedPose);
 
-            // TODO better way of doing this
-            int[] ids = new int[value.fiducialIDsUsed.size()];
-            for (int i = 0; i < ids.length; i++) {
-                ids[i] = value.fiducialIDsUsed.get(i);
+            RepeatedInt idsUsed = msg.getMutableFiducialIdsUsed().reserve(value.fiducialIDsUsed.size());
+            for (int i = 0; i < value.fiducialIDsUsed.size(); i++) {
+                idsUsed.add(value.fiducialIDsUsed.get(i));
             }
-            msg.addAllFiducialIdsUsed(ids);
         }
     }
 

diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java b/photon-targeting/src/main/java/org/photonvision/targeting/PNPResults.java
@@ -189,6 +189,10 @@ public ProtobufPNPResults createMessage() {
 
         @Override
         public PNPResults unpack(ProtobufPNPResults msg) {
+            if (!msg.getIsPresent()) {
+                return new PNPResults();
+            }
+
             return new PNPResults(
                     Transform3d.proto.unpack(msg.getBest()),
                     Transform3d.proto.unpack(msg.getAlt()),
@@ -203,7 +207,8 @@ public void pack(ProtobufPNPResults msg, PNPResults value) {
             Transform3d.proto.pack(msg.getMutableAlt(), value.alt);
             msg.setAmbiguity(value.ambiguity)
                     .setBestReprojErr(value.bestReprojErr)
-                    .setAltReprojErr(value.altReprojErr);
+                    .setAltReprojErr(value.altReprojErr)
+                    .setIsPresent(value.isPresent);
         }
     }
 

diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonTrackedTarget.java
@@ -43,10 +43,10 @@ public class PhotonTrackedTarget {
     private double poseAmbiguity;
 
     // Corners from the min-area rectangle bounding the target
-    private List<TargetCorner> minAreaRectCorners;
+    private List<TargetCorner> minAreaRectCorners = List.of();
 
     // Corners from whatever corner detection method was used
-    private List<TargetCorner> detectedCorners;
+    private List<TargetCorner> detectedCorners = List.of();
 
     public PhotonTrackedTarget() {}
 
@@ -315,6 +315,10 @@ public ProtobufPhotonTrackedTarget createMessage() {
 
         @Override
         public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) {
+            if (msg.getMinAreaRectCorners().length() != 4) {
+                return new PhotonTrackedTarget();
+            }
+
             return new PhotonTrackedTarget(
                     msg.getYaw(),
                     msg.getPitch(),