Adding changes to interfaces AND preliminary PhotonVision interfacing class

src/main/java/frc/robot/interfaces/IAprilTagProvider.java

@@ -3,8 +3,7 @@ package frc.robot.interfaces;
 import java.util.OptionalDouble;
 
 public interface IAprilTagProvider {
-    public OptionalDouble getTagHorizontalOffsetByID(int id);
+    public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters);
-    public OptionalDouble getTagDistanceFromCameraByID(int id);
     public OptionalDouble getTagPitchByID(int id);
     public OptionalDouble getTagYawByID(int id);
 }

src/main/java/frc/robot/interfaces/IVisualPoseProvider.java

@@ -1,9 +1,11 @@
 package frc.robot.interfaces;
 
+import java.util.Optional;
+
 import edu.wpi.first.math.geometry.Pose2d;
 
 public interface IVisualPoseProvider {
     public record VisualPose(Pose2d visualPose, double timestamp) {}
 
-    public VisualPose getVisualPose();
+    public Optional<VisualPose> getVisualPose();
 }

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -25,6 +25,7 @@ import frc.robot.constants.DrivetrainConstants;
 import frc.robot.utilities.MAXSwerveModule;
 import frc.robot.utilities.SwerveUtils;
 import frc.robot.interfaces.IVisualPoseProvider;
+import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.PIDCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -134,10 +135,11 @@ public class Drivetrain extends SubsystemBase {
         );
 
         if (m_visualPoseProvider != null) {
-            m_poseEstimator.addVisionMeasurement(
+            Optional<VisualPose> vPose = m_visualPoseProvider.getVisualPose();
-                m_visualPoseProvider.getVisualPose().visualPose(), 
+
-                m_visualPoseProvider.getVisualPose().timestamp()
+            if (vPose.isPresent()) {
-            );
+                m_poseEstimator.addVisionMeasurement(vPose.get().visualPose(), vPose.get().timestamp());
+            }
         }
     }
 

src/main/java/frc/robot/utilities/PhotonVision.java

@@ -0,0 +1,135 @@
+package frc.robot.utilities;
+
+import java.io.IOException;
+import java.util.List;
+import java.util.Optional;
+import java.util.OptionalDouble;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonUtils;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.util.Units;
+import frc.robot.interfaces.IAprilTagProvider;
+import frc.robot.interfaces.IVisualPoseProvider;
+
+public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider {
+
+    private final PhotonCamera camera;
+
+    private final PhotonPoseEstimator photonPoseEstimator;
+
+    private final double cameraHeightMeters;
+    private final double cameraPitchRadians;
+
+    public PhotonVision(String cameraName, Transform3d robotToCam, double cameraHeightMeters, double cameraPitchRadians) throws IOException {
+        camera = new PhotonCamera(cameraName);
+
+        photonPoseEstimator = new PhotonPoseEstimator(
+            AprilTagFieldLayout.loadFromResource(
+                AprilTagFields.k2024Crescendo.m_resourceFile
+            ), 
+            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, 
+            camera,
+            robotToCam
+        );
+
+        this.cameraHeightMeters = cameraHeightMeters;
+        this.cameraPitchRadians = cameraPitchRadians;
+    }
+
+    @Override
+    public Optional<VisualPose> getVisualPose() {
+        Optional<EstimatedRobotPose> pose = photonPoseEstimator.update();
+
+        if (pose.isEmpty()) {
+            return Optional.empty();
+        }
+
+        return Optional.of(
+            new VisualPose(
+                pose.get().estimatedPose.toPose2d(), 
+                pose.get().timestampSeconds
+            )
+        );
+    }
+
+    @Override
+    public OptionalDouble getTagDistanceFromCameraByID(int id, double targetHeightMeters) {
+        PhotonPipelineResult result = camera.getLatestResult();
+        
+        if (!result.hasTargets()) {
+            return OptionalDouble.empty();
+        }
+
+        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
+
+        if (desiredTarget.isEmpty()) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(
+            PhotonUtils.calculateDistanceToTargetMeters(
+                cameraHeightMeters, 
+                targetHeightMeters, 
+                cameraPitchRadians, 
+                Units.degreesToRadians(desiredTarget.get().getPitch()))
+        );
+    }
+
+    @Override
+    public OptionalDouble getTagPitchByID(int id) {
+        PhotonPipelineResult result = camera.getLatestResult();
+        
+        if (!result.hasTargets()) {
+            return OptionalDouble.empty();
+        }
+
+        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
+
+        if (desiredTarget.isEmpty()) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(
+            desiredTarget.get().getPitch()
+        );
+    }
+
+    @Override
+    public OptionalDouble getTagYawByID(int id) {
+        PhotonPipelineResult result = camera.getLatestResult();
+        
+        if (!result.hasTargets()) {
+            return OptionalDouble.empty();
+        }
+
+        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(result.getTargets(), id);
+
+        if (desiredTarget.isEmpty()) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(
+            desiredTarget.get().getYaw()
+        );
+    }
+
+    private Optional<PhotonTrackedTarget> getTargetFromList(List<PhotonTrackedTarget> targets, int id) {
+        for (PhotonTrackedTarget target : targets) {
+            if (target.getFiducialId() == id) {
+                return Optional.of(target);
+            }
+        }
+
+        return Optional.empty();
+    }
+    
+}

vendordeps/photonlib.json

@@ -0,0 +1,57 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2024.1.2",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+    "frcYear": "2024",
+    "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": "v2024.1.2",
+            "libName": "photonlib",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2024.1.2",
+            "libName": "photontargeting",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-java",
+            "version": "v2024.1.2"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-java",
+            "version": "v2024.1.2"
+        }
+    ]
+}