T265 (#13)

* Reworked `PoseEstimator6328` as `SwervePoseEstimator`

* Removed `PoseEstimator6328`

* Removed unused variable

* Removed unnecessary records and added new ones 😁

* Moved it all to one file

* The Copilot thing aint too smart

* Jd

* Update src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* 🤦‍♂️

* ALWAYS MAKE SURE TO UPDATE BEFORE READING!!!!!!!!!!!

* Added T265

* Small cleaning

* Removed command that shouldn't be here

* Removed whitespace

* Updated logging

* Logic changes

* Optimization

* Moved T265 to CameraConstants

* Cleaned AprilTagCamera stddevs

* Update CameraConstants.java

* Finally fixed logic!!!!!!!!!!!!!!! (I think)

* LLL

* Small clean

* Swerve isn't working, but the other stuff should be

* Removed stupid indent that looks like a change

* Logic works!!! Swerve rotation has no hope

* IT IS NOT WORKING RIGHT NOW, JUST COMMITING

* Fixed half of problem

* Fixed other problem

* Actually fixed logic

* Added letency prevention logci i have no idea what im doingggggggggggggggggggg

* Updated jd

* Squashed commit of the following:

commit 68756ee
Author: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com>
Date:   Mon Dec 23 12:58:26 2024 +0200

    Updated for quickorttrigonlib

commit 04cc361
Author: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com>
Date:   Mon Dec 23 09:09:39 2024 +0200

    Small cleaning

commit de329a9
Author: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com>
Date:   Sat Dec 21 18:58:55 2024 +0200

    Cleaned null translation check

commit e4fa986
Author: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com>
Date:   Thu Dec 19 12:55:32 2024 +0200

    Fixed swerve problem and odometry update problem

commit b297f61
Author: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com>
Date:   Thu Dec 19 12:50:43 2024 +0200

    Cleaning

* Fixed error

* Use Stddevs record function

* Fixed stddevs bug

* Lowered tolerance

* jd + small stuff

* jd robotcentertocam cleaning

* refactor

* Added stuff -JSON

* jd

* Removed Pinhole

* Generalized and cleaned

* Json stuff

* Maybe fixed json stuff?

* Update RelativeRobotPoseSourceT265IO.java

* Update RelativeRobotPoseSourceConstants.java

* Update JSON stuff

* Added replay support + fixes

* 😧

* Lots

* Correct JsonDump logic

* Fixed bugs (updated)

* nnnnnnnnnnnnn

* me made fix

* We don't need this

* Reverted logic (conflicts suck ig)

* oops

* jd

* Simplified

* Update PoseEstimator.java

* Update PoseEstimator.java

* Resolve error

* Made variable that can be final final

* Remove unused command

* Be more efficient with cameraToRobot and explain with a jdoc

* Clean, optimize, and fix logic

* jolbijlb

* quick

* oops

* fps

* Update photonlib.json

* Update TRIGONLib

* Cleaned some of the camera logic

* Extra newline

* Added proper getters

* Fixed pose estimator logic

* Removed non-existent camera

* Cleaned, and added more checks

---------

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Yishai Levy <96019039+levyishai@users.noreply.github.com>

build.gradle

@@ -100,7 +100,7 @@ dependencies {
     simulationRelease wpi.sim.enableRelease()
 
     implementation 'com.google.code.gson:gson:2.10.1'
-    implementation 'com.github.Programming-TRIGON:TRIGONLib:v2025.0.10'
+    implementation 'com.github.Programming-TRIGON:TRIGONLib:v2025.0.11'
 
     def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
     annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version"

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

@@ -60,7 +60,6 @@ private void initializeGeneralSystems() {
 
     private void bindControllerCommands() {
         OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND);
-        OperatorConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER.onTrue(CommandConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND);
         OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND);
         OperatorConstants.TOGGLE_ROTATION_MODE_TRIGGER.onTrue(GeneralCommands.getToggleRotationModeCommand());
         OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand());

src/main/java/frc/trigon/robot/commands/CommandConstants.java

@@ -46,7 +46,6 @@ public class CommandConstants {
                     () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX())
             ),
             RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(new FlippablePose2d(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(), false), new Rotation2d()).get())),
