Merge pull request #1 from FairportRobotics/features/camera-tracking

.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
+

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') {

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<PhotonCamera> cameras = new ArrayList<>();
+    public final ArrayList<PhotonTrackedTarget> 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<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(PhotonTrackedTarget target : getAllTargets()) {
+            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
+                    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; 
+        }
+    }
+
+}

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<EstimatedRobotPose> getEstimatedGlobalPose() {
+        return photonPoseEstimator.update();
+    }
+
+
+}