diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 71943b1f..979e6d36 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 71943b1ff50a644a40445e4f624060b65554cf6a +Subproject commit 979e6d3690d3140a7086d7ae8655dba58d561644 diff --git a/src/main/java/competition/electrical_contract/CompetitionContract.java b/src/main/java/competition/electrical_contract/CompetitionContract.java index 4fc4d576..460c04ae 100644 --- a/src/main/java/competition/electrical_contract/CompetitionContract.java +++ b/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,7 +214,7 @@ 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", @@ -220,46 +222,48 @@ public CameraInfo[] getAprilTagCameraInfo() { 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)) }; } } diff --git a/src/main/java/competition/electrical_contract/ElectricalContract.java b/src/main/java/competition/electrical_contract/ElectricalContract.java index ef024bf5..49df947a 100644 --- a/src/main/java/competition/electrical_contract/ElectricalContract.java +++ b/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(); } diff --git a/src/main/java/competition/subsystems/vision/AprilTagCamera.java b/src/main/java/competition/subsystems/vision/AprilTagCamera.java deleted file mode 100644 index 074de9ee..00000000 --- a/src/main/java/competition/subsystems/vision/AprilTagCamera.java +++ /dev/null @@ -1,47 +0,0 @@ -package competition.subsystems.vision; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import org.photonvision.PhotonCameraExtended; -import org.photonvision.PhotonPoseEstimator; -import xbot.common.injection.electrical_contract.CameraInfo; -import xbot.common.logic.TimeStableValidator; - -import java.util.function.Supplier; - -public class AprilTagCamera implements SimpleCamera { - private final PhotonPoseEstimator poseEstimator; - - private final PhotonCameraExtended camera; - - private final String friendlyName; - - private final TimeStableValidator isStable; - - public AprilTagCamera(CameraInfo cameraInfo, - Supplier poseStableTime, - AprilTagFieldLayout fieldLayout) { - this.camera = new PhotonCameraExtended(cameraInfo.networkTablesName()); - this.friendlyName = cameraInfo.friendlyName(); - this.poseEstimator = new PhotonPoseEstimator(fieldLayout, - PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - this.camera, - cameraInfo.position()); - this.isStable = new TimeStableValidator(poseStableTime); - } - - public String getName() { - return this.friendlyName; - } - - public PhotonCameraExtended getCamera() { - return this.camera; - } - - public PhotonPoseEstimator getPoseEstimator() { - return this.poseEstimator; - } - - public TimeStableValidator getIsStableValidator() { - return isStable; - } -} diff --git a/src/main/java/competition/subsystems/vision/NoteCamera.java b/src/main/java/competition/subsystems/vision/NoteCamera.java index 6cbdb065..4ff05f94 100644 --- a/src/main/java/competition/subsystems/vision/NoteCamera.java +++ b/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; - } } diff --git a/src/main/java/competition/subsystems/vision/SimpleCamera.java b/src/main/java/competition/subsystems/vision/SimpleCamera.java deleted file mode 100644 index 18abba9a..00000000 --- a/src/main/java/competition/subsystems/vision/SimpleCamera.java +++ /dev/null @@ -1,13 +0,0 @@ -package competition.subsystems.vision; - -import org.photonvision.PhotonCameraExtended; - -public interface SimpleCamera { - public String getName(); - public PhotonCameraExtended getCamera(); - - default boolean isCameraWorking() { - return getCamera().doesLibraryVersionMatchCoprocessorVersion() - && getCamera().isConnected(); - } -} diff --git a/src/main/java/competition/subsystems/vision/VisionSubsystem.java b/src/main/java/competition/subsystems/vision/VisionSubsystem.java index fd87318d..fa0b8e10 100644 --- a/src/main/java/competition/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/competition/subsystems/vision/VisionSubsystem.java @@ -8,6 +8,7 @@ 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; @@ -15,6 +16,9 @@ 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 aprilTagCameras; final ArrayList noteCameras; - final ArrayList allPhotonVisionBasedCameras; + final ArrayList allCameras; boolean aprilTagsLoaded = false; long logCounter = 0; @@ -71,19 +75,27 @@ public VisionSubsystem(PropertyFactory pf, XCameraElectricalContract electricalC aprilTagCameras = new ArrayList(); 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(); - 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(); - allPhotonVisionBasedCameras.addAll(aprilTagCameras); - allPhotonVisionBasedCameras.addAll(noteCameras); + allCameras = new ArrayList(); + allCameras.addAll(aprilTagCameras); + allCameras.addAll(noteCameras); } public List> 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(); }