From a8b7ee06aa530970a88d6d375f1d50d6906f4632 Mon Sep 17 00:00:00 2001 From: Declipsonator Date: Thu, 16 Feb 2023 16:34:00 -0800 Subject: [PATCH] Additions to Fix Crashes & Improve Code --- lib/build.gradle | 22 ++--- .../frc/poe/CameraTracking/AprilTags.java | 91 +++++++++++++------ .../CameraTracking/RobotFieldPosition.java | 11 +-- 3 files changed, 76 insertions(+), 48 deletions(-) diff --git a/lib/build.gradle b/lib/build.gradle index 95ce996..689664a 100644 --- a/lib/build.gradle +++ b/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.cameraserver:cameraserver-java:2023.2.1' - implementation 'edu.wpi.first.ntcore:ntcore-java:2023.2.1' - implementation 'edu.wpi.first.wpilibj:wpilibj-java:2023.2.1' - implementation 'edu.wpi.first.wpiutil:wpiutil-java:2023.2.1' - implementation 'edu.wpi.first.wpimath:wpimath-java:2023.2.1' - implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2023.2.1' - implementation 'edu.wpi.first.hal:hal-java:2023.2.1' - implementation "edu.wpi.first.apriltag:apriltag-java:2023.2.1" + implementation 'edu.wpi.first.cscore:cscore-java:2023.+' + implementation 'edu.wpi.first.cameraserver:cameraserver-java:2023.+' + implementation 'edu.wpi.first.ntcore:ntcore-java:2023.+' + implementation 'edu.wpi.first.wpilibj:wpilibj-java:2023.+' + implementation 'edu.wpi.first.wpiutil:wpiutil-java:2023.+' + implementation 'edu.wpi.first.wpimath:wpimath-java:2023.+' + implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2023.+' + implementation 'edu.wpi.first.hal:hal-java:2023.+' + 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:PhotonTargeting-java:v2023.2.1" + implementation "org.photonvision:PhotonLib-java:v2023.+" + implementation "org.photonvision:PhotonTargeting-java:v2023.+" } tasks.named('test') { diff --git a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java index 7aa4568..fc403fe 100644 --- a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java +++ b/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 cameras = new ArrayList<>(); + public final ArrayList frozenResults = new ArrayList<>(); + public boolean frozen = false; public AprilTags(String...cameraNames) { for(String cameraName : cameraNames) { @@ -18,66 +20,95 @@ 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 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 getTargets() { List targets = new ArrayList<>(); - for(PhotonCamera camera : cameras) { - for(PhotonTrackedTarget target : camera.getLatestResult().getTargets()) { - boolean toAdd = true; - // 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); - break; - } else if(target.getFiducialId() == arTarget.getFiducialId()) { - // if the target is already in the list and has the same ID, don't add it - toAdd = false; - break; - } + for(PhotonTrackedTarget target : getAllTargets()) { + boolean toAdd = true; + List 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 + 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 + toAdd = false; + break; } - - if(toAdd) targets.add(target); } - targets.addAll(camera.getLatestResult().getTargets()); + if(toAdd) { tempTargets.add(target); } + targets = tempTargets; } + return targets; } // method to check if any camera has any targets public boolean hasTargets() { - for(PhotonCamera camera : cameras) { - // if a camera has targets, return true - if(camera.getLatestResult().hasTargets()) return true; - } - - return false; + return getAllTargets().size() != 0; } // 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; + } + } + } \ No newline at end of file diff --git a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java index 81fe7e2..17ed464 100644 --- a/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java +++ b/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) { - try { - layout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2022RapidReact.m_resourceFile); - } catch(IOException exception) { - exception.printStackTrace(); - } + public RobotFieldPosition(String camera, Transform3d robotToCam, String layoutResourceFile) throws IOException { + + layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile); + PhotonCamera cam = new PhotonCamera(camera); photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); }