Add additional suppliers for AS visualization

frc868 · Feb 11, 2024 · 389eaad · 389eaad
1 parent 449e43f
commit 389eaad
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 2 deletions.
diff --git a/houndutil b/houndutil
diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
@@ -1,10 +1,10 @@
 package frc.robot.subsystems;
 
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.List;
 import java.util.Optional;
 import java.util.function.Supplier;
-
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.simulation.VisionSystemSim;
 
@@ -111,4 +111,24 @@ public void setPoseEstimator(SwerveDrivePoseEstimator poseEstimator) {
     public void setSimPoseSupplier(Supplier<Pose2d> simPoseSupplier) {
         this.simPoseSupplier = simPoseSupplier;
     }
+
+    @Log
+    public Pose3d[] getEstimatedRobotPoses() {
+        return new Pose3d[] {
+                photonCamera1.getLoggedEstimatedRobotPose(),
+                photonCamera2.getLoggedEstimatedRobotPose(),
+                photonCamera3.getLoggedEstimatedRobotPose() };
+    }
+
+    @Log
+    public Pose3d[] getDetectedAprilTags() {
+        return Arrays
+                .stream(new Pose3d[][] {
+                        photonCamera1.getLoggedDetectedAprilTags(),
+                        photonCamera2.getLoggedDetectedAprilTags(),
+                        photonCamera3.getLoggedDetectedAprilTags() })
+                .flatMap(i -> Arrays.stream(i))
+                .toArray(Pose3d[]::new);
+
+    }
 }