Final commit before merging to main to continue progress

src/main/java/frc/robot/RobotContainer.java

@@ -17,10 +17,12 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.OIConstants;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.PhotonVision;
 import frc.robot.utilities.Elastic;
 import frc.robot.utilities.Utilities;
 
 public class RobotContainer {
+  private PhotonVision vision;
   private Drivetrain drivetrain;
 
   private CommandXboxController driver;
@@ -30,8 +32,11 @@ public class RobotContainer {
   private Timer shiftTimer;
 
   public RobotContainer() {
+    vision = new PhotonVision();
     drivetrain = new Drivetrain();
 
+    vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
+
     driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 
     shiftTimer = new Timer();

src/main/java/frc/robot/constants/PhotonConstants.java

@@ -1,6 +1,9 @@
 package frc.robot.constants;
 
+import java.util.List;
+
 import edu.wpi.first.math.geometry.Transform3d;
+import frc.robot.utilities.PhotonVisionConfig;
 
 public class PhotonConstants {
     public static final String kCamera1Name = "pv1";
@@ -14,4 +17,11 @@ public class PhotonConstants {
     public static final double kCamera1PitchRadians = 0;
     public static final double kCamera2HeightMeters = 0;
     public static final double kCamera2PitchRadians = 0;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final List<PhotonVisionConfig> configs = List.of(
+        new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians),
+        new PhotonVisionConfig(kCamera2Name, kCamera2RobotToCam, kCamera2HeightMeters, kCamera2PitchRadians)
+    );
 }

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

@@ -28,7 +28,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
-import frc.robot.utilities.PhotonVision;
+import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
 import frc.robot.utilities.SwerveModule;
 
 public class Drivetrain extends SubsystemBase {
@@ -173,7 +173,7 @@ public class Drivetrain extends SubsystemBase {
     }
 
     // TODO check both cameras
-    public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
+    /*public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
         if (camera1 == null) {
             return new PrintCommand("Camera 1 not available");
         }
@@ -197,7 +197,7 @@ public class Drivetrain extends SubsystemBase {
                 () -> false
            )
         );
-    }
+    }*/
 
     public Command drivePathPlannerPath(PathPlannerPath path) {
         if(AutoConstants.kAutoConfigOk) {
@@ -229,6 +229,13 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
+    public void consumeVisualPose(VisualPose pose) {
+        estimator.addVisionMeasurement(
+            pose.visualPose(), 
+            pose.timestamp()
+        );
+    }
+
     public void resetEncoders() {
         frontLeft.resetEncoders();
         frontRight.resetEncoders();

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

@@ -0,0 +1,198 @@
+package frc.robot.subsystems;
+
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+import java.util.Optional;
+import java.util.OptionalDouble;
+import java.util.function.Consumer;
+import java.util.stream.Stream;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.PhotonUtils;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.constants.CompetitionConstants;
+import frc.robot.constants.PhotonConstants;
+import frc.robot.interfaces.IAprilTagProvider;
+import frc.robot.interfaces.IVisualPoseProvider;
+import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
+import frc.robot.utilities.PhotonVisionConfig;
+
+public class PhotonVision extends SubsystemBase {
+    private PhotonCamera[] cameras;
+    private PhotonPoseEstimator[] estimators;
+    private List<PhotonPipelineResult> latestResults;
+
+    private ArrayList<Consumer<VisualPose>> poseEstimateConsumers;
+
+    public PhotonVision() {
+        cameras = new PhotonCamera[PhotonConstants.configs.size()];
+        estimators = new PhotonPoseEstimator[PhotonConstants.configs.size()];
+        latestResults = new ArrayList<PhotonPipelineResult>();
+
+        for(int i = 0; i < PhotonConstants.configs.size(); i++) {
+            cameras[i] = new PhotonCamera(PhotonConstants.configs.get(i).cameraName());
+            estimators[i] = new PhotonPoseEstimator(
+                CompetitionConstants.kTagLayout, 
+                PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, 
+                PhotonConstants.configs.get(i).robotToCamera()
+            );
+            latestResults.add(null);
+        }
+
+        poseEstimateConsumers = new ArrayList<Consumer<VisualPose>>();
+    }
+
+    @Override
+    public void periodic() {
+        for(int i = 0; i < cameras.length; i++) {
+            List<PhotonPipelineResult> results = cameras[i].getAllUnreadResults();
+
+            if(!results.isEmpty()) {
+                latestResults.set(i, results.get(results.size() - 1));
+            }
+
+            Optional<EstimatedRobotPose> pose = estimators[i].update(latestResults.get(i));
+
+            if(!pose.isEmpty()) {
+                VisualPose visualPose = new VisualPose(
+                    pose.get().estimatedPose.toPose2d(), 
+                    pose.get().timestampSeconds
+                );
+
+                for(Consumer<VisualPose> consumer: poseEstimateConsumers) {
+                    consumer.accept(visualPose);
+                }
+            }
+        }
+    }
+
+    public void testMethod(int targetID) {
+        Optional<PhotonTrackedTarget> target = latestResults.stream()
+            .filter((p) -> p != null)
+            .map(PhotonPipelineResult::getTargets)
+            .map(List::stream)
+            .reduce(Stream::concat)
+            .get()
+            .filter((p) -> p.getFiducialId() == targetID)
+            .max(
+                Comparator.comparingDouble((ptt) -> {
+                    return (double)ptt.getDetectedObjectConfidence();
+                })
+            );
+
+    }
+
+    public void addPoseEstimateConsumer(Consumer<VisualPose> consumer) {
+        poseEstimateConsumers.add(consumer);
+    }
+
+    /*public Trigger tagPrescenseTrigger(int targetTag) {
+        return new Trigger(() -> {
+            return List.of(latestResults).stream()
+                .filter((p) -> p != null)
+                .anyMatch((p) -> {
+                    return p.getTargets().stream().map(PhotonTrackedTarget::getFiducialId).anyMatch((i) -> {
+                        return i == targetTag;
+                    });
+                });
+            });
+    }*/
+/* 
+    @Override
+    public OptionalDouble getTagDistanceFromCameraByID(int id) {
+        if (latestResult == null) {
+            return OptionalDouble.empty();
+        }
+
+        if (!latestResult.hasTargets()) {
+            return OptionalDouble.empty();
+        }
+
+        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
+        
+        if (desiredTarget.isEmpty()) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(
+            PhotonUtils.calculateDistanceToTargetMeters(
+                cameraHeightMeters, 
+                CompetitionConstants.kTagLayout.getTagPose(id).get().getZ(),
+                cameraPitchRadians, 
+                Units.degreesToRadians(desiredTarget.get().getPitch()))
+        );
+    }
+
+    @Override
+    public OptionalDouble getTagPitchByID(int id) {
+        if(latestResult == null) {
+            OptionalDouble.empty();
+        }
+        
+        if (!latestResult.hasTargets()) {
+            return OptionalDouble.empty();
+        }
+
+        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
+
+        if (desiredTarget.isEmpty()) {
+            return OptionalDouble.empty();
+        }
+
+        return OptionalDouble.of(
+            desiredTarget.get().getPitch()
+        );
+    }
+
+    @Override
+    public OptionalDouble getTagYawByID(int id) {
+        if(latestResult == null) {
+            OptionalDouble.empty();
+        }
+        
+        if (!latestResult.hasTargets()) {
+            return OptionalDouble.empty();
+        }
+
+        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.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();
+    }
+
+    @Override
+    public int[] getVisibleTagIDs() {
+        if(latestResult == null) {
+            return new int[] {};
+        }
+
+        return latestResult.getTargets().stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray();
+    }
+    */
+}

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

@@ -1,165 +0,0 @@
-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.PhotonPoseEstimator.PoseStrategy;
-import org.photonvision.PhotonUtils;
-import org.photonvision.targeting.PhotonPipelineResult;
-import org.photonvision.targeting.PhotonTrackedTarget;
-
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.util.Units;
-import frc.robot.constants.CompetitionConstants;
-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;
-
-    private PhotonPipelineResult latestResult;
-
-    public PhotonVision(String cameraName, Transform3d robotToCam, double cameraHeightMeters, double cameraPitchRadians) throws IOException {
-        camera = new PhotonCamera(cameraName);
-
-        photonPoseEstimator = new PhotonPoseEstimator(
-            CompetitionConstants.kTagLayout, 
-            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, 
-            robotToCam
-        );
-
-        this.cameraHeightMeters = cameraHeightMeters;
-        this.cameraPitchRadians = cameraPitchRadians;
-
-        this.latestResult = null;
-    }
-
-    // TODO This periodic method has to be called from somewhere, even though the cameras
-    // could be used in multiple other subsystems
-    public void periodic() {
-        // TODO Do we care about missed results? Probably not, if we're taking long enough to miss results something else is wrong
-        List<PhotonPipelineResult> results = camera.getAllUnreadResults();
-
-        if(!results.isEmpty()) {
-            latestResult = results.get(results.size() - 1);
-        }
-    }
-
-    @Override
-    public Optional<VisualPose> getVisualPose() {
-        if(latestResult == null) {
-            return Optional.empty();
-        }
-
-        Optional<EstimatedRobotPose> pose = photonPoseEstimator.update(latestResult);
-
-        if (pose.isEmpty()) {
-            return Optional.empty();
-        }
-
-        return Optional.of(
-            new VisualPose(
-                pose.get().estimatedPose.toPose2d(), 
-                pose.get().timestampSeconds
-            )
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagDistanceFromCameraByID(int id) {
-        if (latestResult == null) {
-            return OptionalDouble.empty();
-        }
-
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-        
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            PhotonUtils.calculateDistanceToTargetMeters(
-                cameraHeightMeters, 
-                CompetitionConstants.kTagLayout.getTagPose(id).get().getZ(),
-                cameraPitchRadians, 
-                Units.degreesToRadians(desiredTarget.get().getPitch()))
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagPitchByID(int id) {
-        if(latestResult == null) {
-            OptionalDouble.empty();
-        }
-        
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.getTargets(), id);
-
-        if (desiredTarget.isEmpty()) {
-            return OptionalDouble.empty();
-        }
-
-        return OptionalDouble.of(
-            desiredTarget.get().getPitch()
-        );
-    }
-
-    @Override
-    public OptionalDouble getTagYawByID(int id) {
-        if(latestResult == null) {
-            OptionalDouble.empty();
-        }
-        
-        if (!latestResult.hasTargets()) {
-            return OptionalDouble.empty();
-        }
-
-        Optional<PhotonTrackedTarget> desiredTarget = getTargetFromList(latestResult.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();
-    }
-
-    @Override
-    public int[] getVisibleTagIDs() {
-        if(latestResult == null) {
-            return new int[] {};
-        }
-
-        return latestResult.getTargets().stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray();
-    }
-    
-}

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

@@ -0,0 +1,10 @@
+package frc.robot.utilities;
+
+import edu.wpi.first.math.geometry.Transform3d;
+
+public record PhotonVisionConfig (
+    String cameraName,
+    Transform3d robotToCamera,
+    double cameraHeightMeters,
+    double cameraPitchRadians
+) {}