odometry / vision maybe

src/main/java/frc/robot/subsystems/apriltag/AprilTagConstants.java

@@ -0,0 +1,35 @@
+package frc.robot.subsystems.apriltagvision;
+
+import static frc.robot.Constants.currentRobot;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.util.Units;
+
+public final class AprilTagConstants {
+
+  public static final Transform3d leftCamToRobot = new Transform3d(
+            new Translation3d(
+                Units.inchesToMeters(-12.01),
+                Units.inchesToMeters(11.65),
+                Units.inchesToMeters(10.58)),
+            new Rotation3d(0.0, Units.degreesToRadians(-23.5), Units.degreesToRadians(147)));
+
+
+    public static final Transform3d rightCamToRobot = new Transform3d(
+            new Translation3d(
+                Units.inchesToMeters(-12.01),
+                Units.inchesToMeters(11.65),
+                Units.inchesToMeters(10.58)),
+            new Rotation3d(0.0, Units.degreesToRadians(-23.5), Units.degreesToRadians(147)));
+
+  public static final Matrix<N3, N1> singleTagStdDev =
+      VecBuilder.fill(0.8, 0.8, Double.MAX_VALUE);
+  public static final Matrix<N3, N1> multiTagStdDev =
+      VecBuilder.fill(0.5, 0.5, Double.MAX_VALUE);
+}

src/main/java/frc/robot/subsystems/apriltag/AprilTagVisionIO.java

@@ -0,0 +1,94 @@
+package frc.robot.subsystems.apriltagvision;
+
+import static frc.robot.Constants.aprilTagFieldLayout;
+import static frc.robot.subsystems.apriltagvision.AprilTagConstants.*;
+import static frc.robot.subsystems.apriltagvision.AprilTagConstants.normalMultiTagStdDev;
+
+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.Pose3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import java.util.List;
+import org.littletonrobotics.junction.AutoLog;
+import org.photonvision.EstimatedRobotPose;
+
+public interface AprilTagVisionIO {
+  @AutoLog
+  public static class AprilTagVisionIOInputs {
+    public Pose3d[] visionPoses =
+        List.of(new Pose3d(), new Pose3d(), new Pose3d()).toArray(new Pose3d[0]);
+    public double[] timestamps = new double[3];
+    public double[] latency = new double[3];
+    public double[] visionStdDevs = new double[9];
+  }
+
+  /** Updates the set of loggable inputs. */
+  public default void updateInputs(AprilTagVisionIOInputs inputs) {}
+
+  /** Update the reference pose of the vision system. Currently only used in sim. */
+  public default void updatePose(Pose2d pose) {}
+
+  /**
+   * The standard deviations of the estimated poses from vision cameras, for use with {@link
+   * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}.
+   *
+   * @param estimatedPose The estimated pose to guess standard deviations for.
+   */
+  default Matrix<N3, N1> getEstimationStdDevs(
+      EstimatedRobotPose estimatedPose, CameraResolution resolution) {
+    var estStdDevs =
+        switch (resolution) {
+          case HIGH_RES -> highResSingleTagStdDev;
+          case NORMAL -> normalSingleTagStdDev;
+        };
+    var targets = estimatedPose.targetsUsed;
+    int numTags = 0;
+    double avgDist = 0;
+    for (var tgt : targets) {
+      var tagPose = aprilTagFieldLayout.getTagPose(tgt.getFiducialId());
+      if (tagPose.isEmpty()) continue;
+      numTags++;
+      avgDist +=
+          tagPose
+              .get()
+              .toPose2d()
+              .minus(estimatedPose.estimatedPose.toPose2d())
+              .getTranslation()
+              .getNorm();
+    }
+
+    if (numTags == 0) return estStdDevs;
+    avgDist /= numTags;
+
+    // Decrease std devs if multiple targets are visible
+    if (numTags > 1
+        && avgDist
+            > switch (resolution) {
+              case HIGH_RES -> 8;
+              case NORMAL -> 5;
+            }) {
+      estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+    } else {
+      estStdDevs =
+          switch (resolution) {
+            case HIGH_RES -> highResMultiTagStdDev;
+            case NORMAL -> normalMultiTagStdDev;
+          };
+    }
+    // Increase std devs based on (average) distance
+    if (numTags == 1
+        && avgDist
+            > switch (resolution) {
+              case HIGH_RES -> 6;
+              case NORMAL -> 4;
+            }) {
+      estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+    } else {
+      estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 20));
+    }
+
+    return estStdDevs;
+  }
+}

