AprilTag pose estimation

src/main/java/org/team1540/robot2024/Constants.java

@@ -1,9 +1,13 @@
 package org.team1540.robot2024;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 
+import java.io.IOException;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
  * constants. This class should not be used for any other purpose. All constants should be declared
@@ -78,6 +82,15 @@ public static class Vision {
         public static final double MIN_ACCEPTED_NUM_TAGS = 1;
         public static final double MAX_ACCEPTED_AVG_TAG_DIST_METERS = 8.0;
         public static final double MAX_ACCEPTED_ELEVATOR_SPEED_MPS = 0.05;
+
+        public static final AprilTagFieldLayout SIM_APRILTAG_LAYOUT;
+        static {
+            try {
+                SIM_APRILTAG_LAYOUT = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile);
+            } catch (IOException e) {
+                throw new RuntimeException(e);
+            }
+        }
     }
 
     public static class Elevator {

src/main/java/org/team1540/robot2024/RobotContainer.java

@@ -17,6 +17,7 @@
 import org.team1540.robot2024.subsystems.vision.AprilTagVision;
 import org.team1540.robot2024.subsystems.vision.AprilTagVisionIO;
 import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight;
+import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOSim;
 import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher;
 import org.team1540.robot2024.util.swerve.SwerveFactory;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
@@ -84,11 +85,11 @@ public RobotContainer() {
                 shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim());
                 aprilTagVision =
                         new AprilTagVision(
-                                new AprilTagVisionIO() {},
-                                new AprilTagVisionIO() {},
-                                (ignored) -> {},
-                                () -> 0.0,
-                                new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0));
+                                new AprilTagVisionIOSim(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE, drivetrain::getPose),
+                                new AprilTagVisionIOSim(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE, drivetrain::getPose),
+                                ignored -> {},
+                                () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE
+                                new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE
                 break;
 
             default:

src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java

@@ -0,0 +1,90 @@
+package org.team1540.robot2024.subsystems.vision;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform3d;
+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;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import java.util.List;
+import java.util.Optional;
+import java.util.function.Supplier;
+
+import static org.team1540.robot2024.Constants.Vision.*;
+
+public class AprilTagVisionIOSim implements AprilTagVisionIO {
+    private final VisionSystemSim visionSystemSim;
+    private final PhotonCameraSim cameraSim;
+    private final PhotonCamera camera;
+    private final PhotonPoseEstimator photonEstimator;
+
+    private final Supplier<Pose2d> poseSupplier;
+
+    private Transform3d cameraTransform;
+    private Pose2d lastEstimatedPose;
+
+    public AprilTagVisionIOSim(String name, Pose3d cameraOffsetMeters, Supplier<Pose2d> poseSupplier) {
+        this.visionSystemSim = new VisionSystemSim(name);
+        this.poseSupplier = poseSupplier;
+        visionSystemSim.addAprilTags(SIM_APRILTAG_LAYOUT);
+
+        SimCameraProperties cameraProps = new SimCameraProperties();
+        cameraProps.setCalibration(1280, 960, Rotation2d.fromDegrees(100));
+        cameraProps.setFPS(14.5);
+        cameraProps.setAvgLatencyMs(67.0);
+        camera = new PhotonCamera(name);
+        cameraSim = new PhotonCameraSim(camera, cameraProps);
+
+        cameraTransform = new Transform3d(cameraOffsetMeters.getTranslation(), cameraOffsetMeters.getRotation());
+        visionSystemSim.addCamera(cameraSim, cameraTransform);
+
+        photonEstimator = new PhotonPoseEstimator(
+                SIM_APRILTAG_LAYOUT,
+                PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
+                camera,
+                cameraTransform);
+        photonEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_LAST_POSE);
+        lastEstimatedPose = poseSupplier.get();
+    }
+
+    @Override
+    public void updateInputs(AprilTagVisionIOInputs inputs) {
+        photonEstimator.setReferencePose(lastEstimatedPose);
+        visionSystemSim.update(poseSupplier.get());
+
+        Optional<EstimatedRobotPose> estimatedPose = photonEstimator.update();
+        List<PhotonTrackedTarget> trackedTargets = camera.getLatestResult().getTargets();
+
+        if (estimatedPose.isPresent()) {
+            lastEstimatedPose = estimatedPose.get().estimatedPose.toPose2d();
+            inputs.estimatedPoseMeters = lastEstimatedPose;
+            inputs.lastMeasurementTimestampSecs = estimatedPose.get().timestampSeconds;
+        }
+        inputs.hasTarget = !trackedTargets.isEmpty();
+        inputs.seenTagIDs = new int[trackedTargets.size()];
+        inputs.tagPosesMeters = new Pose2d[trackedTargets.size()];
+        for (int i = 0; i < trackedTargets.size(); i++) {
+            PhotonTrackedTarget target = trackedTargets.get(i);
+            inputs.seenTagIDs[i] = target.getFiducialId();
+            inputs.tagPosesMeters[i] =
+                    new Pose3d().plus(target.getBestCameraToTarget().plus(cameraTransform.inverse())).toPose2d();
+        }
+    }
+
+    @Override
+    public void setPoseOffset(Pose3d newPose) {
+        cameraTransform = new Transform3d(newPose.getTranslation(), newPose.getRotation());
+        visionSystemSim.adjustCamera(cameraSim, cameraTransform);
+    }
+
+    @Override
+    public String getName() {
+        return camera.getName();
+    }
+}

vendordeps/photonlib-json-1.0.json

@@ -0,0 +1,57 @@
+{
+  "fileName": "photonlib.json",
+  "name": "photonlib",
+  "version": "v2024.2.2",
+  "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+  "frcYear": "2024",
+  "mavenUrls": [
+    "https://maven.photonvision.org/repository/internal",
+    "https://maven.photonvision.org/repository/snapshots"
+  ],
+  "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+  "jniDependencies": [],
+  "cppDependencies": [
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photonlib-cpp",
+      "version": "v2024.2.2",
+      "libName": "photonlib",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-cpp",
+      "version": "v2024.2.2",
+      "libName": "photontargeting",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ],
+  "javaDependencies": [
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photonlib-java",
+      "version": "v2024.2.2"
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-java",
+      "version": "v2024.2.2"
+    }
+  ]
+}