From 4e307cbdea214d83ad14bd1fd850ad1c5cc635dc Mon Sep 17 00:00:00 2001
From: Alvin Zhang <41vin2h4n9@gmail.com>
Date: Tue, 30 Jan 2024 09:23:46 -0800
Subject: [PATCH] fix: use optionals for getting vision pose

---
 .../org/team1540/robot2024/Constants.java     |  1 +
 .../subsystems/drive/Drivetrain.java          |  1 -
 .../subsystems/vision/AprilTagVision.java     | 23 ++++++++++---------
 3 files changed, 13 insertions(+), 12 deletions(-)

diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
index 3027088e..300b1dd3 100644
--- a/src/main/java/org/team1540/robot2024/Constants.java
+++ b/src/main/java/org/team1540/robot2024/Constants.java
@@ -71,6 +71,7 @@ public static class Vision {
         public static final Pose3d FRONT_CAMERA_POSE = new Pose3d();
         public static final Pose3d REAR_CAMERA_POSE = new Pose3d();
 
+        // TODO: find these values
         public static final double MAX_VISION_DELAY_SECS = 0.08;
         public static final double MAX_ACCEPTED_ROT_SPEED_RAD_PER_SEC = 1.0;
         public static final double MAX_ACCEPTED_LINEAR_SPEED_MPS = 4.0;
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java
index e57cf87d..a63551a6 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java
@@ -203,7 +203,6 @@ public void setPose(Pose2d pose) {
     }
 
     public void addVisionMeasurement(TimestampedVisionPose visionPose) {
-        if (visionPose == null) return;
         if (VisionPoseAcceptor.shouldAcceptVision(visionPose, kinematics.toChassisSpeeds(getModuleStates()))) {
             poseEstimator.addVisionMeasurement(visionPose.poseMeters(), visionPose.timestampSecs());
         }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java
index 496b99a3..5059e921 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java
@@ -8,6 +8,7 @@
 import org.team1540.robot2024.util.vision.TimestampedVisionPose;
 
 import java.util.Arrays;
+import java.util.Optional;
 import java.util.function.Consumer;
 import java.util.function.Supplier;
 
@@ -80,9 +81,9 @@ public void periodic() {
                     rearCameraInputs.tagPosesMeters);
         }
 
-        TimestampedVisionPose latestPose = getEstimatedPose();
-        Logger.recordOutput("Vision/EstimatedPose", latestPose == null ? new Pose2d() : latestPose.poseMeters());
-        visionPoseConsumer.accept(latestPose);
+        Optional<TimestampedVisionPose> latestPose = getEstimatedPose();
+        latestPose.ifPresent(visionPose -> Logger.recordOutput("Vision/EstimatedPose", visionPose.poseMeters()));
+        latestPose.ifPresent(visionPoseConsumer);
     }
 
     /**
@@ -90,12 +91,12 @@ public void periodic() {
      * Returns null if no tags seen, in simulation, or if the elevator is moving
      * too fast
      */
-    public TimestampedVisionPose getEstimatedPose() {
-        if (Constants.currentMode == Constants.Mode.SIM) return null;
-        if (frontPose.getNumTagsSeen() < 1 && rearPose.getNumTagsSeen() < 1) return null;
-        if (Math.abs(elevatorVelocitySupplierMPS.get()) > MAX_ACCEPTED_ELEVATOR_SPEED_MPS) return null; // TODO: need to change if one camera is stationary
-        else if (frontPose.getNumTagsSeen() < 1) return rearPose;
-        else if (rearPose.getNumTagsSeen() < 1) return frontPose;
+    public Optional<TimestampedVisionPose> getEstimatedPose() {
+        if (Constants.currentMode == Constants.Mode.SIM) return Optional.empty();
+        if (frontPose.getNumTagsSeen() < 1 && rearPose.getNumTagsSeen() < 1) return Optional.empty();
+        if (Math.abs(elevatorVelocitySupplierMPS.get()) > MAX_ACCEPTED_ELEVATOR_SPEED_MPS) return Optional.empty(); // TODO: need to change if one camera is stationary
+        else if (frontPose.getNumTagsSeen() < 1) return Optional.of(rearPose);
+        else if (rearPose.getNumTagsSeen() < 1) return Optional.of(frontPose);
 
         // This just takes the average of the measurements, we could change this to something more advanced if necessary
         int[] allTagIDs = Arrays.copyOf(frontPose.seenTagIDs(), frontPose.getNumTagsSeen() + rearPose.getNumTagsSeen());
@@ -103,10 +104,10 @@ public TimestampedVisionPose getEstimatedPose() {
         Pose2d[] allTagPoses = Arrays.copyOf(frontPose.tagPosesMeters(), frontPose.getNumTagsSeen() + rearPose.getNumTagsSeen());
         System.arraycopy(rearPose.tagPosesMeters(), 0, allTagPoses, frontPose.getNumTagsSeen(), rearPose.getNumTagsSeen());
 
-        return new TimestampedVisionPose(
+        return Optional.of(new TimestampedVisionPose(
                 (frontPose.timestampSecs() + rearPose.timestampSecs()) / 2,
                 frontPose.poseMeters().interpolate(rearPose.poseMeters(), 0.5),
                 allTagIDs,
-                allTagPoses);
+                allTagPoses));
     }
 }