src/main/java/frc/robot/subsystems/apriltag/AprilTagVisionIOReal.java

@@ -0,0 +1,107 @@
+package frc.robot.subsystems.apriltagvision;
+
+import static frc.robot.Constants.aprilTagFieldLayout;
+import static frc.robot.Constants.currentRobot;
+import static frc.robot.subsystems.apriltagvision.AprilTagConstants.*;
+import static java.lang.System.arraycopy;
+import static org.photonvision.PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Optional;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+
+public class AprilTagVisionIOReal implements AprilTagVisionIO {
+  // Left Side Camera
+  private final PhotonCamera leftCam;
+  private final PhotonPoseEstimator leftPhotonPoseEstimator;
+
+  // Right Side Camera
+  private final PhotonCamera rightCam;
+  private final PhotonPoseEstimator rightPhotonPoseEstimator;
+
+  private Pose3d[] poseArray = new Pose3d[2];
+  private double[] timestampArray = new double[2];
+  private double[] visionStdArray = new double[6];
+  private double[] latencyArray = new double[2];
+  private int count = 0;
+
+    // Left Side Camera
+    leftCam = new PhotonCamera("left");
+    leftPhotonPoseEstimator =
+        new PhotonPoseEstimator(
+            aprilTagFieldLayout, MULTI_TAG_PNP_ON_COPROCESSOR, leftCam, leftCamToRobot);
+
+    // Right Side Camera
+    rightCam = new PhotonCamera("right");
+    rightPhotonPoseEstimator =
+        new PhotonPoseEstimator(
+            aprilTagFieldLayout, MULTI_TAG_PNP_ON_COPROCESSOR, rightCam, rightCamToRobot);
+  }
+
+  @Override
+  public void updateInputs(AprilTagVisionIOInputs inputs) {
+    getEstimatedPoseUpdates();
+    inputs.visionPoses = poseArray;
+    inputs.timestamps = timestampArray;
+    inputs.visionStdDevs = visionStdArray;
+    inputs.latency = latencyArray;
+    count += 1;
+    if (count % 500 == 0) {
+      leftCam.takeOutputSnapshot();
+      rightCam.takeOutputSnapshot();
+    }
+  }
+
+  public void getEstimatedPoseUpdates() {
+    Matrix<N3, N1> infiniteStdevs =
+        VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+
+    pose = leftPhotonPoseEstimator.update();
+    pose.ifPresentOrElse(
+        estimatedRobotPose -> {
+          poseArray[1] = estimatedRobotPose.estimatedPose;
+          timestampArray[1] = estimatedRobotPose.timestampSeconds;
+          Matrix<N3, N1> stdDevs =
+              getEstimationStdDevs(
+                  estimatedRobotPose,
+                  switch (currentRobot) {
+                    case CLEF -> CameraResolution.NORMAL;
+                    case LIGHTCYCLE -> CameraResolution.HIGH_RES;
+                  });
+          arraycopy(stdDevs.getData(), 0, visionStdArray, 3, 3);
+          latencyArray[1] = leftCam.getLatestResult().getLatencyMillis() / 1.0e3;
+        },
+        () -> {
+          poseArray[1] = new Pose3d();
+          timestampArray[1] = 0.0;
+          latencyArray[1] = 0.0;
+        });
+
+    pose = rightPhotonPoseEstimator.update();
+    pose.ifPresentOrElse(
+        estimatedRobotPose -> {
+          poseArray[2] = estimatedRobotPose.estimatedPose;
+          timestampArray[2] = estimatedRobotPose.timestampSeconds;
+          Matrix<N3, N1> stdDevs =
+              getEstimationStdDevs(
+                  estimatedRobotPose,
+                  switch (currentRobot) {
+                    case CLEF -> CameraResolution.NORMAL;
+                    case LIGHTCYCLE -> CameraResolution.HIGH_RES;
+                  });
+          arraycopy(stdDevs.getData(), 0, visionStdArray, 6, 3);
+          latencyArray[2] = leftCam.getLatestResult().getLatencyMillis() / 1.0e3;
+        },
+        () -> {
+          poseArray[2] = new Pose3d();
+          timestampArray[2] = 0.0;
+          latencyArray[2] = 0.0;
+        });
+  }
+}

src/main/java/frc/robot/subsystems/apriltag/AprilTagVisionIOSim.java

