Add method to set vision measurement consumer to VisionLocalizer

team401 · Nov 18, 2024 · 87fa4c3 · 87fa4c3
1 parent df9bd58
commit 87fa4c3
Showing 1 changed file with 11 additions and 7 deletions.
diff --git a/vision/src/main/java/coppercore/vision/VisionLocalizer.java b/vision/src/main/java/coppercore/vision/VisionLocalizer.java
@@ -1,9 +1,12 @@
 package coppercore.vision;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import java.util.Optional;
 import java.util.function.Consumer;
 import java.util.function.Supplier;
-
 import org.littletonrobotics.junction.Logger;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonPoseEstimator;
@@ -12,11 +15,6 @@
 import org.photonvision.simulation.VisionSystemSim;
 import org.photonvision.targeting.PhotonPipelineResult;
 
-import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 /** A reliable vision subsystem to be used in any robot project */
 public class VisionLocalizer extends SubsystemBase {
     CameraWrapper[] cameras;
@@ -54,12 +52,18 @@ public VisionLocalizer(
                                     cameraParams[i].robotToCamera()));
 
             if (isSim) {
-                PhotonCameraSim cameraSim = new PhotonCameraSim(cameras[i].getCamera(), cameraParams[i].simCameraProp());
+                PhotonCameraSim cameraSim =
+                        new PhotonCameraSim(
+                                cameras[i].getCamera(), cameraParams[i].simCameraProp());
                 visionSim.addCamera(cameraSim, cameraParams[i].robotToCamera());
             }
         }
     }
 
+    public void setVisionMeasurementConsumer(Consumer<VisionMeasurement> newVisionMeasurementConsumer) {
+        visionMeasurementConsumer = newVisionMeasurementConsumer;
+    }
+
     public void setSimRobotPoseSupplier(Supplier<Pose3d> newSimRobotPoseSupplier) {
         simRobotPoseSupplier = newSimRobotPoseSupplier;
     }