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..270ed39 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 @@ -25,11 +25,12 @@ public class AprilTags { for(PhotonCamera camera : cameras) { for(PhotonTrackedTarget target : camera.getLatestResult().getTargets()) { 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 - 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,10 +38,10 @@ public class AprilTags { break; } } + if(toAdd) { tempTargets.add(target); } - if(toAdd) targets.add(target); + targets = tempTargets; } - targets.addAll(camera.getLatestResult().getTargets()); } return targets; @@ -48,36 +49,31 @@ public class AprilTags { // 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 cameras.stream().anyMatch((camera) -> camera.getLatestResult().hasTargets() == true); } // 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()); } } \ 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); }