WIP - Begin adding vision sim support

vision/src/main/java/CameraParams.java

@@ -1,6 +1,58 @@
 package coppercore.vision;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import org.photonvision.PhotonCamera;
+import org.photonvision.simulation.SimCameraProperties;
 
-public record CameraParams(PhotonCamera camera, Transform3d robotToCamera) {}
+public record CameraParams(
+        PhotonCamera camera, Transform3d robotToCamera, SimCameraProperties simCameraProp) {
+    public static class CameraParamBuilder {
+        String cameraName;
+        Transform3d robotToCamera;
+
+        SimCameraProperties simCameraProp = new SimCameraProperties();
+
+        public CameraParamBuilder withCameraName(String cameraName) {
+            this.cameraName = cameraName;
+
+            return this;
+        }
+
+        public CameraParamBuilder withRobotToCamera(Transform3d robotToCamera) {
+            this.robotToCamera = robotToCamera;
+
+            return this;
+        }
+
+        public CameraParamBuilder withCalibration(int resX, int resY, Rotation2d diagFOV) {
+            simCameraProp.setCalibration(resX, resY, diagFOV);
+
+            return this;
+        }
+
+        public CameraParamBuilder withCalibError(
+                float averageErrorPixels, float errorStdDevPixels) {
+            simCameraProp.setCalibError(averageErrorPixels, errorStdDevPixels);
+
+            return this;
+        }
+
+        public CameraParamBuilder withFPS(int fps) {
+            simCameraProp.setFPS(fps);
+
+            return this;
+        }
+
+        public CameraParamBuilder withLatency(double averageLatencyMs, double latencyStdDevMs) {
+            simCameraProp.setAvgLatencyMs(averageLatencyMs);
+            simCameraProp.setLatencyStdDevMs(latencyStdDevMs);
+
+            return this;
+        }
+
+        public CameraParams build() {
+            return new CameraParams(new PhotonCamera(cameraName), robotToCamera, simCameraProp);
+        }
+    }
+}

vision/src/main/java/VisionLocalizer.java

@@ -1,21 +1,19 @@
 package coppercore.vision;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import java.util.Optional;
 import java.util.function.Consumer;
-
 import org.littletonrobotics.junction.Logger;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.simulation.VisionSystemSim;
 import org.photonvision.targeting.PhotonPipelineResult;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 /** A reliable vision subsystem to be used in any robot project */
 public class VisionLocalizer extends SubsystemBase {
     CameraWrapper cameras[];

vision/src/main/java/VisionSimWrapper.java

@@ -0,0 +1,9 @@
+package coppercore.vision;
+
+import edu.wpi.first.math.geometry.Pose3d;
+
+public interface VisionSimWrapper {
+    public void addCamera(CameraParams cameraParams);
+
+    public void update(Pose3d robotPoseMeters);
+}

vision/src/main/java/VisionSimWrapperBlank.java

@@ -0,0 +1,11 @@
+package coppercore.vision;
+
+import edu.wpi.first.math.geometry.Pose3d;
+
+public class VisionSimWrapperBlank implements VisionSimWrapper {
+    @Override
+    public void addCamera(CameraParams cameraParams) {}
+
+    @Override
+    public void update(Pose3d robotPoseMeters) {}
+}

vision/src/main/java/VisionSimWrapperSimulated.java

@@ -0,0 +1,25 @@
+package coppercore.vision;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.geometry.Pose3d;
+import org.photonvision.simulation.PhotonCameraSim;
+import org.photonvision.simulation.VisionSystemSim;
+
+public class VisionSimWrapperSimulated implements VisionSimWrapper {
+    VisionSystemSim visionSim;
+
+    public VisionSimWrapperSimulated(String simName, AprilTagFieldLayout fieldLayout) {
+        visionSim = new VisionSystemSim(simName);
+        visionSim.addAprilTags(fieldLayout);
+    }
+
+    @Override
+    public void addCamera(CameraParams cameraParams) {
+        PhotonCameraSim cameraSim =
+                new PhotonCameraSim(cameraParams.camera(), cameraParams.simCameraProp());
+        visionSim.addCamera(cameraSim, cameraParams.robotToCamera());
+    }
+
+    @Override
+    public void update(Pose3d robotPoseMeters) {}
+}