Camera sim improvements

src/main/java/org/curtinfrc/frc2025/Constants.java

@@ -15,7 +15,7 @@
  * (log replay from a file).
  */
 public final class Constants {
-  public static final RobotType robotType = RobotType.COMPBOT;
+  public static final RobotType robotType = RobotType.SIMBOT;
   public static final double ROBOT_X = 0.705;
   public static final double ROBOT_Y = 0.730;
   public static final double FIELD_LENGTH = aprilTagLayout.getFieldLength();

src/main/java/org/curtinfrc/frc2025/Robot.java

@@ -216,10 +216,14 @@ public Robot() {
           vision =
               new Vision(
                   drive::addVisionMeasurement,
-                  new VisionIOPhotonVisionSim(camera0Name, robotToCamera0, drive::getPose),
-                  new VisionIOPhotonVisionSim(camera1Name, robotToCamera1, drive::getPose),
-                  new VisionIOPhotonVisionSim(camera2Name, robotToCamera2, drive::getPose),
-                  new VisionIOPhotonVisionSim(camera3Name, robotToCamera3, drive::getPose));
+                  new VisionIOPhotonVisionSim(
+                      camera0Name, robotToCamera0, drive::getSimGroundTruth),
+                  new VisionIOPhotonVisionSim(
+                      camera1Name, robotToCamera1, drive::getSimGroundTruth),
+                  new VisionIOPhotonVisionSim(
+                      camera2Name, robotToCamera2, drive::getSimGroundTruth),
+                  new VisionIOPhotonVisionSim(
+                      camera3Name, robotToCamera3, drive::getSimGroundTruth));
 
           elevator = new Elevator(new ElevatorIOSim());
           intake = new Intake(new IntakeIOSim());

src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java

@@ -23,6 +23,7 @@
 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.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.numbers.N1;
@@ -44,6 +45,7 @@
 import java.text.NumberFormat;
 import java.util.LinkedList;
 import java.util.List;
+import java.util.Optional;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 import java.util.function.DoubleSupplier;
@@ -74,7 +76,8 @@ public class Drive extends SubsystemBase {
         new SwerveModulePosition(),
         new SwerveModulePosition()
       };
-  private SwerveDrivePoseEstimator poseEstimator =
+  private final Optional<SwerveDriveOdometry> simGroundTruth;
+  private final SwerveDrivePoseEstimator poseEstimator =
       new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, Pose2d.kZero);
 
   private final PIDController xController = new PIDController(3.5, 0, 0);
@@ -151,6 +154,15 @@ public Drive(
     headingController.enableContinuousInput(-Math.PI, Math.PI);
 
     headingFollower.enableContinuousInput(-Math.PI, Math.PI);
+
+    if (Constants.getMode() == Constants.Mode.SIM) {
+      simGroundTruth =
+          Optional.of(
+              new SwerveDriveOdometry(
+                  kinematics, rawGyroRotation, lastModulePositions, Pose2d.kZero));
+    } else {
+      simGroundTruth = Optional.empty();
+    }
   }
 
   public void followTrajectory(SwerveSample sample) {
@@ -238,6 +250,9 @@ public void periodic() {
 
       // Apply update
       poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
+      if (Constants.getMode() == Constants.Mode.SIM) {
+        simGroundTruth.get().update(rawGyroRotation, modulePositions);
+      }
     }
 
     // Update gyro alert
@@ -689,4 +704,11 @@ private void autoAlign(Pose2d _setpoint) {
   public Command autoAlign(Supplier<Pose2d> _setpoint) {
     return run(() -> autoAlign(_setpoint.get())).withName("AutoAlign");
   }
+
+  public Pose2d getSimGroundTruth() {
+    if (Constants.getMode() != Constants.Mode.SIM) {
+      throw new RuntimeException("You may only use sim ground truth in sim");
+    }
+    return simGroundTruth.get().getPoseMeters();
+  }
 }

src/main/java/org/curtinfrc/frc2025/subsystems/vision/VisionIOPhotonVisionSim.java

@@ -48,7 +48,10 @@ public VisionIOPhotonVisionSim(
 
     // Add sim camera
     var cameraProperties = new SimCameraProperties();
-    cameraProperties.setFPS(80);
+    cameraProperties.setFPS(100);
+    cameraProperties.setCalibError(0.5, 0.5); // TODO
+    cameraProperties.setAvgLatencyMs(15); // TODO
+    cameraProperties.setLatencyStdDevMs(5); // TODO
     cameraSim = new PhotonCameraSim(camera, cameraProperties, aprilTagLayout);
     cameraSim.enableRawStream(false);
     cameraSim.enableProcessedStream(false);