feat: AprilTagVision subsystem

Author: mimizh2418

src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java

@@ -0,0 +1,70 @@
+package org.team1540.robot2024.subsystems.vision;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+import org.team1540.robot2024.Constants;
+import org.team1540.robot2024.util.vision.TimestampedVisionPose;
+
+import java.util.Arrays;
+
+public class AprilTagVision extends SubsystemBase {
+    private final AprilTagVisionIO frontCameraIO;
+    private final AprilTagVisionIOInputsAutoLogged frontCameraInputs = new AprilTagVisionIOInputsAutoLogged();
+
+    private final AprilTagVisionIO rearCameraIO;
+    private final AprilTagVisionIOInputsAutoLogged rearCameraInputs = new AprilTagVisionIOInputsAutoLogged();
+
+    private TimestampedVisionPose frontPose;
+    private TimestampedVisionPose rearPose;
+
+    public AprilTagVision(AprilTagVisionIO frontCameraIO, AprilTagVisionIO rearCameraIO) {
+        this.frontCameraIO = frontCameraIO;
+        this.rearCameraIO = rearCameraIO;
+    }
+
+    @Override
+    public void periodic() {
+        frontCameraIO.updateInputs(frontCameraInputs);
+        rearCameraIO.updateInputs(rearCameraInputs);
+        Logger.processInputs("Vision/FrontCamera", frontCameraInputs);
+        Logger.processInputs("Vision/RearCamera", rearCameraInputs);
+
+        if (frontCameraInputs.lastMeasurementTimestampSecs > frontPose.timestampSecs()) {
+            frontPose = new TimestampedVisionPose(
+                    frontCameraInputs.lastMeasurementTimestampSecs,
+                    frontCameraInputs.estimatedPoseMeters,
+                    frontCameraInputs.seenTagIDs,
+                    frontCameraInputs.tagPosesMeters);
+        }
+        if (rearCameraInputs.lastMeasurementTimestampSecs > rearPose.timestampSecs()) {
+            rearPose = new TimestampedVisionPose(
+                    rearCameraInputs.lastMeasurementTimestampSecs,
+                    rearCameraInputs.estimatedPoseMeters,
+                    rearCameraInputs.seenTagIDs,
+                    rearPose.tagPosesMeters());
+        }
+    }
+
+    /**
+     * Gets the estimated pose by fusing individual computed poses from each camera
+     */
+    public TimestampedVisionPose getEstimatedPose() {
+        if (Constants.currentMode == Constants.Mode.SIM) return null;
+        if (frontPose.getNumTagsSeen() < 1 && rearPose.getNumTagsSeen() < 1) return null;
+        else if (frontPose.getNumTagsSeen() < 1) return rearPose;
+        else if (rearPose.getNumTagsSeen() < 1) return frontPose;
+
+        // This just takes the average of the measurements, we could change this to something more advanced if necessary
+        int[] allTagIDs = Arrays.copyOf(frontPose.seenTagIDs(), frontPose.getNumTagsSeen() + rearPose.getNumTagsSeen());
+        System.arraycopy(rearPose.seenTagIDs(), 0, allTagIDs, frontPose.getNumTagsSeen(), rearPose.getNumTagsSeen());
+        Pose2d[] allTagPoses = Arrays.copyOf(frontPose.tagPosesMeters(), frontPose.getNumTagsSeen() + rearPose.getNumTagsSeen());
+        System.arraycopy(rearPose.tagPosesMeters(), 0, allTagPoses, frontPose.getNumTagsSeen(), rearPose.getNumTagsSeen());
+
+        return new TimestampedVisionPose(
+                (frontPose.timestampSecs() + rearPose.timestampSecs()) / 2,
+                frontPose.poseMeters().interpolate(rearPose.poseMeters(), 0.5),
+                allTagIDs,
+                allTagPoses);
+    }
+}

src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java

