Add object detection vision subsystem support

Introduces object detection vision subsystem with new IO classes for Limelight, Photon, and simulation. Updates RobotContainer to instantiate ObjectVision, and VisionConstants to provide configuration and IO selection for object detection cameras.

Author: FarhanJ2

src/main/java/org/steelhawks/RobotContainer.java

@@ -76,6 +76,8 @@ public RobotContainer() {
                     s_Vision =
                         new Vision(
                             s_Swerve::accept, false);
+                    s_ObjVision =
+                        new ObjectVision();
                     s_Elevator =
                         new Elevator(
                             new ElevatorIOTalonFX());
@@ -153,6 +155,8 @@ public RobotContainer() {
                     s_Vision =
                         new Vision(
                             s_Swerve::accept, true);
+                    s_ObjVision =
+                        new ObjectVision();
                     s_Elevator =
                         new Elevator(
                             new ElevatorIOSim());

src/main/java/org/steelhawks/subsystems/vision/VisionConstants.java

@@ -10,6 +10,10 @@
 import edu.wpi.first.math.util.Units;
 import org.steelhawks.RobotContainer;
 import org.steelhawks.subsystems.swerve.Swerve;
+import org.steelhawks.subsystems.vision.objdetect.ObjectVisionIO;
+import org.steelhawks.subsystems.vision.objdetect.ObjectVisionIOLimelight;
+import org.steelhawks.subsystems.vision.objdetect.ObjectVisionIOPhoton;
+import org.steelhawks.subsystems.vision.objdetect.ObjectVisionSim;
 
 public class VisionConstants {
     // AprilTag layout
@@ -112,6 +116,15 @@ public enum CameraType {
         )
     };
 
+    private static final CameraConfig[] OMEGA_OBJ_DETECT_CONFIG = {
+        new CameraConfig(
+            "limelight",
+            Constants.fromOnshapeCoordinates(4.469, 9.261, 21.578, 15.0, -20.0, 0.0),
+            0.0,
+            CameraType.LIMELIGHT
+        )
+    };
+
     private static final CameraConfig[] ALPHA_CAMERA_CONFIG = {
         new CameraConfig(
             "limelight-coral",
@@ -144,6 +157,13 @@ public static CameraConfig[] getCameraConfig() {
         };
     }
 
+    public static CameraConfig[] getObjDetectConfig() {
+        return switch (Constants.getRobot()) {
+            case SIMBOT, OMEGABOT -> OMEGA_OBJ_DETECT_CONFIG;
+            default -> null;
+        };
+    }
+
     public static VisionIO[] getIO() {
         CameraConfig[] config = getCameraConfig();
         VisionIO[] io = new VisionIO[config.length];
@@ -165,6 +185,28 @@ public static VisionIO[] getIO() {
         return io;
     }
 
+    public static ObjectVisionIO[] getObjIO() {
+        CameraConfig[] config = getObjDetectConfig();
+        assert config != null;
+        ObjectVisionIO[] io = new ObjectVisionIO[config.length];
+        for (int i = 0; i < config.length; i++) {
+            if (RobotBase.isReal()) {
+                switch (config[i].cameraType) {
+                    case LIMELIGHT -> io[i] = new ObjectVisionIOLimelight(config[i].name);
+                    case PHOTON -> io[i] = new ObjectVisionIOPhoton(config[i].name, config[i].robotToCamera);
+                }
+            } else if (Constants.getRobot() == Constants.RobotType.SIMBOT && !RobotBase.isReal()) {
+                io[i] = new ObjectVisionSim(
+                    config[i].name,
+                    config[i].robotToCamera,
+                    Swerve.getDriveSimulation()::getSimulatedDriveTrainPose);
+            } else if (Constants.getRobot() != Constants.RobotType.SIMBOT && !RobotBase.isReal()) {
+                io[i] = new ObjectVisionIO() {};
+            }
+        }
+        return io;
+    }
+
     // Basic filtering thresholds
     public static double MAX_AMBIGUITY = 0.3;
     public static double MAX_ZERROR = 0.75;

src/main/java/org/steelhawks/subsystems/vision/objdetect/ObjectVision.java

@@ -7,6 +7,7 @@
 import org.littletonrobotics.junction.Logger;
 import org.steelhawks.FieldConstants;
 import org.steelhawks.RobotContainer;
+import org.steelhawks.subsystems.vision.VisionConstants;
 
 import java.util.*;
 import java.util.stream.Collectors;
@@ -27,8 +28,8 @@ record CoralPose(
     private final ArrayList<ObjectVisionIO.ObjectObservation> allObservations = new ArrayList<>();
     private Set<CoralPose> coralPoses = new HashSet<>();
 
-    public ObjectVision(ObjectVisionIO... io) {
-        this.io = io;
+    public ObjectVision() {
+        this.io = VisionConstants.getObjIO();
         this.inputs = new ObjectVisionIOInputsAutoLogged[io.length];
         for (int i = 0; i < inputs.length; i++) {
             inputs[i] = new ObjectVisionIOInputsAutoLogged();

src/main/java/org/steelhawks/subsystems/vision/objdetect/ObjectVisionIOLimelight.java

@@ -1,8 +1,5 @@
 package org.steelhawks.subsystems.vision.objdetect;
 
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.networktables.BooleanSubscriber;
 import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.IntegerSubscriber;
 import edu.wpi.first.networktables.NetworkTableInstance;

src/main/java/org/steelhawks/subsystems/vision/objdetect/ObjectVisionIOPhoton.java

@@ -0,0 +1,14 @@
+package org.steelhawks.subsystems.vision.objdetect;
+
+import edu.wpi.first.math.geometry.Transform3d;
+
+public class ObjectVisionIOPhoton implements ObjectVisionIO {
+
+    private final String name;
+    private final Transform3d robotToCamera;
+
+    public ObjectVisionIOPhoton(String name, Transform3d robotToCamera) {
+        this.name = name;
+        this.robotToCamera = robotToCamera;
+    }
+}

src/main/java/org/steelhawks/subsystems/vision/objdetect/ObjectVisionSim.java

@@ -0,0 +1,19 @@
+package org.steelhawks.subsystems.vision.objdetect;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Transform3d;
+
+import java.util.function.Supplier;
+
+public class ObjectVisionSim implements ObjectVisionIO {
+
+    private final String name;
+    private final Transform3d robotToCamera;
+    private final Supplier<Pose2d> poseSupplier;
+
+    public ObjectVisionSim(String name, Transform3d robotToCamera, Supplier<Pose2d> poseSupplier) {
+        this.name = name;
+        this.robotToCamera = robotToCamera;
+        this.poseSupplier = poseSupplier;
+    }
+}