-            SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND = new InstantCommand(RobotContainer.POSE_ESTIMATOR::setGyroHeadingToBestSolvePNPHeading).ignoringDisable(true),
             SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand(
                     () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER),
                     () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER),

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

@@ -1,8 +1,5 @@
 package frc.trigon.robot.constants;
 
 public class CameraConstants {
-    public static final double
-            TRANSLATIONS_STD_EXPONENT = 0.02,
-            THETA_STD_EXPONENT = 0.02;
     //TODO: implement CameraConstants
 }

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

@@ -21,7 +21,6 @@ public class OperatorConstants {
 
     public static final Trigger
             RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(),
-            SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER = OPERATOR_CONTROLLER.r().or(DRIVER_CONTROLLER.a()),
             TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton),
             TOGGLE_ROTATION_MODE_TRIGGER = DRIVER_CONTROLLER.b(),
             DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1),

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

@@ -5,10 +5,10 @@
 import org.trigon.utilities.FilesHandler;
 
 public class RobotConstants {
-    private static final RobotHardwareStats.ReplayType REPLAY_TYPE = RobotHardwareStats.ReplayType.NONE;
-    private static final double PERIODIC_TIME_SECONDS = 0.02;
     public static final String CANIVORE_NAME = "CANivore";
     public static final String LOGGING_PATH = Robot.IS_REAL ? "/media/sda1/akitlogs/" : FilesHandler.DEPLOY_PATH + "logs/";
+    private static final RobotHardwareStats.ReplayType REPLAY_TYPE = RobotHardwareStats.ReplayType.NONE;
+    private static final double PERIODIC_TIME_SECONDS = 0.02;
 
     public static void init() {
         RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, REPLAY_TYPE);

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

@@ -13,11 +13,9 @@
 import java.util.function.Function;
 
 public class AprilTagCameraConstants {
-    static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS = 2;
-    static final int CALCULATE_YAW_ITERATIONS = 3;
+    static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_SOLVE_PNP_RESULT_METERS = 2;
     static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0];
-
-    public static final double MAXIMUM_AMBIGUITY = 0.4;
+    static final double MAXIMUM_AMBIGUITY = 0.4;
 
     public static final VisionSystemSim VISION_SIMULATION = new VisionSystemSim("VisionSimulation");
     private static final int

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

@@ -3,8 +3,17 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import org.littletonrobotics.junction.AutoLog;
+import org.trigon.hardware.RobotHardwareStats;
 
 public class AprilTagCameraIO {
+    static AprilTagCameraIO generateIO(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name) {
+        if (RobotHardwareStats.isReplay())
+            return new AprilTagCameraIO();
+        if (RobotHardwareStats.isSimulation())
+            return AprilTagCameraConstants.AprilTagCameraType.SIMULATION_CAMERA.createIOFunction.apply(name);
+        return aprilTagCameraType.createIOFunction.apply(name);
+    }
+
     protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) {
     }
 
@@ -22,9 +31,7 @@ public static class AprilTagCameraInputs {
         public double latestResultTimestampSeconds = 0;
         public Pose3d cameraSolvePNPPose = new Pose3d();
         public int[] visibleTagIDs = new int[0];
-        public double bestTargetRelativeYawRadians = 0;
-        public double bestTargetRelativePitchRadians = 0;
-        public double distanceFromBestTag = 0;
         public double poseAmbiguity = 0;
+        public double[] distancesFromTags = new double[0];
     }
 }

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

@@ -1,13 +1,11 @@
 package frc.trigon.robot.poseestimation.apriltagcamera.io;
 
 import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.math.util.Units;
