fix: use optionals for getting vision pose

flamingchickens1540 · Jan 30, 2024 · 4e307cb · 4e307cb
1 parent 138a18e
commit 4e307cb
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 12 deletions.
diff --git 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
@@ -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
@@ -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,33 +81,33 @@ 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);
     }
 
     /**
      * Gets the estimated pose by fusing individual computed poses from each camera.
      * 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());
         System.arraycopy(rearPose.seenTagIDs(), 0, allTagIDs, frontPose.getNumTagsSeen(), rearPose.getNumTagsSeen());
         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));
     }
 }