AprilTag pose estimation

src/main/java/org/team1540/robot2024/Constants.java

@@ -1,5 +1,6 @@
 package org.team1540.robot2024;
 
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 
@@ -62,6 +63,21 @@ public static class Drivetrain {
         public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
     }
 
+    public static class Vision {
+        public static final String FRONT_CAMERA_NAME = "limelight-front";
+        public static final String REAR_CAMERA_NAME = "limelight-rear";
+
+        // TODO: measure these offsets
+        public static final Pose3d FRONT_CAMERA_POSE = new Pose3d();
+        public static final Pose3d REAR_CAMERA_POSE = new Pose3d();
+
+        public static final double MAX_VISION_DELAY_SECS = 0.08;
+        public static final double MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC = 1.0;
+        public static final double MAX_ACCEPTED_LINEAR_SPEED_MPS = 4.0;
+        public static final double MIN_ACCEPTED_NUM_TAGS = 1;
+        public static final double MAX_ACCEPTED_AVG_TAG_DIST_METERS = 8.0;
+    }
+
     public static class Elevator {
         public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(21.0);
     }

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -14,6 +14,9 @@
 import org.team1540.robot2024.commands.SwerveDriveCommand;
 import org.team1540.robot2024.subsystems.drive.*;
 import org.team1540.robot2024.subsystems.shooter.*;
+import org.team1540.robot2024.subsystems.vision.AprilTagVision;
+import org.team1540.robot2024.subsystems.vision.AprilTagVisionIO;
+import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight;
 import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
 import org.team1540.robot2024.util.swerve.SwerveFactory;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
@@ -30,6 +33,7 @@ public class RobotContainer {
     // Subsystems
     public final Drivetrain drivetrain;
     public final Shooter shooter;
+    public final AprilTagVision aprilTagVision;
 
     // Controller
     public final CommandXboxController driver = new CommandXboxController(0);
@@ -59,6 +63,10 @@ public RobotContainer() {
                                 new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_LEFT, SwerveFactory.SwerveCorner.BACK_LEFT), odometrySignalRefresher),
                                 new ModuleIOTalonFX(SwerveFactory.getModuleMotors(SwerveConfig.BACK_RIGHT, SwerveFactory.SwerveCorner.BACK_RIGHT), odometrySignalRefresher));
                 shooter = new Shooter(new ShooterPivotIOTalonFX(), new FlywheelsIOTalonFX());
+                aprilTagVision = new AprilTagVision(
+                        new AprilTagVisionIOLimelight(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE),
+                        new AprilTagVisionIOLimelight(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE),
+                        drivetrain::addVisionMeasurement);
                 break;
 
             case SIM:
@@ -71,6 +79,7 @@ public RobotContainer() {
                                 new ModuleIOSim(),
                                 new ModuleIOSim());
                 shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
+                aprilTagVision = new AprilTagVision(new AprilTagVisionIO() {}, new AprilTagVisionIO() {}, (ignored) -> {});
                 break;
 
             default:
@@ -83,6 +92,7 @@ public RobotContainer() {
                                 new ModuleIO() {},
                                 new ModuleIO() {});
                 shooter = new Shooter(new ShooterPivotIO() {}, new FlywheelsIO() {});
+                aprilTagVision = new AprilTagVision(new AprilTagVisionIO() {}, new AprilTagVisionIO() {}, (ignored) -> {});
                 break;
         }
 

src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java

@@ -5,10 +5,11 @@
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
@@ -18,6 +19,8 @@
 import org.team1540.robot2024.util.LocalADStarAK;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
+import org.team1540.robot2024.util.vision.TimestampedVisionPose;
+import org.team1540.robot2024.util.vision.VisionPoseAcceptor;
 
 import static org.team1540.robot2024.Constants.Drivetrain.*;
 
@@ -27,10 +30,11 @@ public class Drivetrain extends SubsystemBase {
     private final Module[] modules = new Module[4]; // FL, FR, BL, BR
 
     private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations());
