Make the SCL camera bits generic again (#138)

* Make the SCL camera bits generic again

* More refactoring

* SCL to main

src/main/java/competition/electrical_contract/CompetitionContract.java

@@ -8,8 +8,10 @@
 import xbot.common.injection.electrical_contract.DeviceInfo;
 import xbot.common.injection.swerve.SwerveInstance;
 import xbot.common.math.XYPair;
+import xbot.common.subsystems.vision.CameraCapabilities;
 
 import javax.inject.Inject;
+import java.util.EnumSet;
 
 public class CompetitionContract extends ElectricalContract {
     protected final double simulationScalingValue = 256.0 * PoseSubsystem.INCHES_IN_A_METER;
@@ -212,54 +214,56 @@ public boolean getArmEncoderIsOnLeftMotor() {
     private static double aprilCameraYaw = Math.toRadians(14);
 
     @Override
-    public CameraInfo[] getAprilTagCameraInfo() {
+    public CameraInfo[] getCameraInfo() {
         return new CameraInfo[] {
             new CameraInfo("Apriltag_FrontLeft_Camera",
                     "AprilTagFrontLeft",
                     new Transform3d(new Translation3d(
                             aprilCameraXDisplacement,
                             aprilCameraYDisplacement,
                             aprilCameraZDisplacement),
-                            new Rotation3d(0, aprilCameraPitch, aprilCameraYaw))),
+                            new Rotation3d(0, aprilCameraPitch, aprilCameraYaw)),
+                    EnumSet.of(CameraCapabilities.APRIL_TAG)),
             new CameraInfo("Apriltag_FrontRight_Camera",
                     "AprilTagFrontRight",
                     new Transform3d(new Translation3d(
                             aprilCameraXDisplacement,
                             -aprilCameraYDisplacement,
                             aprilCameraZDisplacement),
-                            new Rotation3d(0, aprilCameraPitch, -aprilCameraYaw))),
+                            new Rotation3d(0, aprilCameraPitch, -aprilCameraYaw)),
+                    EnumSet.of(CameraCapabilities.APRIL_TAG)),
             new CameraInfo("Apriltag_RearLeft_Camera",
                     "AprilTagRearLeft",
                     new Transform3d(new Translation3d(
                             -aprilCameraXDisplacement,
                             aprilCameraYDisplacement,
                             aprilCameraZDisplacement),
-                            new Rotation3d(0, aprilCameraPitch, Math.toRadians(180) - aprilCameraYaw))),
+                            new Rotation3d(0, aprilCameraPitch, Math.toRadians(180) - aprilCameraYaw)),
+                    EnumSet.of(CameraCapabilities.APRIL_TAG)),
             new CameraInfo("Apriltag_RearRight_Camera",
                     "AprilTagRearRight",
                     new Transform3d(new Translation3d(
                             -aprilCameraXDisplacement,
                             -aprilCameraYDisplacement,
                             aprilCameraZDisplacement),
-                            new Rotation3d(0, aprilCameraPitch, Math.toRadians(180) + aprilCameraYaw)))
-        };
-    }
-
-    @Override
-    public CameraInfo[] getNoteCameraInfo() {
-        return new CameraInfo[] {
+                            new Rotation3d(0, aprilCameraPitch, Math.toRadians(180) + aprilCameraYaw)),
+                    EnumSet.of(CameraCapabilities.APRIL_TAG)),
             new CameraInfo("GamePiece_FrontLeft_Camera",
                     "NoteFrontLeft",
-                    new Transform3d(new Translation3d(), new Rotation3d())),
+                    new Transform3d(new Translation3d(), new Rotation3d()),
+                    EnumSet.of(CameraCapabilities.GAME_SPECIFIC)),
             new CameraInfo("GamePiece_FrontRight_Camera",
                     "NoteFrontRight",
-                    new Transform3d(new Translation3d(), new Rotation3d())),
+                    new Transform3d(new Translation3d(), new Rotation3d()),
+                    EnumSet.of(CameraCapabilities.GAME_SPECIFIC)),
             new CameraInfo("GamePiece_RearLeft_Camera",
                 "NoteRearLeft",
-                new Transform3d(new Translation3d(), new Rotation3d())),
+                new Transform3d(new Translation3d(), new Rotation3d()),
+                    EnumSet.of(CameraCapabilities.GAME_SPECIFIC)),
             new CameraInfo("GamePiece_RearRight_Camera",
                     "NoteRearRight",
-                    new Transform3d(new Translation3d(), new Rotation3d()))
+                    new Transform3d(new Translation3d(), new Rotation3d()),
+                    EnumSet.of(CameraCapabilities.GAME_SPECIFIC))
         };
     }
 }

src/main/java/competition/electrical_contract/ElectricalContract.java

@@ -64,9 +64,7 @@ public abstract class ElectricalContract
     public abstract DeviceInfo getShooterMotorFollower();
 
     // Vision
-    public abstract CameraInfo[] getAprilTagCameraInfo();
-
-    public abstract CameraInfo[] getNoteCameraInfo();
+    public abstract CameraInfo[] getCameraInfo();
 }
 
 

src/main/java/competition/subsystems/vision/NoteCamera.java

@@ -1,22 +1,12 @@
 package competition.subsystems.vision;
 
-import org.photonvision.PhotonCameraExtended;
 import xbot.common.injection.electrical_contract.CameraInfo;
+import xbot.common.subsystems.vision.SimpleCamera;
 
