Port basic version of 2024 robot code vision into coppercore

vision/build.gradle

@@ -39,8 +39,8 @@ dependencies {
 
 	implementation 'org.littletonrobotics.akit.junction:junction-core:3.2.1'
 
-	implementation 'org.photonvision:photonlib-java:v2025.0.0-alpha-0'
-	implementation 'org.photonvision:photontargeting-java:v2025.0.0-alpha-0'
+	implementation 'org.photonvision:photonlib-java:v2024.3.1'
+	implementation 'org.photonvision:photontargeting-java:v2024.3.1'
 
 	testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
 

vision/src/main/java/coppercore/vision/Camera.java

@@ -0,0 +1,89 @@
+package coppercore.vision;
+
+import coppercore.vision.CoreVisionConstants.CameraParams;
+import coppercore.vision.VisionLocalizer.CameraMeasurement;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import org.littletonrobotics.junction.Logger;
+
+public class Camera {
+
+    public enum CameraTrustZone {
+        LEFT,
+        RIGHT,
+        MIDDLE,
+    }
+
+    public final String name;
+    public final CameraTrustZone zone;
+
+    private final CameraIO io;
+    private final CameraIOInputsAutoLogged inputs;
+
+    public Camera(CameraParams params, CameraIO io) {
+        name = params.name();
+        zone = params.zone();
+
+        this.io = io;
+        inputs = new CameraIOInputsAutoLogged();
+    }
+
+    public void update() {
+        io.updateInputs(inputs);
+        Logger.processInputs("Vision/" + name, inputs);
+    }
+
+    public boolean hasNewMeasurement() {
+        return inputs.wasAccepted;
+    }
+
+    public boolean isConnected() {
+        return inputs.connected;
+    }
+
+    public CameraMeasurement getLatestMeasurement() {
+        return new CameraMeasurement(
+                inputs.latestFieldToRobot, inputs.latestTimestampSeconds, getLatestVariance());
+    }
+
+    public Matrix<N3, N1> getLatestVariance() {
+        // If the robot is not in teleop, trust cameras based on their location relative to the tags
+        return CoreVisionConstants.lowCameraUncertainty;
+        // TODO: Actually calculate variances!
+        // if (!DriverStation.isTeleop()) {
+        //     switch (zone) {
+        //         case LEFT:
+        //             if (AllianceUtil.isRightOfSpeaker(inputs.latestFieldToRobot.getY(), 2)) {
+        //                 return VisionConstants.lowCameraUncertainty;
+        //             }
+        //             break;
+        //         case RIGHT:
+        //             if (AllianceUtil.isLeftOfSpeaker(inputs.latestFieldToRobot.getY(), 2)) {
+        //                 return VisionConstants.lowCameraUncertainty;
+        //             }
+        //             break;
+        //         case MIDDLE:
+        //             break;
+        //     }
+        // }
+
+        // // If the robot is very close, trust highly
+        // if (inputs.averageTagDistanceM < VisionConstants.skewCutoffDistance) {
+        //     return DriverStation.isTeleop()
+        //             ? VisionConstants.teleopCameraUncertainty
+        //             : VisionConstants.lowCameraUncertainty;
+        //     // If the robot is somewhat close, check if the cameras are at extreme angles, and
+        // trust
+        //     // accordingly
+        // } else if (inputs.averageTagDistanceM < VisionConstants.lowUncertaintyCutoffDistance) {
+        //     return Math.abs(inputs.averageTagYaw.getDegrees()) <
+        // VisionConstants.skewCutoffRotation
+        //             ? VisionConstants.lowCameraUncertainty
+        //             : VisionConstants.highCameraUncertainty;
+        //     // If the robot is past the final distance cutoff, distrust
+        // } else {
+        //     return VisionConstants.highCameraUncertainty;
+        // }
+    }
+}

vision/src/main/java/coppercore/vision/CameraContainer.java

@@ -0,0 +1,11 @@
+package coppercore.vision;
+
+import java.util.List;
+
+/** This is not an AdvantageKit IO interface, but a class to hold cameras and associated data */
+public interface CameraContainer {
+
+    public List<Camera> getCameras();
+
+    public void update();
+}

vision/src/main/java/coppercore/vision/CameraContainerReal.java

@@ -0,0 +1,26 @@
+package coppercore.vision;
+
+import coppercore.vision.CoreVisionConstants.CameraParams;
+import java.util.ArrayList;
+import java.util.List;
+
+public class CameraContainerReal implements CameraContainer {
+
+    private List<Camera> cameras = new ArrayList<>();
+
+    public CameraContainerReal(List<CameraParams> params) {
+        for (CameraParams param : params) {
+            cameras.add(new Camera(param, CameraIOPhoton.fromRealCameraParams(param)));
+        }
+    }
+
+    public List<Camera> getCameras() {
+        return cameras;
+    }
+
+    public void update() {
+        for (Camera camera : cameras) {
+            camera.update();
+        }
+    }
+}

vision/src/main/java/coppercore/vision/CameraContainerReplay.java

@@ -0,0 +1,25 @@
+package coppercore.vision;
+
+import coppercore.vision.CoreVisionConstants.CameraParams;
+import java.util.ArrayList;
+import java.util.List;
+
+public class CameraContainerReplay implements CameraContainer {
+    private List<Camera> cameras = new ArrayList<>();
+
+    public CameraContainerReplay(List<CameraParams> params) {
+        for (CameraParams param : params) {
+            cameras.add(new Camera(param, new CameraIO() {}));
+        }
+    }
+
+    public List<Camera> getCameras() {
+        return cameras;
+    }
+
+    public void update() {
+        for (Camera camera : cameras) {
+            camera.update();
+        }
+    }
+}

vision/src/main/java/coppercore/vision/CameraContainerSim.java

@@ -0,0 +1,99 @@
+package coppercore.vision;
+
+import coppercore.vision.CoreVisionConstants.CameraParams;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.Logger;
+import org.photonvision.simulation.VisionSystemSim;
+
+public class CameraContainerSim implements CameraContainer {
+
+    private VisionSystemSim visionSim = new VisionSystemSim("main");
+
+    private List<Camera> cameras = new ArrayList<>();
+
+    private Supplier<SwerveModuleState[]> getModuleStates;
+    private Pose2d latestOdometryPose;
+    private SwerveModulePosition[] lastModulePositions =
+            new SwerveModulePosition[] {
+                new SwerveModulePosition(),
+                new SwerveModulePosition(),
+                new SwerveModulePosition(),
+                new SwerveModulePosition()
+            };
+    private Timer dtTimer = new Timer();
+
+    SwerveDriveKinematics kinematics;
+
+    public CameraContainerSim(
+            List<CameraParams> params,
+            Pose2d initialPose,
+            SwerveDriveKinematics kinematics,
+            Supplier<SwerveModuleState[]> getModuleStates) {
+        this.getModuleStates = getModuleStates;
+
+        visionSim.addAprilTags(CoreVisionConstants.fieldLayout);
+
+        this.kinematics = kinematics;
+
+        latestOdometryPose =
+                new Pose2d(
+                        initialPose.getX(),
+                        initialPose.getY(),
+                        Rotation2d.fromRadians(initialPose.getRotation().getRadians()));
+
+        for (CameraParams param : params) {
+            cameras.add(
+                    new Camera(param, CameraIOPhoton.fromSimCameraParams(param, visionSim, true)));
+        }
+    }
+
+    public List<Camera> getCameras() {
+        return cameras;
+    }
+
+    public void update() {
+        updateOdometry();
+        visionSim.update(latestOdometryPose);
+        SmartDashboard.putData("PhotonSimField", visionSim.getDebugField());
+
+        for (Camera camera : cameras) {
+            camera.update();
+        }
+    }
+
+    private void updateOdometry() {
+        SwerveModulePosition[] deltas = new SwerveModulePosition[4];
+        SwerveModuleState[] states = getModuleStates.get();
+
+        double dt = dtTimer.get();
+        dtTimer.reset();
+        dtTimer.start();
+
+        for (int i = 0; i < states.length; i++) {
+            deltas[i] =
+                    new SwerveModulePosition(
+                            states[i].speedMetersPerSecond * dt
+                                    - lastModulePositions[i].distanceMeters,
+                            Rotation2d.fromRadians(
+                                    states[i]
+                                            .angle
+                                            .minus(lastModulePositions[i].angle)
+                                            .getRadians()));
+        }
+
+        Twist2d twist = kinematics.toTwist2d(deltas);
+        latestOdometryPose = latestOdometryPose.exp(twist);
+
+        Logger.recordOutput("Vision/GroundTruth", latestOdometryPose);
+    }
+}

vision/src/main/java/coppercore/vision/CameraIO.java

@@ -0,0 +1,28 @@
+package coppercore.vision;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface CameraIO {
+
+    @AutoLog
+    public static class CameraIOInputs {
+        public Pose2d latestFieldToRobot = new Pose2d();
+        public double averageTagDistanceM = 0.0;
+        public Rotation2d averageTagYaw = new Rotation2d();
+        public int nTags = 0;
+
+        public double latestTimestampSeconds = 0.0;
+        public boolean connected = false;
+        public boolean isNewMeasurement = false;
+
+        /**
+         * Whether the new camera measurement was accepted by the initial filters. Always false if
+         * `isNewMeasurement` is false.
+         */
+        public boolean wasAccepted = false;
+    }
+
+    public default void updateInputs(CameraIOInputs inputs) {}
+}