-    private Pose2d pose = new Pose2d();
-    private Rotation2d lastGyroRotation = new Rotation2d();
+    private Rotation2d rawGyroRotation = new Rotation2d();
     private boolean forceModuleAngleChange = false;
 
+    private final SwerveDrivePoseEstimator poseEstimator;
+
     public Drivetrain(
             GyroIO gyroIO,
             ModuleIO flModuleIO,
@@ -43,6 +47,15 @@ public Drivetrain(
         modules[2] = new Module(blModuleIO, 2);
         modules[3] = new Module(brModuleIO, 3);
 
+        poseEstimator = new SwerveDrivePoseEstimator(
+                kinematics,
+                rawGyroRotation,
+                getModulePositions(),
+                new Pose2d(),
+                // TODO: tune std devs (scale vision by distance?)
+                VecBuilder.fill(0.1, 0.1, 0.1),
+                VecBuilder.fill(0.5, 0.5, 5.0)); // Trust the gyro more than the AprilTags
+
         // Configure AutoBuilder for PathPlanner
         AutoBuilder.configureHolonomic(
                 this::getPose,
@@ -60,44 +73,35 @@ public Drivetrain(
                 (targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose));
     }
 
+    @Override
     public void periodic() {
         gyroIO.updateInputs(gyroInputs);
         Logger.processInputs("Drivetrain/Gyro", gyroInputs);
         for (Module module : modules) {
             module.periodic();
         }
 
-        // Stop moving when disabled
-        if (DriverStation.isDisabled()) {
-            for (Module module : modules) {
-                module.stop();
-            }
-        }
-        // Log empty setpoint states when disabled
         if (DriverStation.isDisabled()) {
+            // Stop moving when disabled
+            for (Module module : modules) module.stop();
+
+            // Log empty setpoint states when disabled
             Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[]{});
             Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{});
         }
 
-        // Update odometry
+        // Calculate module deltas
         SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4];
         for (int i = 0; i < 4; i++) {
             wheelDeltas[i] = modules[i].getPositionDelta();
         }
-        // The twist represents the motion of the robot since the last
-        // loop cycle in x, y, and theta based only on the modules,
-        // without the gyro. The gyro is always disconnected in simulation.
-        Twist2d twist = kinematics.toTwist2d(wheelDeltas);
-        if (gyroInputs.connected) {
-            // If the gyro is connected, replace the theta component of the twist
-            // with the change in angle since the last loop cycle.
-            twist =
-                    new Twist2d(
-                            twist.dx, twist.dy, gyroInputs.yawPosition.minus(lastGyroRotation).getRadians());
-            lastGyroRotation = gyroInputs.yawPosition;
-        }
-        // Apply the twist (change since last loop cycle) to the current pose
-        pose = pose.exp(twist);
+        // Use gyro rotation if gyro is connected, otherwise use module deltas to calculate rotation delta
+        rawGyroRotation =
+                gyroInputs.connected ?
+                        gyroInputs.yawPosition
+                        : rawGyroRotation.plus(Rotation2d.fromRadians(kinematics.toTwist2d(wheelDeltas).dtheta));
+        // Update odometry
+        poseEstimator.update(rawGyroRotation, getModulePositions());
     }
 
     /**
@@ -181,21 +185,37 @@ private SwerveModuleState[] getModuleStates() {
      */
     @AutoLogOutput(key = "Odometry/Robot")
     public Pose2d getPose() {
-        return pose;
+        return poseEstimator.getEstimatedPosition();
     }
 
     /**
      * Returns the current odometry rotation.
      */
     public Rotation2d getRotation() {
-        return pose.getRotation();
+        return getPose().getRotation();
     }
 
     /**
      * Resets the current odometry pose.
      */
     public void setPose(Pose2d pose) {
-        this.pose = pose;
+        poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose);
+    }
+
+    public void addVisionMeasurement(TimestampedVisionPose visionPose) {
+        if (visionPose == null) return;
+        if (VisionPoseAcceptor.shouldAcceptVision(visionPose, kinematics.toChassisSpeeds(getModuleStates()))) {
+            poseEstimator.addVisionMeasurement(visionPose.poseMeters(), visionPose.timestampSecs());
+        }
+    }
+
+    public SwerveModulePosition[] getModulePositions() {
+        return new SwerveModulePosition[]{
+                modules[0].getPosition(),
+                modules[1].getPosition(),
+                modules[2].getPosition(),
+                modules[3].getPosition(),
+        };
     }
 
     /**

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

@@ -0,0 +1,84 @@
+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;
+import java.util.function.Consumer;
+
+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 final Consumer<TimestampedVisionPose> visionPoseConsumer;
+
+    private TimestampedVisionPose frontPose =
+            new TimestampedVisionPose(-1, new Pose2d(), new int[0], new Pose2d[0]);
+    private TimestampedVisionPose rearPose =
+            new TimestampedVisionPose(-1, new Pose2d(), new int[0], new Pose2d[0]);
+
+    public AprilTagVision(
+            AprilTagVisionIO frontCameraIO,
+            AprilTagVisionIO rearCameraIO,
+            Consumer<TimestampedVisionPose> visionPoseConsumer) {
+        this.frontCameraIO = frontCameraIO;
+        this.rearCameraIO = rearCameraIO;
+        this.visionPoseConsumer = visionPoseConsumer;
+    }
+
+    @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());
+        }
+
+        TimestampedVisionPose latestPose = getEstimatedPose();
+        Logger.recordOutput("Vision/EstimatedPose", latestPose == null ? new Pose2d() : latestPose.poseMeters());
+        visionPoseConsumer.accept(latestPose);
+    }
+
+    /**
+     * Gets the estimated pose by fusing individual computed poses from each camera.
+     * Returns null if no tags seen or in simulation
+     */
+    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