-public class NoteCamera implements SimpleCamera{
-    private final PhotonCameraExtended camera;
-    private final String friendlyName;
+public class NoteCamera extends SimpleCamera {
 
     public NoteCamera(CameraInfo cameraInfo) {
-        this.camera = new PhotonCameraExtended(cameraInfo.networkTablesName());
-        this.friendlyName = cameraInfo.friendlyName();
+        super(cameraInfo);
     }
 
-    public String getName() {
-        return this.friendlyName;
-    }
-
-    public PhotonCameraExtended getCamera() {
-        return this.camera;
-    }
 }

src/main/java/competition/subsystems/vision/VisionSubsystem.java

@@ -8,13 +8,17 @@
 import org.photonvision.PhotonPoseEstimator;
 import xbot.common.advantage.DataFrameRefreshable;
 import xbot.common.command.BaseSubsystem;
+import xbot.common.injection.electrical_contract.CameraInfo;
 import xbot.common.injection.electrical_contract.XCameraElectricalContract;
 import xbot.common.logging.RobotAssertionManager;
 import xbot.common.logic.TimeStableValidator;
 import xbot.common.properties.BooleanProperty;
 import xbot.common.properties.DoubleProperty;
 import xbot.common.properties.Property;
 import xbot.common.properties.PropertyFactory;
+import xbot.common.subsystems.vision.AprilTagCamera;
+import xbot.common.subsystems.vision.CameraCapabilities;
+import xbot.common.subsystems.vision.SimpleCamera;
 
 import javax.inject.Inject;
 import javax.inject.Singleton;
@@ -37,7 +41,7 @@ public class VisionSubsystem extends BaseSubsystem implements DataFrameRefreshab
     AprilTagFieldLayout aprilTagFieldLayout;
     final ArrayList<AprilTagCamera> aprilTagCameras;
     final ArrayList<NoteCamera> noteCameras;
-    final ArrayList<SimpleCamera> allPhotonVisionBasedCameras;
+    final ArrayList<SimpleCamera> allCameras;
     boolean aprilTagsLoaded = false;
     long logCounter = 0;
 
@@ -71,19 +75,27 @@ public VisionSubsystem(PropertyFactory pf, XCameraElectricalContract electricalC
         aprilTagCameras = new ArrayList<AprilTagCamera>();
         if (aprilTagsLoaded) {
             PhotonCameraExtended.setVersionCheckEnabled(false);
-            for (var cameraInfo : electricalContract.getAprilTagCameraInfo()) {
-                aprilTagCameras.add(new AprilTagCamera(cameraInfo, waitForStablePoseTime::get, aprilTagFieldLayout));
+            var aprilTagCapableCameras = Arrays
+                    .stream(electricalContract.getCameraInfo())
+                    .filter(info -> info.capabilities().contains(CameraCapabilities.APRIL_TAG))
+                    .toArray(CameraInfo[]::new);
+            for (var camera : aprilTagCapableCameras) {
+                aprilTagCameras.add(new AprilTagCamera(camera, waitForStablePoseTime::get, aprilTagFieldLayout));
             }
         }
 
         noteCameras = new ArrayList<NoteCamera>();
-        for (var cameraInfo : electricalContract.getNoteCameraInfo()) {
-            noteCameras.add(new NoteCamera(cameraInfo));
+        var noteTrackingCapableCameras = Arrays
+                .stream(electricalContract.getCameraInfo())
+                .filter(info -> info.capabilities().contains(CameraCapabilities.GAME_SPECIFIC))
+                .toArray(CameraInfo[]::new);
+        for (var camera : noteTrackingCapableCameras) {
+            noteCameras.add(new NoteCamera(camera));
         }
 
-        allPhotonVisionBasedCameras = new ArrayList<SimpleCamera>();
-        allPhotonVisionBasedCameras.addAll(aprilTagCameras);
-        allPhotonVisionBasedCameras.addAll(noteCameras);
+        allCameras = new ArrayList<SimpleCamera>();
+        allCameras.addAll(aprilTagCameras);
+        allCameras.addAll(noteCameras);
     }
 
     public List<Optional<EstimatedRobotPose>> getPhotonVisionEstimatedPoses(Pose2d previousEstimatedRobotPose) {
@@ -201,7 +213,7 @@ public double getNoteArea(NoteCamera camera) {
     public void periodic() {
         loopCounter++;
 
-        var anyCameraBroken = allPhotonVisionBasedCameras.stream().anyMatch(state -> !state.isCameraWorking());
+        var anyCameraBroken = allCameras.stream().anyMatch(state -> !state.isCameraWorking());
 
         // If one of the cameras is not working, see if they have self healed every 5 seconds
         if (loopCounter % (50 * 5) == 0 && (anyCameraBroken)) {
@@ -213,7 +225,7 @@ public void periodic() {
             }
         }
 
-        for (SimpleCamera camera : allPhotonVisionBasedCameras) {
+        for (SimpleCamera camera : allCameras) {
             aKitLog.record(camera.getName() + "CameraWorking", camera.isCameraWorking());
         }
 
@@ -228,7 +240,7 @@ public void periodic() {
     @Override
     public void refreshDataFrame() {
         if (aprilTagsLoaded) {
-            for (SimpleCamera camera : allPhotonVisionBasedCameras) {
+            for (SimpleCamera camera : allCameras) {
                 if (camera.isCameraWorking()) {
                     camera.getCamera().refreshDataFrame();
                 }