@@ -8,11 +8,9 @@ public interface AprilTagVisionIO {
     class AprilTagVisionIOInputs {
         public Pose2d estimatedPoseMeters = new Pose2d();
         public boolean hasTarget = false;
-        public int[] seenTargetIDs = {};
-        public Pose2d[] targetPosesMeters = {};
-
-        public double pipelineLatency = 0.0;
-        public double captureLatency = 0.0;
+        public int[] seenTagIDs = {};
+        public Pose2d[] tagPosesMeters = {};
+        public double lastMeasurementTimestampSecs = 0.0;
     }
 
     default void updateInputs(AprilTagVisionIOInputs inputs) {}

src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java

@@ -3,6 +3,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.wpilibj.Timer;
 import org.team1540.robot2024.util.vision.LimelightHelpers;
 
 public class AprilTagVisionIOLimelight implements AprilTagVisionIO {
@@ -36,13 +37,17 @@ public void updateInputs(AprilTagVisionIOInputs inputs) {
                 )
         ).toPose2d();
         inputs.hasTarget = results.valid;
-        inputs.seenTargetIDs = new int[fiducialTargets.length];
-        for (int i = 0; i < fiducialTargets.length; i++) inputs.seenTargetIDs[i] = (int) fiducialTargets[i].fiducialID; // fiducial tag ids had better be ints
-        inputs.targetPosesMeters = new Pose2d[fiducialTargets.length];
-        for (int i = 0; i < fiducialTargets.length; i++) inputs.targetPosesMeters[i] = fiducialTargets[i].getTargetPose_RobotSpace2D();
 
-        inputs.captureLatency = results.latency_capture;
-        inputs.pipelineLatency = results.latency_pipeline;
+        inputs.seenTagIDs = new int[fiducialTargets.length];
+        for (int i = 0; i < fiducialTargets.length; i++)
+            inputs.seenTagIDs[i] = (int) fiducialTargets[i].fiducialID; // fiducial tag ids had better be ints
+
+        inputs.tagPosesMeters = new Pose2d[fiducialTargets.length];
+        for (int i = 0; i < fiducialTargets.length; i++)
+            inputs.tagPosesMeters[i] = fiducialTargets[i].getTargetPose_RobotSpace2D();
+
+        inputs.lastMeasurementTimestampSecs =
+                Timer.getFPGATimestamp() - (results.latency_capture + results.latency_pipeline) / 1000.0;
     }
 
     @Override

src/main/java/org/team1540/robot2024/util/vision/LimelightHelpers.java

@@ -24,6 +24,9 @@
 import com.fasterxml.jackson.databind.DeserializationFeature;
 import com.fasterxml.jackson.databind.ObjectMapper;
 
+// NOTE: This file is available at
+// https://github.com/LimelightVision/limelightlib-wpijava/blob/main/LimelightHelpers.java
+
 public class LimelightHelpers {
 
     public static class LimelightTarget_Retro {

src/main/java/org/team1540/robot2024/util/vision/TimestampedVisionPose.java

@@ -0,0 +1,40 @@
+package org.team1540.robot2024.util.vision;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+import java.util.Arrays;
+import java.util.Objects;
+
+/**
+ * @param timestampSecs  RIO FPGA timestamp of this vision pose
+ * @param poseMeters     Measured, blue alliance-origin pose from vision
+ * @param seenTagIDs     Array of all tags used to compute this measurement
+ * @param tagPosesMeters Pose in robot-space of all tags used to compute this measurement
+ */
+public record TimestampedVisionPose(
+        double timestampSecs,
+        Pose2d poseMeters,
+        int[] seenTagIDs,
+        Pose2d[] tagPosesMeters) {
+
+    public int getNumTagsSeen() {
+        return seenTagIDs.length;
+    }
+
+    public double getAverageTagDistance() {
+        double sumDistances = 0.0;
+        for (Pose2d pose : tagPosesMeters) sumDistances += pose.getTranslation().getNorm();
+        return sumDistances / getNumTagsSeen();
+    }
+
+    @Override
+    public boolean equals(Object o) {
+        if (this == o) return true;
+        if (o == null || getClass() != o.getClass()) return false;
+        TimestampedVisionPose that = (TimestampedVisionPose) o;
+        return Double.compare(timestampSecs, that.timestampSecs) == 0
+                && Objects.equals(poseMeters, that.poseMeters)
+                && Arrays.equals(seenTagIDs, that.seenTagIDs)
+                && Arrays.equals(tagPosesMeters, that.tagPosesMeters);
+    }
+}