Additions to Fix Crashes & Improve Code

lib/build.gradle

@@ -29,15 +29,15 @@ dependencies {
 
     // This dependency is used internally, and not exposed to consumers on their own compile classpath.
     implementation 'com.google.guava:guava:30.1.1-jre'
-    implementation 'edu.wpi.first.cscore:cscore-java:2023.2.1'
+    implementation 'edu.wpi.first.cscore:cscore-java:2023.+'
-    implementation 'edu.wpi.first.cameraserver:cameraserver-java:2023.2.1'
+    implementation 'edu.wpi.first.cameraserver:cameraserver-java:2023.+'
-    implementation 'edu.wpi.first.ntcore:ntcore-java:2023.2.1'
+    implementation 'edu.wpi.first.ntcore:ntcore-java:2023.+'
-    implementation 'edu.wpi.first.wpilibj:wpilibj-java:2023.2.1'
+    implementation 'edu.wpi.first.wpilibj:wpilibj-java:2023.+'
-    implementation 'edu.wpi.first.wpiutil:wpiutil-java:2023.2.1'
+    implementation 'edu.wpi.first.wpiutil:wpiutil-java:2023.+'
-    implementation 'edu.wpi.first.wpimath:wpimath-java:2023.2.1'
+    implementation 'edu.wpi.first.wpimath:wpimath-java:2023.+'
-    implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2023.2.1'
+    implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2023.+'
-    implementation 'edu.wpi.first.hal:hal-java:2023.2.1'
+    implementation 'edu.wpi.first.hal:hal-java:2023.+'
-    implementation "edu.wpi.first.apriltag:apriltag-java:2023.2.1"
+    implementation "edu.wpi.first.apriltag:apriltag-java:2023.+"
     implementation "org.ejml:ejml-simple:0.38"
     implementation "com.fasterxml.jackson.core:jackson-annotations:2.10.0"
     implementation "com.fasterxml.jackson.core:jackson-core:2.10.0"
@@ -45,8 +45,8 @@ dependencies {
     implementation 'edu.wpi.first.thirdparty.frc2022.opencv:opencv-java:4.5.2-1'
     implementation 'com.ctre.phoenix:api-java:5.30.3'
     implementation 'com.ctre.phoenix:wpiapi-java:5.30.3'
-    implementation "org.photonvision:PhotonLib-java:v2023.2.1"
+    implementation "org.photonvision:PhotonLib-java:v2023.+"
-    implementation "org.photonvision:PhotonTargeting-java:v2023.2.1"
+    implementation "org.photonvision:PhotonTargeting-java:v2023.+"
 }
 
 tasks.named('test') {

lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java

@@ -8,9 +8,11 @@ import org.photonvision.targeting.PhotonTrackedTarget;
 
 import edu.wpi.first.math.geometry.Transform3d;
 
-public class AprilTags { 
+public class AprilTags implements Cloneable { 
     // an ArrayList to store the cameras
     public final ArrayList<PhotonCamera> cameras = new ArrayList<>();
+    public final ArrayList<PhotonTrackedTarget> frozenResults = new ArrayList<>();
+    public boolean frozen = false;
 
     public AprilTags(String...cameraNames) {
         for(String cameraName : cameraNames) {
@@ -18,18 +20,23 @@ public class AprilTags {
         }
     }
 
-    // Method to get a list of all targets from all cameras
+    //Method to get a list of all targets without any filtering
+    public List<PhotonTrackedTarget> getAllTargets() {
+        return frozen ? frozenResults : cameras.stream().flatMap((camera) -> camera.getLatestResult().getTargets().stream()).toList();
+    }
+
+    // Method to get a list of all targets with duplicates removed
     public List<PhotonTrackedTarget> getTargets() {
         List<PhotonTrackedTarget> targets = new ArrayList<>();
 
-        for(PhotonCamera camera : cameras) {
+        for(PhotonTrackedTarget target : getAllTargets()) {
-            for(PhotonTrackedTarget target : camera.getLatestResult().getTargets()) {
             boolean toAdd = true;
+            List<PhotonTrackedTarget> tempTargets = targets;
             // check if the target is already in the list and if it has a higher pose ambiguity
             for(PhotonTrackedTarget arTarget : targets) {
                 if(target.getFiducialId() == arTarget.getFiducialId() && target.getPoseAmbiguity() < arTarget.getPoseAmbiguity()) {
                     // if it does, remove it from the list
-                        targets.remove(arTarget);
+                    tempTargets.remove(arTarget);
                     break;
                 } else if(target.getFiducialId() == arTarget.getFiducialId()) {
                     // if the target is already in the list and has the same ID, don't add it
@@ -37,47 +44,71 @@ public class AprilTags {
                     break;
                 }
             }
+            if(toAdd) { tempTargets.add(target); }
+            targets = tempTargets;
+        }
         
-                if(toAdd) targets.add(target);
-            }
-            targets.addAll(camera.getLatestResult().getTargets());
-        }
 
         return targets;
     }
 
     // method to check if any camera has any targets
     public boolean hasTargets() {
-        for(PhotonCamera camera : cameras) {
+        return getAllTargets().size() != 0;
-            // if a camera has targets, return true
-            if(camera.getLatestResult().hasTargets()) return true;
-        } 
-        
-        return false;
     }      
 
     // method to get the closest target
     public PhotonTrackedTarget getClosestTarget() {
-        if(!hasTargets()) return null;
+        if(!hasTargets()) { return null; }
         PhotonTrackedTarget closestTarget = null;
         for(PhotonTrackedTarget target : getTargets()) {
-            if(closestTarget == null || calculateDistance(target.getBestCameraToTarget()) > calculateDistance(target.getBestCameraToTarget()) ) closestTarget = target;
+            if(closestTarget == null || calculateDistance(closestTarget.getBestCameraToTarget()) > calculateDistance(target.getBestCameraToTarget())) { closestTarget = target; }
         }
         return closestTarget;
     }
 
     // method to get the target with the lowest pose ambiguity
     public PhotonTrackedTarget getLowestAmbiguityTarget() {
-        if(!hasTargets()) return null;
+        if(!hasTargets()) { return null; }
         PhotonTrackedTarget lowestAmbiguity = null;
         for(PhotonTrackedTarget target : getTargets()) {
             // check if the target has a lower pose ambiguity than the current lowest ambiguity target
-            if(lowestAmbiguity == null || lowestAmbiguity.getPoseAmbiguity() > target.getPoseAmbiguity()) lowestAmbiguity = target;
+            if(lowestAmbiguity == null || lowestAmbiguity.getPoseAmbiguity() > target.getPoseAmbiguity()) { lowestAmbiguity = target; }
         }
         return lowestAmbiguity;
     }
 
     private double calculateDistance(Transform3d t) {
-        return Math.sqrt(t.getX()*t.getX() + t.getY()*t.getY() + t.getZ()*t.getZ());
+        return Math.sqrt(t.getX()*t.getX() + t.getY()*t.getY());
     }
+
+    /*
+     * In cases where data needs to be referenced more than once, use this function
+     * to keep the data consistent and prevent any errors.
+     * 
+     * Very Important!
+     */
+    public void freeze() {
+        frozenResults.addAll(getAllTargets());
+        frozen = true;
+    }
+
+    //Unfreeze opposite of freeze
+    public void unFreeze() {
+        frozenResults.clear();
+        frozen = false;
+    }
+
+
+    // There might be a case where you need this so I'm keeping it
+    public Object cloneAndFreeze() {  
+        try{  
+            AprilTags clone = (AprilTags) super.clone();
+            clone.freeze();
+            return clone;  
+        }catch(Exception e){ 
+            return null; 
+        }
+    }
+
 }

lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java

@@ -9,7 +9,6 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.EstimatedRobotPose;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Transform3d;
 
@@ -23,12 +22,10 @@ public class RobotFieldPosition {
         Transform3d robotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0,0,0)); 
         Cam mounted facing forward, half a meter forward of center, half a meter up from center.
     */
-    public RobotFieldPosition(String camera, Transform3d robotToCam) {
+    public RobotFieldPosition(String camera, Transform3d robotToCam, String layoutResourceFile) throws IOException {
-        try {
+
-            layout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2022RapidReact.m_resourceFile);
+        layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile);
-        } catch(IOException exception) {
+        
-            exception.printStackTrace();
-        }
         PhotonCamera cam = new PhotonCamera(camera);
         photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam);
     }