Added vision consumer to replace Optional framework

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

@@ -26,6 +26,7 @@
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.VisionSubsystem;
 
 /*
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -39,6 +40,7 @@ public class RobotContainer {
   private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem();
   private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem();
   private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem();
+  private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();
 
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
   private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort);
@@ -60,6 +62,8 @@ public RobotContainer() {
             new ReplanningConfig(true, true)),
         () -> false, m_robotDrive);
 
+    m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement);
+
     // Configure the trigger bindings
     configureBindings();
 

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -4,8 +4,6 @@
 
 package frc.robot.subsystems;
 
-import java.util.Optional;
-
 import com.kauailabs.navx.frc.AHRS;
 
 import edu.wpi.first.math.MathUtil;
@@ -71,8 +69,6 @@ public class DriveSubsystem extends SubsystemBase {
       m_gyro.getRotation2d(), m_swerveModulePositions, new Pose2d(), VisionConstants.kOdometrySTDDevs,
       VisionConstants.kVisionSTDDevs);
 
-  private final VisionSubsystem m_visionSubsystem = new VisionSubsystem();
-
   private final Field2d m_field = new Field2d();
 
   /** Creates a new DriveSubsystem. */
@@ -96,14 +92,6 @@ public void periodic() {
     m_poseEstimator.update(Robot.isReal() ? m_gyro.getRotation2d() : new Rotation2d(m_gyroAngle),
         m_swerveModulePositions);
 
-    Optional<Measurement> latestReading = m_visionSubsystem.getMeasurement();
-
-    // SmartDashboard.putBoolean("reading present", latestReading.isPresent());
-
-    if (latestReading.isPresent()) {
-      m_poseEstimator.addVisionMeasurement(latestReading.get().pose.toPose2d(), latestReading.get().timestamp);
-    }
-
     m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
 
     SmartDashboard.putNumber("gyro angle", m_gyro.getAngle());
@@ -213,8 +201,8 @@ public void zeroHeading() {
     m_gyroAngle = 0;
   }
 
-  public void addVisionMeasurement(Pose2d pose, double timestamp) {
-    m_poseEstimator.addVisionMeasurement(pose, timestamp);
+  public void addVisionMeasurement(Measurement measurement) {
+    m_poseEstimator.addVisionMeasurement(measurement.pose.toPose2d(), measurement.timestamp);
   }
 
   /**

src/main/java/frc/robot/subsystems/VisionSubsystem.java

@@ -5,7 +5,8 @@
 package frc.robot.subsystems;
 
 import java.util.Arrays;
-import java.util.Optional;
+import java.util.List;
+import java.util.function.Consumer;
 
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.geometry.Pose3d;
@@ -49,12 +50,14 @@
  */
 public class VisionSubsystem extends SubsystemBase {
 
-  NetworkTable m_visionNetworkTable = NetworkTableInstance.getDefault().getTable("limelight");
+  private NetworkTable m_visionNetworkTable = NetworkTableInstance.getDefault().getTable("limelight");
 
   private final DoubleArraySubscriber m_botPose;
 
   private final IntegerSubscriber m_tv;
 
+  private List<Consumer<Measurement>> m_consumerList;
+
   /** Creates a new Limelight. */
   public VisionSubsystem() {
     // Provide the limelight with the camera pose relative to the center of the
@@ -66,28 +69,41 @@ public VisionSubsystem() {
     m_tv = m_visionNetworkTable.getIntegerTopic("tv").subscribe(0);
   }
 
+  /** Add a consumer, which the vision subsystem will push an update to every time there is an updated measurement */
+  public void addConsumer(Consumer<Measurement> consumer) {
+    m_consumerList.add(consumer);
+  }
+
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
 
+    Measurement latestMeasurement = getMeasurement();
+
+    if (latestMeasurement != null) {
+      for (Consumer<Measurement> consumer : m_consumerList) {
+        consumer.accept(latestMeasurement);
+      }
+    }
+
     SmartDashboard.putBoolean("Limelight Has Target", m_tv.get() == 1);
   }
 
-  public Optional<Measurement> getMeasurement() {
+  public Measurement getMeasurement() {
     TimestampedDoubleArray[] updates = m_botPose.readQueue();
 
     // If we have had no updates since the last time this method ran then return
     // nothing
     if (updates.length == 0) {
-      return Optional.empty();
+      return null;
     }
 
     TimestampedDoubleArray update = updates[updates.length - 1];
 
     // If the latest update is empty or we don't see an april tag then return
     // nothing
     if (Arrays.equals(update.value, new double[6]) || m_tv.get() == 0) {
-      return Optional.empty();
+      return null;
     }
 
     double x = update.value[0];
@@ -100,10 +116,10 @@ public Optional<Measurement> getMeasurement() {
     double timestamp = Timer.getFPGATimestamp() - (update.value[6] / 1000.0);
     Pose3d pose = new Pose3d(new Translation3d(x, y, z), new Rotation3d(roll, pitch, yaw));
 
-    return Optional.of(new Measurement(
+    return new Measurement(
         timestamp,
         pose,
-        VisionConstants.kVisionSTDDevs));
+        VisionConstants.kVisionSTDDevs);
   }
 
   public static class Measurement {