Turret Camera

src/main/java/frc/trigon/robot/RobotContainer.java

@@ -40,7 +40,10 @@
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 public class RobotContainer {
-    public static final RobotPoseEstimator ROBOT_POSE_ESTIMATOR = new RobotPoseEstimator();
+    public static final RobotPoseEstimator ROBOT_POSE_ESTIMATOR = new RobotPoseEstimator(
+            CameraConstants.RIGHT_TURRET_CAMERA,
+            CameraConstants.LEFT_TURRET_CAMERA
+    );
     public static final ObjectPoseEstimator OBJECT_POSE_ESTIMATOR = new ObjectPoseEstimator(
             CameraConstants.OBJECT_POSE_ESTIMATOR_DELETION_THRESHOLD_SECONDS,
             SimulatedGamePieceConstants.GamePieceType.FUEL,

src/main/java/frc/trigon/robot/constants/CameraConstants.java

@@ -5,9 +5,14 @@
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
 import frc.trigon.robot.misc.objectdetection.objectdetectioncamera.ObjectDetectionCamera;
+import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera;
+import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants;
+import frc.trigon.robot.poseestimation.apriltagcamera.DynamicCameraTransform;
+import frc.trigon.robot.poseestimation.robotposeestimator.StandardDeviations;
+import frc.trigon.robot.subsystems.turret.TurretCameraTransformCalculator;
 
 public class CameraConstants {
-    private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d(//TODO: AHAHAHAHAH
+    private static final Transform3d ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA = new Transform3d(//TODO: Find actual values
             new Translation3d(0, 0, 0.8),
             new Rotation3d(0, Units.degreesToRadians(30), 0)
     );
@@ -17,4 +22,23 @@ public class CameraConstants {
             "ObjectDetectionCamera",
             ROBOT_CENTER_TO_OBJECT_DETECTION_CAMERA
     );
+
+    private static final StandardDeviations APRIL_TAG_CAMERA_STANDARD_DEVIATIONS = new StandardDeviations(
+            0.015,
+            0.01
+    );
+    public static final AprilTagCamera
+            RIGHT_TURRET_CAMERA = new AprilTagCamera(
+            AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA,
+            "RightTurretCamera",
+            new DynamicCameraTransform(TurretCameraTransformCalculator.getInstance()::calculateRobotToRightCameraAtTime),
+            APRIL_TAG_CAMERA_STANDARD_DEVIATIONS
+    ),
+            LEFT_TURRET_CAMERA = new AprilTagCamera(
+                    AprilTagCameraConstants.AprilTagCameraType.PHOTON_CAMERA,
+                    "LeftTurretCamera",
+                    new DynamicCameraTransform(TurretCameraTransformCalculator.getInstance()::calculateRobotToLeftCameraAtTime),
+                    APRIL_TAG_CAMERA_STANDARD_DEVIATIONS
+            );
+
 }

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java

@@ -1,6 +1,9 @@
 package frc.trigon.robot.poseestimation.apriltagcamera;
 
-import edu.wpi.first.math.geometry.*;
+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 edu.wpi.first.wpilibj.DriverStation;
 import frc.trigon.robot.RobotContainer;
 import frc.trigon.robot.constants.FieldConstants;
@@ -15,31 +18,44 @@
 public class AprilTagCamera {
     protected final String name;
     private final AprilTagCameraInputsAutoLogged inputs = new AprilTagCameraInputsAutoLogged();
-    private final Transform2d cameraToRobotCenter;
+    private final DynamicCameraTransform dynamicCameraTransform;
     private final StandardDeviations standardDeviations;
     private final AprilTagCameraIO aprilTagCameraIO;
     private Pose2d estimatedRobotPose = new Pose2d();
 
     /**
-     * Constructs a new AprilTagCamera.
+     * Constructs a new AprilTagCamera with a static camera transform.
      *
      * @param aprilTagCameraType  the type of camera
      * @param name                the camera's name
-     * @param robotCenterToCamera the transform of the robot's origin point to the camera.
-     *                            only the x, y and yaw values will be used for transforming the camera pose to the robot's center,
-     *                            to avoid more inaccuracies like pitch and roll.
-     *                            The reset will be used for creating a camera in simulation
+     * @param robotCenterToCamera the static transform from robot center to camera
      * @param standardDeviations  the initial calibrated standard deviations for the camera's estimated pose,
-     *                            will be changed as the distance from the tag(s) changes and the number of tags changes
+     *                            adjusted based on distance from tags and number of visible tags
      */
     public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType,
                           String name, Transform3d robotCenterToCamera,
                           StandardDeviations standardDeviations) {
+        this(aprilTagCameraType, name, new DynamicCameraTransform(robotCenterToCamera), standardDeviations);
+    }
+
+    /**
+     * Constructs a new AprilTagCamera with a dynamic camera transform.
+     *
+     * @param aprilTagCameraType     the type of camera
+     * @param name                   the camera's name
+     * @param dynamicCameraTransform the dynamic transform from robot center to camera,
+     *                               supports time-dependent camera positioning
+     * @param standardDeviations     the initial calibrated standard deviations for the camera's estimated pose,
+     *                               adjusted based on distance from tags and number of visible tags
+     */
+    public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType,
+                          String name, DynamicCameraTransform dynamicCameraTransform,
+                          StandardDeviations standardDeviations) {
         this.name = name;
         this.standardDeviations = standardDeviations;
-        this.cameraToRobotCenter = toTransform2d(robotCenterToCamera).inverse();
+        this.dynamicCameraTransform = dynamicCameraTransform;
 
-        aprilTagCameraIO = AprilTagCameraIO.generateIO(aprilTagCameraType, name, robotCenterToCamera);
+        aprilTagCameraIO = AprilTagCameraIO.generateIO(aprilTagCameraType, name, dynamicCameraTransform);
     }
 
     public void update() {
@@ -106,18 +122,18 @@ private Pose2d chooseBestRobotPose() {
         if (!inputs.hasConstrainedResult || isWithinBestTagRangeForAccurateSolvePNPResult())
             return chooseBestNormalSolvePNPPose();
 
-        return cameraPoseToRobotPose(inputs.constrainedSolvePNPPose.toPose2d());
+        return cameraPoseToRobotPose(inputs.constrainedSolvePNPPose.toPose2d(), inputs.latestResultTimestampSeconds);
     }
 
     private Pose2d chooseBestNormalSolvePNPPose() {
-        final Pose2d bestPose = cameraPoseToRobotPose(inputs.bestCameraSolvePNPPose.toPose2d());
+        final Pose2d bestPose = cameraPoseToRobotPose(inputs.bestCameraSolvePNPPose.toPose2d(), inputs.latestResultTimestampSeconds);
 
         if (inputs.bestCameraSolvePNPPose.equals(inputs.alternateCameraSolvePNPPose))
             return bestPose;
         if (inputs.alternateCameraSolvePNPPose.getTranslation().toTranslation2d().getDistance(FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).getTranslation().toTranslation2d()) < 0.1 || DriverStation.isDisabled())
             return bestPose;
 
-        final Pose2d alternatePose = cameraPoseToRobotPose(inputs.alternateCameraSolvePNPPose.toPose2d());
+        final Pose2d alternatePose = cameraPoseToRobotPose(inputs.alternateCameraSolvePNPPose.toPose2d(), inputs.latestResultTimestampSeconds);
         final Rotation2d robotAngleAtResultTime = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(inputs.latestResultTimestampSeconds).getRotation();
 
         final double bestAngleDifference = Math.abs(bestPose.getRotation().minus(robotAngleAtResultTime).getRadians());
@@ -126,8 +142,8 @@ private Pose2d chooseBestNormalSolvePNPPose() {
         return bestAngleDifference > alternateAngleDifference ? alternatePose : bestPose;
     }
 
-    private Pose2d cameraPoseToRobotPose(Pose2d cameraPose) {
-        return cameraPose.transformBy(cameraToRobotCenter);
+    private Pose2d cameraPoseToRobotPose(Pose2d cameraPose, double resultTimestampSeconds) {
+        return dynamicCameraTransform.calculate2dRobotPose(cameraPose, resultTimestampSeconds);
     }
 
     /**
@@ -167,13 +183,6 @@ private void logUsedTags() {
         Logger.recordOutput("UsedTags/" + this.getName(), usedTagPoses);
     }
 
-    private Transform2d toTransform2d(Transform3d transform3d) {
-        final Translation2d robotCenterToCameraTranslation = transform3d.getTranslation().toTranslation2d();
-        final Rotation2d robotCenterToCameraRotation = transform3d.getRotation().toRotation2d();
-
-        return new Transform2d(robotCenterToCameraTranslation, robotCenterToCameraRotation);
-    }
-
     private double calculateAverageDistanceFromTags() {
         double totalDistance = 0;
         for (int visibleTagID : inputs.visibleTagIDs) {

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java

@@ -2,15 +2,14 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform3d;
+import frc.trigon.lib.hardware.RobotHardwareStats;
 import frc.trigon.robot.constants.FieldConstants;
 import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO;
 import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO;
 import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagSimulationCameraIO;
 import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.simulation.SimCameraProperties;
 import org.photonvision.simulation.VisionSystemSim;
-import frc.trigon.lib.hardware.RobotHardwareStats;
 
 import java.util.function.BiFunction;
 
@@ -57,9 +56,9 @@ public enum AprilTagCameraType {
         SIMULATION_CAMERA(AprilTagSimulationCameraIO::new),
         LIMELIGHT(AprilTagLimelightIO::new);
 
-        final BiFunction<String, Transform3d, AprilTagCameraIO> createIOFunction;
+        final BiFunction<String, DynamicCameraTransform, AprilTagCameraIO> createIOFunction;
 
-        AprilTagCameraType(BiFunction<String, Transform3d, AprilTagCameraIO> createIOFunction) {
+        AprilTagCameraType(BiFunction<String, DynamicCameraTransform, AprilTagCameraIO> createIOFunction) {
             this.createIOFunction = createIOFunction;
         }
     }

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java

@@ -1,18 +1,17 @@
 package frc.trigon.robot.poseestimation.apriltagcamera;
 
 import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Transform3d;
-import org.littletonrobotics.junction.AutoLog;
 import frc.trigon.lib.hardware.RobotHardwareStats;
+import org.littletonrobotics.junction.AutoLog;
 
 public class AprilTagCameraIO {
-    static AprilTagCameraIO generateIO(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name, Transform3d robotToCamera) {
+    static AprilTagCameraIO generateIO(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name, DynamicCameraTransform dynamicCameraTransform) {
         if (RobotHardwareStats.isReplay())
             return new AprilTagCameraIO();
         if (RobotHardwareStats.isSimulation())
             aprilTagCameraType = AprilTagCameraConstants.AprilTagCameraType.SIMULATION_CAMERA;
 
-        return aprilTagCameraType.createIOFunction.apply(name, robotToCamera);
+        return aprilTagCameraType.createIOFunction.apply(name, dynamicCameraTransform);
     }
 
     protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) {

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/DynamicCameraTransform.java

@@ -0,0 +1,110 @@
+package frc.trigon.robot.poseestimation.apriltagcamera;
+
+import edu.wpi.first.math.geometry.*;
+
+import java.util.function.Function;
+
+/**
+ * Handles transformations between camera and robot coordinate frames.
+ * Supports both static and time-dependent camera positions, useful for cameras on moving mechanisms.
+ */
+public class DynamicCameraTransform {
+    private final Function<Double, Transform3d> robotCenterToCameraFunction;
+
+    /**
+     * Constructs a DynamicCameraTransform with a static camera position.
+     *
+     * @param robotCenterToCamera the fixed transform from robot center to camera
+     */
+    public DynamicCameraTransform(Transform3d robotCenterToCamera) {
+        this(timestamp -> robotCenterToCamera);
+    }
+
+    /**
+     * Constructs a DynamicCameraTransform with a time-dependent camera position.
+     *
+     * @param robotCenterToCameraFunction function that returns the transform from robot center to camera at a given timestamp
+     */
+    public DynamicCameraTransform(Function<Double, Transform3d> robotCenterToCameraFunction) {
+        this.robotCenterToCameraFunction = robotCenterToCameraFunction;
+    }
+
+    /**
+     * Transforms a camera pose to a robot pose in 2D.
+     *
+     * @param cameraPose       the camera's pose on the field
+     * @param timestampSeconds the timestamp for the camera transform
+     * @return the robot's pose on the field
+     */
+    public Pose2d calculate2dRobotPose(Pose2d cameraPose, double timestampSeconds) {
+        final Transform2d cameraToRobotCenter = get2dCameraToRobotCenter(timestampSeconds);
+        return cameraPose.transformBy(cameraToRobotCenter);
+    }
+
+    /**
+     * Transforms a camera pose to a robot pose in 3D.
+     *
+     * @param cameraPose       the camera's pose in 3D space
+     * @param timestampSeconds the timestamp for the camera transform
+     * @return the robot's pose in 3D space
+     */
+    public Pose3d calculate3dRobotPose(Pose3d cameraPose, double timestampSeconds) {
+        final Transform3d cameraToRobotCenter = get3dCameraToRobotCenter(timestampSeconds);
+        return cameraPose.transformBy(cameraToRobotCenter);
+    }
+
+    /**
+     * Gets the 2D transform from robot center to camera.
+     * Only x, y, and yaw components are preserved to avoid pitch and roll inaccuracies.
+     *
+     * @param timestampSeconds the timestamp for the transform
+     * @return the 2D transform from robot center to camera
+     */
+    public Transform2d get2dRobotCenterToCamera(double timestampSeconds) {
+        return toTransform2d(get3dRobotCenterToCamera(timestampSeconds));
+    }
+
+    /**
+     * Gets the 2D transform from camera to robot center.
+     * Only x, y, and yaw components are preserved to avoid pitch and roll inaccuracies.
+     *
+     * @param timestampSeconds the timestamp for the transform
+     * @return the 2D transform from camera to robot center
+     */
+    public Transform2d get2dCameraToRobotCenter(double timestampSeconds) {
+        return toTransform2d(get3dCameraToRobotCenter(timestampSeconds));
+    }
+
+    /**
+     * Gets the 3D transform from robot center to camera.
+     *
+     * @param timestampSeconds the timestamp for the transform
+     * @return the 3D transform from robot center to camera
+     */
+    public Transform3d get3dRobotCenterToCamera(double timestampSeconds) {
+        return robotCenterToCameraFunction.apply(timestampSeconds);
+    }
+
+    /**
+     * Gets the 3D transform from camera to robot center.
+     *
+     * @param timestampSeconds the timestamp for the transform
+     * @return the 3D transform from camera to robot center
+     */
+    public Transform3d get3dCameraToRobotCenter(double timestampSeconds) {
+        return get3dRobotCenterToCamera(timestampSeconds).inverse();
+    }
+
+    /**
+     * Converts a 3D transform to a 2D transform using only x, y, and yaw components.
+     *
+     * @param transform3d the 3D transform to convert
+     * @return the 2D transform
+     */
+    private Transform2d toTransform2d(Transform3d transform3d) {
+        final Translation2d robotCenterToCameraTranslation = transform3d.getTranslation().toTranslation2d();
+        final Rotation2d robotCenterToCameraRotation = transform3d.getRotation().toRotation2d();
+
+        return new Transform2d(robotCenterToCameraTranslation, robotCenterToCameraRotation);
+    }
+}

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java

@@ -1,16 +1,16 @@
 package frc.trigon.robot.poseestimation.apriltagcamera.io;
 
 import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Transform3d;
+import frc.trigon.lib.utilities.LimelightHelpers;
 import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO;
 import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged;
-import frc.trigon.lib.utilities.LimelightHelpers;
+import frc.trigon.robot.poseestimation.apriltagcamera.DynamicCameraTransform;
 
 // TODO: Fully implement this class, Limelight currently not supported.
 public class AprilTagLimelightIO extends AprilTagCameraIO {
     private final String hostname;
 
-    public AprilTagLimelightIO(String hostname, Transform3d robotToCamera) {
+    public AprilTagLimelightIO(String hostname, DynamicCameraTransform dynamicCameraTransform) {
         this.hostname = hostname;
     }