finished implementing camera setup for testbed

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

@@ -18,6 +18,8 @@
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -26,12 +28,15 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.subsystems.drive.module.Module;
 import frc.robot.subsystems.drive.module.ModuleIO;
+import frc.robot.subsystems.vision.VisionSubsystem;
 import frc.robot.util.LocalADStarAK;
 
 import java.util.List;
@@ -55,23 +60,45 @@ public class DriveSubsystem extends SubsystemBase {
 
   private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
   private Pose2d pose = new Pose2d();
+  private Pose2d visionPose = new Pose2d();
   private Rotation2d lastGyroRotation = new Rotation2d();
 
+  private final VisionSubsystem[] m_cameras;
+  private final Matrix<N3, N1> m_stdDevs;
+
   public DriveSubsystem(
       GyroIO gyroIO,
       ModuleIO flModuleIO,
       ModuleIO frModuleIO,
       ModuleIO blModuleIO,
       ModuleIO brModuleIO) {
+    this (gyroIO,
+        flModuleIO,
+        frModuleIO,
+        blModuleIO,
+        brModuleIO,
+        new VisionSubsystem[]{});
+  }
+
+  public DriveSubsystem(
+      GyroIO gyroIO,
+      ModuleIO flModuleIO,
+      ModuleIO frModuleIO,
+      ModuleIO blModuleIO,
+      ModuleIO brModuleIO,
+      VisionSubsystem[] cameras) {
     this.gyroIO = gyroIO;
     modules[0] = new Module(flModuleIO);
     modules[1] = new Module(frModuleIO);
     modules[2] = new Module(blModuleIO);
     modules[3] = new Module(brModuleIO);
 
+    m_cameras = cameras;
+    m_stdDevs = VecBuilder.fill(Units.inchesToMeters(0.5), Units.inchesToMeters(0.5), Units.degreesToRadians(15));
+
     // Configure AutoBuilder for PathPlanner
     AutoBuilder.configureHolonomic(
-        this::getPose,
+        this::getVisionPose,
         this::setPose,
         () -> kinematics.toChassisSpeeds(getModuleStates()),
         this::runVelocity,
@@ -143,6 +170,11 @@ public void periodic() {
       // Apply the twist (change since last sample) to the current pose
       pose = pose.exp(twist);
     }
+
+    for (VisionSubsystem camera : m_cameras) {
+      var cameraPose = camera.getPose(getPose());
+      cameraPose.ifPresent(visionUpdate -> visionPose = visionUpdate.apply(pose, m_stdDevs));
+    }
   }
 
   /**
@@ -218,6 +250,12 @@ public Pose2d getPose() {
     return pose;
   }
 
+  /** Returns the robot pose with vision updates */
+  @AutoLogOutput(key = "Odometry/RobotVision")
+  public Pose2d getVisionPose() {
+    return visionPose;
+  }
+
   /** Returns the current odometry rotation. */
   public Rotation2d getRotation() {
     return pose.getRotation();

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

@@ -3,11 +3,19 @@
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.Logger;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
@@ -16,13 +24,14 @@
 import java.io.IOException;
 import java.util.Optional;
 
-public class VisionSubsystem extends SubsystemBase {
+public class VisionSubsystem {
 
   private final PhotonCamera m_camera;
   private final Transform3d robotToCam;
   private PhotonPoseEstimator m_photonPoseEstimator;
   private DriverStation.Alliance m_alliance;
   private AprilTagFieldLayout m_aprilTagFieldLayout;
+  private final String m_name;
 
   private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged();
 
@@ -32,10 +41,13 @@ public void updateInputs(){
     inputs.setTagIds(getTargetIDs());
     inputs.setNumTagsFound(m_camera.getLatestResult().targets.size());
     inputs.setTagsFound(m_camera.getLatestResult().hasTargets());
+
+    Logger.processInputs("PhotonVision/" + m_name, inputs);
   }
 
   public VisionSubsystem(String camName, Transform3d camPose) {
     m_camera = new PhotonCamera(camName);
+    m_name = camName;
     robotToCam = camPose;
 //    if(DriverStation.getAlliance().isPresent()) {
 //      m_alliance = DriverStation.getAlliance().get();
@@ -59,13 +71,7 @@ public VisionSubsystem(String camName, Transform3d camPose) {
     }
   }
 
-  @Override
-  public void periodic() {
-    updateInputs();
-    super.periodic();
-  }
-
-  public Optional<EstimatedRobotPose> getPose(Pose2d prevEstimatedRobotPose) {
+  public Optional<VisionUpdate> getPose(Pose2d prevEstimatedRobotPose) {
 
     m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
 
@@ -86,7 +92,16 @@ public Optional<EstimatedRobotPose> getPose(Pose2d prevEstimatedRobotPose) {
 //    }
 
     PhotonPipelineResult camResult = m_camera.getLatestResult();
-    return m_photonPoseEstimator.update(camResult);
+    Optional<EstimatedRobotPose> opPose = m_photonPoseEstimator.update(camResult);
+
+    if (opPose.isEmpty()) {
+      return Optional.empty();
+    } else {
+      EstimatedRobotPose estPose = opPose.get();
+      return Optional.of(new VisionUpdate(estPose.estimatedPose.toPose2d(),
+          VecBuilder.fill(Units.inchesToMeters(0.5), Units.inchesToMeters(0.5), Units.degreesToRadians(15)),
+          estPose.timestampSeconds));
+    }
   }
 
   public int[] getTargetIDs () {
@@ -107,5 +122,40 @@ public int[] getTargetIDs () {
 
     return results;
   }
+
+  public record VisionUpdate(Pose2d pose, Matrix<N3, N1> stdDevs, double timestamp) {
+    public Pose2d apply(Pose2d lastPose, Matrix<N3, N1> q) {
+      // Apply vision updates
+      // Calculate Kalman gains based on std devs
+      // (https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/estimator/)
+      Matrix<N3, N3> visionK = new Matrix<>(Nat.N3(), Nat.N3());
+      var r = new double[3];
+      for (int i = 0; i < 3; ++i) {
+        r[i] = this.stdDevs().get(i, 0) * this.stdDevs().get(i, 0);
+      }
+      for (int row = 0; row < 3; ++row) {
+        if (q.get(row, 0) == 0.0) {
+          visionK.set(row, row, 0.0);
+        } else {
+          visionK.set(
+              row, row, q.get(row, 0) / (q.get(row, 0) + Math.sqrt(q.get(row, 0) * r[row])));
+        }
+      }
+
+      // Calculate twist between current and vision pose
+      var visionTwist = lastPose.log(this.pose());
+
+      // Multiply by Kalman gain matrix
+      var twistMatrix =
+          visionK.times(VecBuilder.fill(visionTwist.dx, visionTwist.dy, visionTwist.dtheta));
+
+      // Apply twist
+      lastPose =
+          lastPose.exp(
+              new Twist2d(twistMatrix.get(0, 0), twistMatrix.get(1, 0), twistMatrix.get(2, 0)));
+
+      return lastPose;
+    }
+  }
 }