Update camera data structuring to hold camera names in an object inst…

…ead of as an input

src/main/java/frc/robot/Constants.java

@@ -23,7 +23,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import frc.robot.subsystems.drive.CommandSwerveDrivetrain;
-import frc.robot.subsystems.localization.VisionLocalizer.CameraTrustZone;
+import frc.robot.subsystems.localization.Camera.CameraTrustZone;
 
 import java.io.IOException;
 import java.util.Collections;

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

@@ -178,7 +178,7 @@ public void configureSubsystems() {
                 if (FeatureFlags.runVision) {
                     tagVision =
                             new VisionLocalizer(
-                                    new CameraContainerReplay(VisionConstants.cameras.size()));
+                                    new CameraContainerReplay(VisionConstants.cameras));
                 }
                 break;
         }
@@ -187,14 +187,6 @@ public void configureSubsystems() {
             drivetrain.registerTelemetry(driveTelemetry::telemeterize);
             drivetrain.setPoseSupplier(driveTelemetry::getFieldToRobot);
             drivetrain.setVelocitySupplier(driveTelemetry::getVelocity);
-
-            if (FeatureFlags.runVision) {
-                tagVision.setCameraConsumer(
-                        (m) ->
-                                drivetrain.addVisionMeasurement(
-                                        m.pose(), m.timestamp(), m.variance()));
-                tagVision.setFieldToRobotSupplier(() -> driveTelemetry.getFieldToRobot());
-            }
         }
 
         if (FeatureFlags.runScoring) {
@@ -231,7 +223,7 @@ public void configureSubsystems() {
             leds = new LED(scoringSubsystem);
 
             if (FeatureFlags.runVision) {
-                leds.setVisionWorkingSupplier(() -> tagVision.getVisionWorking());
+                leds.setVisionWorkingSupplier(() -> tagVision.coprocessorConnected());
             }
         }
     }

src/main/java/frc/robot/subsystems/localization/Camera.java

@@ -0,0 +1,80 @@
+package frc.robot.subsystems.localization;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj.DriverStation;
+import frc.robot.Constants.FieldConstants;
+import frc.robot.Constants.VisionConstants;
+import frc.robot.Constants.VisionConstants.CameraParams;
+import frc.robot.subsystems.localization.VisionLocalizer.CameraMeasurement;
+
+public class Camera {
+
+    public enum CameraTrustZone {
+        LEFT,
+        RIGHT,
+        MIDDLE,
+    }
+
+    public final String name;
+    public final CameraTrustZone zone;
+
+    private final CameraIO io;
+    private final CameraIOInputsAutoLogged inputs;
+
+
+    public Camera(CameraParams params, CameraIO io) {
+        name = params.name();
+        zone = params.zone();
+
+        this.io = io;
+        inputs = new CameraIOInputsAutoLogged();
+    }
+
+    public void update() {
+        io.updateInputs(inputs);
+        Logger.processInputs("Vision/" + name, inputs);
+    }
+
+    public boolean hasNewMeasurement() {
+        return inputs.wasAccepted;
+    }
+
+    public boolean isConnected() {
+        return inputs.connected;
+    }
+
+    public CameraMeasurement getLatestMeasurement() {
+        return new CameraMeasurement(inputs.latestFieldToRobot, inputs.latestTimestampSeconds, getLatestVariance());
+    }
+
+    public Matrix<N3, N1> getLatestVariance() {
+        if (!DriverStation.isTeleop()) {
+            if (inputs.latestFieldToRobot.getY() > FieldConstants.fieldToBlueSpeaker.getY() - 2
+                    && zone == CameraTrustZone.LEFT) {
+                return VisionConstants.highCameraUncertainty;
+            }
+
+            if (inputs.latestFieldToRobot.getY() < FieldConstants.fieldToBlueSpeaker.getY() + 2
+                    && zone == CameraTrustZone.RIGHT) {
+                return VisionConstants.highCameraUncertainty;
+            }
+        }
+
+        if (inputs.averageTagDistanceM < VisionConstants.skewCutoffDistance) {
+            if (!DriverStation.isTeleop()) {
+                return VisionConstants.lowCameraUncertainty;
+            } else {
+                return VisionConstants.teleopCameraUncertainty;
+            }
+        } else if (inputs.averageTagDistanceM < VisionConstants.lowUncertaintyCutoffDistance
+                && Math.abs(inputs.averageTagYaw.getDegrees()) < VisionConstants.skewCutoffRotation) {
+            return VisionConstants.lowCameraUncertainty;
+        } else {
+            return VisionConstants.highCameraUncertainty;
+        }
+    }
+}

