Add AprilTags and RobotFieldPosition

.gitignore

@@ -1,5 +1,6 @@
 .gradle
 /build/
+/lib/bin
 
 # Ignore Gradle GUI config
 gradle-app.setting
@@ -15,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.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 "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"

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

@@ -0,0 +1,78 @@
+package com.fairportrobotics.frc.poe.CameraTracking;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+public class AprilTags { 
+    // an ArrayList to store the cameras
+    public final ArrayList<PhotonCamera> cameras = new ArrayList<>();
+
+    public AprilTags(String...cameraNames) {
+        for(String cameraName : cameraNames) {
+            cameras.add(new PhotonCamera(cameraName));
+        }
+    }
+
+    // Method to get a list of all targets from all cameras
+    public List<PhotonTrackedTarget> getTargets() {
+        List<PhotonTrackedTarget> 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;
+                    }
+                }
+
+                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) {
+            // 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;
+        PhotonTrackedTarget closestTarget = null;
+        for(PhotonTrackedTarget target : getTargets()) {
+            if(closestTarget == null || Math.abs(target.getBestCameraToTarget().getY()) > Math.abs(target.getBestCameraToTarget().getY()) ) 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;
+    }
+
+}

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

@@ -0,0 +1,42 @@
+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.apriltag.AprilTagFields;
+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) {
+        try {
+            layout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2022RapidReact.m_resourceFile);
+        } catch(IOException exception) {
+            exception.printStackTrace();
+        }
+        PhotonCamera cam = new PhotonCamera(camera);
+        photonPoseEstimator = new PhotonPoseEstimator(layout, PoseStrategy.LOWEST_AMBIGUITY, cam, robotToCam);
+    }  
+
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
+        photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
+        return photonPoseEstimator.update();
+    }
+
+
+}   

vendordeps/photonlib.json

@@ -0,0 +1,41 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2023.2.1",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ",
+    "mavenUrls": [
+        "https://maven.photonvision.org/repository/internal",
+        "https://maven.photonvision.org/repository/snapshots"
+    ],
+    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-cpp",
+            "version": "v2023.2.1",
+            "libName": "Photon",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonLib-java",
+            "version": "v2023.2.1"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "PhotonTargeting-java",
+            "version": "v2023.2.1"
+        }
+    ]
+}