-import frc.trigon.robot.constants.FieldConstants;
 import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO;
 import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged;
 import org.trigon.utilities.LimelightHelpers;
 
+// TODO: Fully implement this class, Limelight currently not supported.
 public class AprilTagLimelightIO extends AprilTagCameraIO {
     private final String hostname;
 
@@ -29,20 +27,14 @@ protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) {
     }
 
     private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, LimelightHelpers.LimelightResults results) {
-        final Rotation3d bestTagRelativeRotation = getBestTargetRelativeRotation(results);
-
         inputs.cameraSolvePNPPose = results.getBotPose3d_wpiBlue();
         inputs.latestResultTimestampSeconds = results.timestamp_RIOFPGA_capture;
         inputs.visibleTagIDs = getVisibleTagIDs(results);
-        inputs.bestTargetRelativeYawRadians = bestTagRelativeRotation.getZ();
-        inputs.bestTargetRelativePitchRadians = bestTagRelativeRotation.getY();
-        inputs.distanceFromBestTag = getDistanceFromBestTag(results);
     }
 
     private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) {
         inputs.cameraSolvePNPPose = new Pose3d();
         inputs.visibleTagIDs = new int[0];
-        inputs.distanceFromBestTag = Double.POSITIVE_INFINITY;
     }
 
     private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) {
@@ -64,25 +56,6 @@ private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) {
         return visibleTagIDs;
     }
 
-    /**
-     * Estimates the camera's rotation relative to the apriltag.
-     *
-     * @param results the camera's pipeline result
-     * @return the estimated rotation
-     */
-    private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.LimelightResults results) {
-        final LimelightHelpers.LimelightTarget_Fiducial bestTag = getBestTarget(results);
-        return new Rotation3d(0, -Units.degreesToRadians(bestTag.tx), -Units.degreesToRadians(bestTag.ty));
-    }
-
-    private double getDistanceFromBestTag(LimelightHelpers.LimelightResults results) {
-        return getDistanceFromTag((int) getBestTarget(results).fiducialID, results.getBotPose3d_wpiBlue());
-    }
-
-    private double getDistanceFromTag(int fiducialID, Pose3d estimatedRobotPose) {
-        return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getTranslation().getDistance(estimatedRobotPose.getTranslation());
-    }
-
     private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.LimelightResults results) {
         return results.targets_Fiducials[0];
     }

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

@@ -1,9 +1,7 @@
 package frc.trigon.robot.poseestimation.apriltagcamera.io;
 
 import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.util.Units;
 import frc.trigon.robot.constants.FieldConstants;
 import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO;
 import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged;
@@ -24,11 +22,12 @@ public AprilTagPhotonCameraIO(String cameraName) {
     protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) {
         final PhotonPipelineResult latestResult = getLatestPipelineResult();
 
-        inputs.hasResult = latestResult != null && latestResult.hasTargets() && !latestResult.getTargets().isEmpty();
+        inputs.hasResult = latestResult != null && latestResult.hasTargets();
         if (inputs.hasResult) {
             updateHasResultInputs(inputs, latestResult);
             return;
         }
+
         updateNoResultInputs(inputs);
     }
 
@@ -38,48 +37,16 @@ private PhotonPipelineResult getLatestPipelineResult() {
     }
 
     private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) {
-        final PhotonTrackedTarget bestTarget = getBestTarget(latestResult);
-        final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(bestTarget);
-
-        inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult, bestTarget);
+        inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult);
         inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds();
