Add 2d based pose estimate

DevilBotz2876 · Mar 27, 2024 · 20b8e20 · 20b8e20
1 parent 9d6c160
commit 20b8e20
Showing 1 changed file with 13 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 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.math.util.Units;
@@ -101,6 +102,7 @@ private Transform3d getRobotToCamera() {
   @AutoLogOutput private double debugTargetDistance = 0;
   @AutoLogOutput private double debugTargetYaw = 0;
   @AutoLogOutput private Pose2d debugEstimatedPose;
+  @AutoLogOutput private Pose2d debugEstimatedPose2d;
 
   /* Simulation Support*/
   private boolean simEnabled = false;
@@ -149,6 +151,17 @@ public void periodic() {
       camera.update();
       if (camera == primaryCamera) {
         debugTargetsVisible = camera.result.getTargets().size();
+
+        if (debugTargetsVisible > 0) {
+          PhotonTrackedTarget target = camera.result.getBestTarget();
+          Optional<Pose3d> fieldToTarget = fieldLayout.getTagPose(target.getFiducialId());
+          if (fieldToTarget.isPresent()) {
+            debugEstimatedPose2d =
+                PhotonUtils.estimateFieldToRobotAprilTag(
+                        target.getBestCameraToTarget(), fieldToTarget.get(), camera.robotToCamera)
+                    .toPose2d();
+          }
+        }
       }
 
       Optional<EstimatedRobotPose> currentEstimatedRobotPose = camera.getEstimatedRobotPose();