@@ -0,0 +1,106 @@
+package frc.robot.subsystems.apriltagvision;
+
+import static frc.robot.Constants.aprilTagFieldLayout;
+import static frc.robot.subsystems.apriltagvision.AprilTagConstants.*;
+import static java.lang.System.arraycopy;
+import static org.photonvision.PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.*;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import java.util.Optional;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.SimCameraProperties;
+import org.photonvision.simulation.VisionSystemSim;
+
+public class AprilTagVisionIOSim implements AprilTagVisionIO {
+  private final VisionSystemSim visionSim;
+
+  // Left Side Camera
+  private final PhotonCameraSim leftCam;
+  private final PhotonPoseEstimator leftPhotonPoseEstimator;
+
+  // Right Side Camera
+  private final PhotonCameraSim rightCam;
+  private final PhotonPoseEstimator rightPhotonPoseEstimator;
+
+  private Pose3d[] poseArray = new Pose3d[3];
+  private double[] timestampArray = new double[3];
+  private double[] visionStdArray = new double[9];
+
+  public AprilTagVisionIOSim() {
+    PhotonCamera left = new PhotonCamera("left");
+    PhotonCamera right = new PhotonCamera("right");
+
+    leftPhotonPoseEstimator =
+        new PhotonPoseEstimator(
+            aprilTagFieldLayout, MULTI_TAG_PNP_ON_COPROCESSOR, left, leftCamToRobot);
+    rightPhotonPoseEstimator =
+        new PhotonPoseEstimator(
+            aprilTagFieldLayout, MULTI_TAG_PNP_ON_COPROCESSOR, right, rightCamToRobot);
+
+    visionSim = new VisionSystemSim("main");
+    visionSim.addAprilTags(aprilTagFieldLayout);
+
+    SimCameraProperties sideCameraProp = new SimCameraProperties(); // Arducam OV9281, not Spinel
+    sideCameraProp.setCalibration(1280, 800, Rotation2d.fromDegrees(84.47));
+    sideCameraProp.setCalibError(0.25, 0.10);
+    sideCameraProp.setFPS(40);
+    sideCameraProp.setAvgLatencyMs(40);
+    sideCameraProp.setLatencyStdDevMs(10);
+
+    leftCam = new PhotonCameraSim(left, sideCameraProp);
+    rightCam = new PhotonCameraSim(right, sideCameraProp);
+
+    visionSim.addCamera(leftCam, leftCamToRobot);
+    visionSim.addCamera(rightCam, rightCamToRobot);
+
+    leftCam.enableDrawWireframe(true);
+    rightCam.enableDrawWireframe(true);
+  }
+
+  @Override
+  public void updateInputs(AprilTagVisionIOInputs inputs) {
+    getEstimatedPoseUpdates();
+    inputs.visionPoses = poseArray;
+    inputs.timestamps = timestampArray;
+    inputs.visionStdDevs = visionStdArray;
+  }
+
+  public void updatePose(Pose2d pose) {
+    visionSim.update(pose);
+  }
+
+  public void getEstimatedPoseUpdates() {
+    pose = leftPhotonPoseEstimator.update();
+    pose.ifPresentOrElse(
+        estimatedRobotPose -> {
+          poseArray[1] = estimatedRobotPose.estimatedPose;
+          timestampArray[1] = estimatedRobotPose.timestampSeconds;
+          Matrix<N3, N1> stdDevs =
+              getEstimationStdDevs(estimatedRobotPose, CameraResolution.NORMAL);
+          arraycopy(stdDevs.getData(), 0, visionStdArray, 3, 3);
+        },
+        () -> {
+          poseArray[1] = new Pose3d();
+          timestampArray[1] = 0.0;
+        });
+    pose = rightPhotonPoseEstimator.update();
+    pose.ifPresentOrElse(
+        estimatedRobotPose -> {
+          poseArray[2] = estimatedRobotPose.estimatedPose;
+          timestampArray[2] = estimatedRobotPose.timestampSeconds;
+          Matrix<N3, N1> stdDevs =
+              getEstimationStdDevs(estimatedRobotPose, CameraResolution.NORMAL);
+          arraycopy(stdDevs.getData(), 0, visionStdArray, 6, 3);
+        },
+        () -> {
+          poseArray[2] = new Pose3d();
+          timestampArray[2] = 0.0;
+        });
+  }
+}