Add vision pose estimates to drivetrain

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -1,7 +1,10 @@
 package frc.robot.subsystems.drive;
 
+import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public interface Drive {
@@ -66,4 +69,7 @@ public default double getAngle() {
 
   public default void lockPose() {}
   ;
+
+  public default void addVisionMeasurement(
+      Pose2d robotPose, double timestamp, Matrix<N3, N1> visionMeasurementStdDevs) {}
 }

src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java

@@ -4,9 +4,12 @@
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.ReplanningConfig;
+import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
@@ -155,4 +158,11 @@ public void periodic() {
     io.updateInputs(inputs, swerveDrive);
     Logger.processInputs("Drive", inputs);
   }
+
+  @Override
+  public void addVisionMeasurement(
+      Pose2d robotPose, double timestamp, Matrix<N3, N1> visionMeasurementStdDevs) {
+
+    swerveDrive.addVisionMeasurement(robotPose, timestamp, visionMeasurementStdDevs);
+  }
 }

src/main/java/frc/robot/subsystems/vision/Vision.java

@@ -2,7 +2,6 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import java.text.DecimalFormat;
-import java.util.List;
 import java.util.Optional;
 import java.util.function.Supplier;
 
@@ -83,13 +82,5 @@ public default boolean setPrimaryCamera(String name) {
    */
   public Optional<Double> getYawToAprilTag(int id);
 
-  /**
-   * Returns a list of vision-based estimated robot poses
-   *
-   * @return list of estimated poses. list length may be zero if updated pose estimation data is not
-   *     available.
-   */
-  public List<VisionPose> getEstimatedRobotPoses();
-
   public default void enableSimulation(Supplier<Pose2d> poseSupplier, boolean enableWireFrame) {}
 }

src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java

@@ -1,12 +1,14 @@
 package frc.robot.subsystems.vision;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.config.RobotConfig;
 import frc.robot.util.DevilBotState;
 import java.util.ArrayList;
 import java.util.List;
@@ -90,8 +92,6 @@ private Transform3d getRobotToCamera() {
   private VisionCameraImpl primaryCamera = null;
   private final AprilTagFieldLayout fieldLayout;
 
-  private List<VisionPose> estimatedPoses = new ArrayList<VisionPose>();
-
   /* Debug Info */
   @AutoLogOutput private int debugTargetsVisible;
   @AutoLogOutput private int debugCurrentTargetId = DevilBotState.getActiveTargetId();
@@ -112,7 +112,6 @@ private Transform3d getRobotToCamera() {
   public VisionSubsystem(List<VisionCamera> cameras, AprilTagFieldLayout fieldLayout) {
     for (VisionCamera camera : cameras) {
       this.cameras.add(new VisionCameraImpl(camera, fieldLayout));
-      estimatedPoses.add(new VisionPose());
     }
     this.fieldLayout = fieldLayout;
     if (0 != cameras.size()) {
@@ -166,14 +165,21 @@ public void periodic() {
 
       Optional<EstimatedRobotPose> currentEstimatedRobotPose = camera.getEstimatedRobotPose();
       if (currentEstimatedRobotPose.isPresent()) {
-        estimatedPoses.get(camera.getIndex()).robotPose =
-            currentEstimatedRobotPose.get().estimatedPose.toPose2d();
-        estimatedPoses.get(camera.getIndex()).timestamp =
-            currentEstimatedRobotPose.get().timestampSeconds;
-        estimatedPoses.get(camera.getIndex()).cameraName = camera.getName();
+        Optional<Double> distanceToBestTarget =
+            getDistanceToAprilTag(camera.getBestTarget().getFiducialId());
+        if (distanceToBestTarget.isPresent()) {
+          double distance = distanceToBestTarget.get();
+
+          // Add vision measurement to the drivetrain.
+          // TODO: clean up this abstraction
+          RobotConfig.drive.addVisionMeasurement(
+              currentEstimatedRobotPose.get().estimatedPose.toPose2d(),
+              currentEstimatedRobotPose.get().timestampSeconds,
+              VecBuilder.fill(distance / 2, distance / 2, 100));
+        }
 
         if (camera == primaryCamera) {
-          debugEstimatedPose = estimatedPoses.get(camera.getIndex()).robotPose;
+          debugEstimatedPose = currentEstimatedRobotPose.get().estimatedPose.toPose2d();
         }
       }
     }
@@ -257,11 +263,6 @@ public Optional<Double> getYawToAprilTag(int id) {
     return Optional.empty();
   }
 
-  @Override
-  public List<VisionPose> getEstimatedRobotPoses() {
-    return estimatedPoses;
-  }
-
   @Override
   public boolean setPrimaryCamera(String name) {
     boolean foundCamera = false;