advanced object detection

Author: BenGamer3

src/main/java/frc/robot/subsystems/vision/VisionIO.java

@@ -9,20 +9,26 @@
 
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import java.util.Optional;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface VisionIO {
   @AutoLog
   public static class VisionIOInputs {
     public boolean connected = false;
-    public TargetObservation latestTargetObservation =
-        new TargetObservation(Rotation2d.kZero, Rotation2d.kZero);
+    public Optional<TargetObservation> latestTargetObservation = Optional.empty();
     public PoseObservation[] poseObservations = new PoseObservation[0];
     public int[] tagIds = new int[0];
   }
 
   /** Represents the angle to a simple target, not used for pose estimation. */
-  public static record TargetObservation(Rotation2d tx, Rotation2d ty) {}
+  public static record TargetObservation(
+      Rotation2d yaw,
+      Rotation2d pitch,
+      Rotation2d skew,
+      double area,
+      float confidince,
+      int objectID) {}
 
   /** Represents a robot pose sample used for pose estimation. */
   public static record PoseObservation(

src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java

@@ -13,6 +13,7 @@
 import java.util.HashSet;
 import java.util.LinkedList;
 import java.util.List;
+import java.util.Optional;
 import java.util.Set;
 import org.photonvision.PhotonCamera;
 
@@ -39,15 +40,22 @@ public void updateInputs(VisionIOInputs inputs) {
     // Read new camera observations
     Set<Short> tagIds = new HashSet<>();
     List<PoseObservation> poseObservations = new LinkedList<>();
+
     for (var result : camera.getAllUnreadResults()) {
       // Update latest target observation
       if (result.hasTargets()) {
+        var bestTarget = result.getBestTarget();
         inputs.latestTargetObservation =
-            new TargetObservation(
-                Rotation2d.fromDegrees(result.getBestTarget().getYaw()),
-                Rotation2d.fromDegrees(result.getBestTarget().getPitch()));
+            Optional.of(
+                new TargetObservation(
+                    Rotation2d.fromDegrees(bestTarget.getYaw()),
+                    Rotation2d.fromDegrees(bestTarget.getPitch()),
+                    Rotation2d.fromDegrees(bestTarget.getSkew()),
+                    bestTarget.getArea(),
+                    bestTarget.getDetectedObjectConfidence(),
+                    bestTarget.getDetectedObjectClassID()));
       } else {
-        inputs.latestTargetObservation = new TargetObservation(Rotation2d.kZero, Rotation2d.kZero);
+        inputs.latestTargetObservation = Optional.empty();
       }
 
       // Add pose observation