src/main/java/frc/robot/subsystems/localization/CameraContainer.java

@@ -5,5 +5,7 @@
 /** This is not an AdvantageKit IO interface, but a class to hold cameras and associated data */
 public interface CameraContainer {
 
-    public List<CameraIOInputsAutoLogged> getInputs();
+    public List<Camera> getCameras();
+
+    public void update();
 }

src/main/java/frc/robot/subsystems/localization/CameraContainerReal.java

@@ -3,27 +3,24 @@
 import frc.robot.Constants.VisionConstants.CameraParams;
 import java.util.ArrayList;
 import java.util.List;
-import org.littletonrobotics.junction.Logger;
 
 public class CameraContainerReal implements CameraContainer {
 
-    private List<CameraIO> cameras = new ArrayList<>();
-    private List<CameraIOInputsAutoLogged> inputs = new ArrayList<>();
+    private List<Camera> cameras = new ArrayList<>();
 
     public CameraContainerReal(List<CameraParams> params) {
         for (CameraParams param : params) {
-            cameras.add(CameraIOPhoton.fromRealCameraParams(param));
-            inputs.add(new CameraIOInputsAutoLogged());
+            cameras.add(new Camera(param, CameraIOPhoton.fromRealCameraParams(param)));
         }
     }
 
-    public List<CameraIOInputsAutoLogged> getInputs() {
-        for (int i = 0; i < cameras.size(); i++) {
-            cameras.get(i).updateInputs(inputs.get(i));
+    public List<Camera> getCameras() {
+        return cameras;
+    }
 
-            Logger.processInputs("Vision/Camera" + i, inputs.get(i));
+    public void update() {
+        for (Camera camera : cameras) {
+            camera.update();
         }
-
-        return inputs;
     }
 }

src/main/java/frc/robot/subsystems/localization/CameraContainerReplay.java

@@ -2,26 +2,25 @@
 
 import java.util.ArrayList;
 import java.util.List;
-import org.littletonrobotics.junction.Logger;
+
+import frc.robot.Constants.VisionConstants.CameraParams;
 
 public class CameraContainerReplay implements CameraContainer {
-    private List<CameraIO> cameras = new ArrayList<>();
-    private List<CameraIOInputsAutoLogged> inputs;
+    private List<Camera> cameras = new ArrayList<>();
 
-    public CameraContainerReplay(int nCameras) {
-        for (int i = 0; i < nCameras; i++) {
-            cameras.add(new CameraIO() {});
-            inputs.add(new CameraIOInputsAutoLogged());
+    public CameraContainerReplay(List<CameraParams> params) {
+        for (CameraParams param : params) {
+            cameras.add(new Camera(param, new CameraIO() {}));
         }
     }
 
-    public List<CameraIOInputsAutoLogged> getInputs() {
-        for (int i = 0; i < cameras.size(); i++) {
-            cameras.get(i).updateInputs(inputs.get(i));
+    public List<Camera> getCameras() {
+        return cameras;
+    }
 
-            Logger.processInputs("Vision/Camera" + i, inputs.get(i));
+    public void update() {
+        for (Camera camera : cameras) {
+            camera.update();
         }
-
-        return inputs;
     }
 }

src/main/java/frc/robot/subsystems/localization/CameraContainerSim.java

@@ -21,8 +21,7 @@ public class CameraContainerSim implements CameraContainer {
 
     private VisionSystemSim visionSim = new VisionSystemSim("main");
 
-    private List<CameraIO> cameras = new ArrayList<>();
-    private List<CameraIOInputsAutoLogged> inputs = new ArrayList<>();
+    private List<Camera> cameras = new ArrayList<>();
 
     private Supplier<SwerveModuleState[]> getModuleStates;
     private Pose2d latestOdometryPose;
@@ -49,23 +48,22 @@ public CameraContainerSim(
                         Rotation2d.fromRadians(pose.getRotation().getRadians()));
 
         for (CameraParams param : params) {
-            cameras.add(CameraIOPhoton.fromSimCameraParams(param, visionSim, true));
-            inputs.add(new CameraIOInputsAutoLogged());
+            cameras.add(new Camera(param, CameraIOPhoton.fromSimCameraParams(param, visionSim, true)));
         }
     }
 
-    public List<CameraIOInputsAutoLogged> getInputs() {
+    public List<Camera> getCameras() {
+        return cameras;
+    }
+
+    public void update() {
         updateOdometry();
         visionSim.update(latestOdometryPose);
         SmartDashboard.putData("PhotonSimField", visionSim.getDebugField());
 
-        for (int i = 0; i < cameras.size(); i++) {
-            cameras.get(i).updateInputs(inputs.get(i));
-
-            Logger.processInputs("Vision/Camera" + i, inputs.get(i));
+        for (Camera camera : cameras) {
+            camera.update();
         }
-
-        return inputs;
     }
 
     private void updateOdometry() {

src/main/java/frc/robot/subsystems/localization/CameraIO.java

@@ -15,9 +15,12 @@ public static class CameraIOInputs {
 
         public double latestTimestampSeconds = 0.0;
         public boolean connected = false;
-        public boolean isNew = false;
-
-        public String name = "";
+        public boolean isNewMeasurement = false;
+        /**
+         * Whether the new camera measurement was accepted by the initial filters. Always false if
+         * `isNewMeasurement` is false.
+         */
+        public boolean wasAccepted = false;
     }
 
     public default void updateInputs(CameraIOInputs inputs) {}

src/main/java/frc/robot/subsystems/localization/CameraIOPhoton.java

@@ -23,16 +23,13 @@ public class CameraIOPhoton implements CameraIO {
 
     private double latestTimestampSeconds = 0.0;
 
-    private String name;
-
     public CameraIOPhoton(String name, Transform3d robotToCamera) {
         this(new PhotonCamera(name), robotToCamera);
     }
 
     public CameraIOPhoton(PhotonCamera camera, Transform3d robotToCamera) {
         this.camera = camera;
 
-        name = camera.getName();
         poseEstimator =
                 new PhotonPoseEstimator(
                         VisionConstants.fieldLayout,
@@ -66,27 +63,31 @@ public static CameraIOPhoton fromSimCameraParams(
     public void updateInputs(CameraIOInputs inputs) {
         inputs.connected = camera.isConnected();
 
-        inputs.name = this.name;
-
         PhotonPipelineResult result = camera.getLatestResult();
         if (result.getTimestampSeconds() == latestTimestampSeconds) {
-            inputs.isNew = false;
+            inputs.isNewMeasurement = false;
+            inputs.wasAccepted = false;
             return;
         }
-        inputs.isNew = true;
+        inputs.isNewMeasurement = true;
         latestTimestampSeconds = result.getTimestampSeconds();
         Optional<EstimatedRobotPose> photonPose = poseEstimator.update(result);
 
         photonPose.filter(CameraIOPhoton::filterPhotonPose);
 
-        photonPose.ifPresent(
+        photonPose.ifPresentOrElse(
                 (pose) -> {
                     inputs.latestFieldToRobot = pose.estimatedPose.toPose2d();
                     inputs.nTags = pose.targetsUsed.size();
 
                     inputs.latestTimestampSeconds = this.latestTimestampSeconds;
                     inputs.averageTagDistanceM = calculateAverageTagDistance(pose);
                     inputs.averageTagYaw = calculateAverageTagYaw(pose);
+
+                    inputs.wasAccepted = true;
+                },
+                () -> {
+                    inputs.wasAccepted = false;
                 });
     }