diff --git a/.gitignore b/.gitignore index e519109..b7e94a2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,6 @@ .gradle -build/ - -lib/bin +/build/ +/lib/bin # Ignore Gradle GUI config gradle-app.setting @@ -17,3 +16,4 @@ gradle-app.setting # Ignore Gradle build output directory build + diff --git a/lib/build.gradle b/lib/build.gradle index 409832f..689664a 100644 --- a/lib/build.gradle +++ b/lib/build.gradle @@ -17,15 +17,10 @@ repositories { mavenCentral() mavenLocal() gradlePluginPortal() - maven{ - url "https://maven.photonvision.org/repository/snapshots" - } - maven{ - url "https://maven.photonvision.org/repository/internal" - } - maven{ - url "https://maven.ctr-electronics.com/release/" - } + + maven {url "https://maven.photonvision.org/repository/snapshots"} + maven {url "https://maven.photonvision.org/repository/internal"} + maven {url "https://maven.ctr-electronics.com/release/"} } dependencies { @@ -34,14 +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:2022.+' - implementation 'edu.wpi.first.cameraserver:cameraserver-java:2022.+' - implementation 'edu.wpi.first.ntcore:ntcore-java:2022.+' - implementation 'edu.wpi.first.wpilibj:wpilibj-java:2022.+' - implementation 'edu.wpi.first.wpiutil:wpiutil-java:2022.+' - implementation 'edu.wpi.first.wpimath:wpimath-java:2022.+' - implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2022.+' - implementation 'edu.wpi.first.hal:hal-java:2022.+' + 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" @@ -49,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 new file mode 100644 index 0000000..fc403fe --- /dev/null +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/AprilTags.java @@ -0,0 +1,114 @@ +package com.fairportrobotics.frc.poe.CameraTracking; + +import java.util.ArrayList; +import java.util.List; + +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonTrackedTarget; + +import edu.wpi.first.math.geometry.Transform3d; + +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) { + cameras.add(new PhotonCamera(cameraName)); + } + } + + //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(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) { tempTargets.add(target); } + targets = tempTargets; + } + + + return targets; + } + + // method to check if any camera has any targets + public boolean hasTargets() { + return getAllTargets().size() != 0; + } + + // method to get the closest target + public PhotonTrackedTarget getClosestTarget() { + if(!hasTargets()) { return null; } + PhotonTrackedTarget closestTarget = null; + for(PhotonTrackedTarget target : getTargets()) { + 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; } + 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; } + } + return lowestAmbiguity; + } + + private double calculateDistance(Transform3d t) { + 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 new file mode 100644 index 0000000..2c62ef5 --- /dev/null +++ b/lib/src/main/java/com/fairportrobotics/frc/poe/CameraTracking/RobotFieldPosition.java @@ -0,0 +1,38 @@ +package com.fairportrobotics.frc.poe.CameraTracking; + +import java.io.IOException; +import java.util.Optional; + +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.EstimatedRobotPose; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; + +public class RobotFieldPosition { + AprilTagFieldLayout layout; + PhotonPoseEstimator photonPoseEstimator; + + /* + Example of Transform3d: + + 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, String layoutResourceFile) throws IOException { + + layout = AprilTagFieldLayout.loadFromResource(layoutResourceFile); + + PhotonCamera cam = new PhotonCamera(camera); + photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam); + } + + public Optional getEstimatedGlobalPose() { + return photonPoseEstimator.update(); + } + + +} \ No newline at end of file