-        inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY();
-        inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ();
-        inputs.visibleTagIDs = getVisibleTagIDs(latestResult, bestTarget);
-        inputs.distanceFromBestTag = getDistanceFromBestTag(bestTarget);
-        inputs.poseAmbiguity = bestTarget.getPoseAmbiguity();
+        inputs.visibleTagIDs = getVisibleTagIDs(latestResult);
+        inputs.poseAmbiguity = latestResult.getMultiTagResult().isPresent() ? 0 : latestResult.getBestTarget().getPoseAmbiguity();
+        inputs.distancesFromTags = getDistancesFromTags(latestResult);
     }
 
     private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) {
         inputs.cameraSolvePNPPose = new Pose3d();
         inputs.visibleTagIDs = new int[0];
-        inputs.distanceFromBestTag = Double.POSITIVE_INFINITY;
-    }
-
-    /**
-     * Calculates the best tag from the pipeline result based on the area that the tag takes up.
-     *
-     * @param result the camera's pipeline result
-     * @return the best target
-     */
-    private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) {
-        PhotonTrackedTarget bestTarget = result.getBestTarget();
-        for (PhotonTrackedTarget target : result.getTargets()) {
-            if (target.getArea() > bestTarget.area)
-                bestTarget = target;
-        }
-        return bestTarget;
-    }
-
-    /**
-     * Estimates the camera's rotation relative to the apriltag.
-     *
-     * @param bestTag the best apriltag visible to the camera
-     * @return the estimated rotation
-     */
-    private Rotation3d getBestTargetRelativeRotation(PhotonTrackedTarget bestTag) {
-        final Rotation3d cameraRotation = bestTag.getBestCameraToTarget().getRotation();
-        return new Rotation3d(0, Units.degreesToRadians(cameraRotation.getY()), Units.degreesToRadians(cameraRotation.getZ()));
     }
 
     /**
@@ -88,35 +55,38 @@ private Rotation3d getBestTargetRelativeRotation(PhotonTrackedTarget bestTag) {
      * @param result the camera's pipeline result
      * @return the estimated pose
      */
-    private Pose3d getSolvePNPPose(PhotonPipelineResult result, PhotonTrackedTarget bestTarget) {
+    private Pose3d getSolvePNPPose(PhotonPipelineResult result) {
         if (result.getMultiTagResult().isPresent()) {
             final Transform3d cameraPoseTransform = result.getMultiTagResult().get().estimatedPose.best;
             return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin());
         }
 
-        final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(bestTarget.getFiducialId());
-        final Transform3d targetToCamera = bestTarget.getBestCameraToTarget().inverse();
+        final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(result.getBestTarget().getFiducialId());
+        final Transform3d targetToCamera = result.getBestTarget().getBestCameraToTarget().inverse();
         return tagPose.transformBy(targetToCamera);
     }
 
-    private int[] getVisibleTagIDs(PhotonPipelineResult result, PhotonTrackedTarget bestTarget) {
+    private int[] getVisibleTagIDs(PhotonPipelineResult result) {
         final List<PhotonTrackedTarget> targets = result.getTargets();
         final int[] visibleTagIDs = new int[targets.size()];
-        visibleTagIDs[0] = bestTarget.getFiducialId();
-
-        boolean hasSeenBestTarget = false;
-        for (int i = 0; i < visibleTagIDs.length; i++) {
-            final int targetID = targets.get(i).getFiducialId();
-            if (targetID == visibleTagIDs[0]) {
-                hasSeenBestTarget = true;
-                continue;
-            }
-            visibleTagIDs[hasSeenBestTarget ? i : i + 1] = targetID;
-        }
+
+        for (int i = 0; i < targets.size(); i++)
+            visibleTagIDs[i] = targets.get(i).getFiducialId();
+
         return visibleTagIDs;
     }
 
-    private double getDistanceFromBestTag(PhotonTrackedTarget bestTag) {
-        return bestTag.getBestCameraToTarget().getTranslation().getNorm();
+    private double[] getDistancesFromTags(PhotonPipelineResult result) {
+        final List<PhotonTrackedTarget> targets = result.getTargets();
+        final double[] distances = new double[targets.size()];
+
+        for (int i = 0; i < targets.size(); i++)
+            distances[i] = getDistanceFromTarget(targets.get(i));
+
+        return distances;
+    }
+
+    private double getDistanceFromTarget(PhotonTrackedTarget target) {
+        return target.getBestCameraToTarget().getTranslation().getNorm();
     }
 }