[swerve] pass vision measurements to odometry

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

@@ -50,4 +50,7 @@ public final class Constants {
 
   public static final double DrivebaseMaxSpeed = TunerConstants.kSpeedAt12VoltsMps;
   public static final double DrivebaseMaxAngularRate = 1.5 * Math.PI;
+
+  public static final double fieldX = 16.56588;
+  public static final double fieldY = 8.160512;
 }

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -23,7 +23,12 @@
 import com.pathplanner.lib.commands.*;
 import com.pathplanner.lib.util.*;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.networktables.BooleanPublisher;
 import edu.wpi.first.networktables.NetworkTable;
@@ -41,6 +46,7 @@
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.Constants;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.generated.TunerConstants;
 import java.util.ArrayList;
@@ -55,6 +61,8 @@
  */
 @SuppressWarnings("PMD.SingularField")
 public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem {
+  private final NetworkTable m_limelight = NetworkTableInstance.getDefault().getTable("limelight");
+  private Pose3d m_botpose;
   private final NetworkTable driveStats = NetworkTableInstance.getDefault().getTable("Drive");
   private final StringPublisher m_activeCommand =
       driveStats.getStringTopic("Active Command").publish();
@@ -277,13 +285,44 @@ public void periodic() {
               });
     }
 
+    if (DriverStation.getAlliance().isPresent()) {
+      double[] data;
+      if (DriverStation.getAlliance().get() == Alliance.Blue) {
+        data = m_limelight.getEntry("botpose_wpiblue").getDoubleArray(new double[6]);
+      } else {
+        data = m_limelight.getEntry("botpose_wpired").getDoubleArray(new double[6]);
+      }
+
+      if ((int) data[7] < 2) {
+        DataLogManager.log("Couldn't see enough tags, skipping vision measurement.");
+        return;
+      }
+
+      m_botpose =
+          new Pose3d(
+              new Translation3d(data[0], data[1], data[2]),
+              new Rotation3d(data[4], data[5], data[6]));
+
+      if (reasonablePose(m_botpose)) {
+        addVisionMeasurement(m_botpose.toPose2d(), frc.robot.Utils.now());
+      }
+    }
+
     var currentcommand = getCurrentCommand();
     if (currentcommand != null) {
       m_activeCommand.set(currentcommand.toString());
       m_activeCommandFinished.set(currentcommand.isFinished());
     }
   }
 
+  private boolean reasonablePose(Pose3d pose) {
+    return !((Math.abs(pose.getZ()) > 0.1)
+        && (pose.getX() > Constants.fieldX)
+        && (pose.getX() < 0)
+        && (pose.getY() > Constants.fieldY)
+        && (pose.getY() < 0));
+  }
+
   /**
    * Follow the given trajectory.
    *