feat: log seen tag ids for vision

flamingchickens1540 · Apr 15, 2024 · b321b3d · b321b3d
1 parent 9c913f1
commit b321b3d
Show file tree

Hide file tree

Showing 7 changed files with 15 additions and 6 deletions.
diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
@@ -120,7 +120,6 @@ public static class Vision {
         public static final double ROT_STD_DEV_COEFF = 0.5;
 
         public static final double MAX_AMBIGUITY_RATIO = 0.3;
-        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;
         public static final double MIN_ACCEPTED_NUM_TAGS = 1;

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVision.java
@@ -107,7 +107,7 @@ public void periodic() {
         updateAndAcceptPose(frontCameraInputs, frontPose);
         updateAndAcceptPose(rearCameraInputs, rearPose);
 
-        if (TAKE_SNAPSHOTS && /*DriverStation.isFMSAttached() &&*/ RobotState.isEnabled()) {
+        if (TAKE_SNAPSHOTS && RobotState.isEnabled()) {
             Logger.runEveryN((int) (SNAPSHOT_PERIOD_SECS / Constants.LOOP_PERIOD_SECS),
                     () -> takeSnapshot(
                             String.format("%s_%s%d_%d",

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIO.java
@@ -8,6 +8,7 @@ public interface AprilTagVisionIO {
     class AprilTagVisionIOInputs {
         public Pose3d estimatedPoseMeters = new Pose3d();
         public int numTagsSeen = 0;
+        public int[] seenTagIDs = new int[0];
         public double avgTagDistance = Double.POSITIVE_INFINITY;
         public double lastMeasurementTimestampSecs = 0.0;
     }

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOLimelight.java
@@ -3,6 +3,8 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import org.team1540.robot2024.util.vision.LimelightHelpers;
 
+import java.util.Arrays;
+
 public class AprilTagVisionIOLimelight implements AprilTagVisionIO {
     private final String name;
 
@@ -26,6 +28,9 @@ public void updateInputs(AprilTagVisionIOInputs inputs) {
             inputs.lastMeasurementTimestampSecs = measurement.timestampSeconds;
         }
         inputs.numTagsSeen = measurement.tagCount;
+        inputs.seenTagIDs = Arrays.stream(measurement.rawFiducials).mapToInt(fiducial -> fiducial.id).toArray();
+        for (int i = 0; i < inputs.seenTagIDs.length; i++)
+            inputs.seenTagIDs[i] = measurement.rawFiducials[i].id;
         inputs.avgTagDistance = measurement.avgTagDist;
     }
 

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOMegaTag2.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOMegaTag2.java
@@ -5,6 +5,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import org.team1540.robot2024.util.vision.LimelightHelpers;
 
+import java.util.Arrays;
 import java.util.function.Supplier;
 
 public class AprilTagVisionIOMegaTag2 implements AprilTagVisionIO {
@@ -23,7 +24,7 @@ public AprilTagVisionIOMegaTag2(String name, Pose3d cameraOffsetMeters, Supplier
     public void updateInputs(AprilTagVisionIOInputs inputs) {
         LimelightHelpers.SetRobotOrientation(name, heading.get().getDegrees(), 0, 0, 0, 0, 0);
         LimelightHelpers.PoseEstimate measurement;
-        if(DriverStation.isDisabled()){
+        if (DriverStation.isDisabled()) {
             measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue(name);
             if (measurement.tagCount > 1) {
                 Pose3d limelightPose = LimelightHelpers.getBotPose3d_wpiBlue(name);
@@ -34,11 +35,9 @@ public void updateInputs(AprilTagVisionIOInputs inputs) {
                         limelightPose.getRotation());
                 inputs.lastMeasurementTimestampSecs = measurement.timestampSeconds;
             }
-        } else{
+        } else {
             measurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(name);
-
             Pose3d limelightPose = new Pose3d(measurement.pose);
-
             inputs.estimatedPoseMeters = new Pose3d(
                     limelightPose.getX(),
                     limelightPose.getY() + 0.105,
@@ -48,6 +47,9 @@ public void updateInputs(AprilTagVisionIOInputs inputs) {
         }
 
         inputs.numTagsSeen = measurement.tagCount;
+        inputs.seenTagIDs = Arrays.stream(measurement.rawFiducials).mapToInt(fiducial -> fiducial.id).toArray();
+        for (int i = 0; i < inputs.seenTagIDs.length; i++)
+            inputs.seenTagIDs[i] = measurement.rawFiducials[i].id;
         inputs.avgTagDistance = measurement.avgTagDist;
     }
 

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOPhoton.java
@@ -49,6 +49,7 @@ public void updateInputs(AprilTagVisionIOInputs inputs) {
         }
 
         inputs.numTagsSeen = targets.size();
+        inputs.seenTagIDs = targets.stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray();
         if (inputs.numTagsSeen > 0) {
             inputs.avgTagDistance = 0;
             for (PhotonTrackedTarget target : targets)

diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java
@@ -83,6 +83,7 @@ public void updateInputs(AprilTagVisionIOInputs inputs) {
             inputs.lastMeasurementTimestampSecs = estimatedPose.get().timestampSeconds;
 
             inputs.numTagsSeen = targets.size();
+            inputs.seenTagIDs = targets.stream().mapToInt(PhotonTrackedTarget::getFiducialId).toArray();
             if (inputs.numTagsSeen > 0) {
                 inputs.avgTagDistance = 0;
                 for (PhotonTrackedTarget target : targets)