@@ -0,0 +1,21 @@
+package org.team1540.robot2024.subsystems.vision;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface AprilTagVisionIO {
+    @AutoLog
+    class AprilTagVisionIOInputs {
+        public Pose2d estimatedPoseMeters = new Pose2d();
+        public boolean hasTarget = false;
+        public int[] seenTagIDs = {};
+        public Pose2d[] tagPosesMeters = {};
+        public double lastMeasurementTimestampSecs = 0.0;
+    }
+
+    default void updateInputs(AprilTagVisionIOInputs inputs) {}
+
+    default String getName() {
+        return "";
+    }
+}

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

@@ -0,0 +1,57 @@
+package org.team1540.robot2024.subsystems.vision;
+
+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 {
+    private final String name;
+
+    public AprilTagVisionIOLimelight(String name, Pose3d cameraOffsetMeters) {
+        this.name = name;
+        LimelightHelpers.setCameraPose_RobotSpace(
+                name,
+                cameraOffsetMeters.getX(),
+                cameraOffsetMeters.getY(),
+                cameraOffsetMeters.getZ(),
+                Math.toDegrees(cameraOffsetMeters.getRotation().getX()),
+                Math.toDegrees(cameraOffsetMeters.getRotation().getY()),
+                Math.toDegrees(cameraOffsetMeters.getRotation().getZ()));
+    }
+
+    @Override
+    public void updateInputs(AprilTagVisionIOInputs inputs) {
+        LimelightHelpers.Results results = LimelightHelpers.getLatestResults(name).targetingResults;
+        LimelightHelpers.LimelightTarget_Fiducial[] fiducialTargets = results.targets_Fiducials;
+
+        inputs.estimatedPoseMeters = new Pose3d(
+                results.botpose_wpiblue[0],
+                results.botpose_wpiblue[1],
+                results.botpose_wpiblue[2],
+                new Rotation3d(
+                        Math.toRadians(results.botpose_wpiblue[3]),
+                        Math.toRadians(results.botpose_wpiblue[4]),
+                        Math.toRadians(results.botpose_wpiblue[5])
+                )
+        ).toPose2d();
+        inputs.hasTarget = results.valid;
+
+        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
+    public String getName() {
+        return name;
+    }
+}