From 53b5c715bd0f8809ca72f86fc183e031f004dfda Mon Sep 17 00:00:00 2001 From: JohnGilb Date: Tue, 27 Feb 2024 19:15:39 -0800 Subject: [PATCH 1/3] Some cleanup from Alex's PR feedback (#137) --- .../CompetitionContract.java | 38 +++++++++++-------- .../subsystems/oracle/DynamicOracle.java | 8 ++-- 2 files changed, 26 insertions(+), 20 deletions(-) diff --git a/src/main/java/competition/electrical_contract/CompetitionContract.java b/src/main/java/competition/electrical_contract/CompetitionContract.java index 38fa7cb7..4fc4d576 100644 --- a/src/main/java/competition/electrical_contract/CompetitionContract.java +++ b/src/main/java/competition/electrical_contract/CompetitionContract.java @@ -205,37 +205,43 @@ public boolean getArmEncoderIsOnLeftMotor() { return true; } + private static double aprilCameraXDisplacement = 13.48 / PoseSubsystem.INCHES_IN_A_METER; + private static double aprilCameraYDisplacement = 13.09 / PoseSubsystem.INCHES_IN_A_METER; + private static double aprilCameraZDisplacement = 9.25 / PoseSubsystem.INCHES_IN_A_METER; + private static double aprilCameraPitch = Math.toRadians(-30.5); + private static double aprilCameraYaw = Math.toRadians(14); + @Override public CameraInfo[] getAprilTagCameraInfo() { return new CameraInfo[] { new CameraInfo("Apriltag_FrontLeft_Camera", "AprilTagFrontLeft", new Transform3d(new Translation3d( - 13.48 / PoseSubsystem.INCHES_IN_A_METER, - 13.09 / PoseSubsystem.INCHES_IN_A_METER, - 9.25 / PoseSubsystem.INCHES_IN_A_METER), - new Rotation3d(0, Math.toRadians(-30.5), Math.toRadians(14)))), + aprilCameraXDisplacement, + aprilCameraYDisplacement, + aprilCameraZDisplacement), + new Rotation3d(0, aprilCameraPitch, aprilCameraYaw))), new CameraInfo("Apriltag_FrontRight_Camera", "AprilTagFrontRight", new Transform3d(new Translation3d( - 13.48 / PoseSubsystem.INCHES_IN_A_METER, - -13.09 / PoseSubsystem.INCHES_IN_A_METER, - 9.25 / PoseSubsystem.INCHES_IN_A_METER), - new Rotation3d(0, Math.toRadians(-30.5), Math.toRadians(-14)))), + aprilCameraXDisplacement, + -aprilCameraYDisplacement, + aprilCameraZDisplacement), + new Rotation3d(0, aprilCameraPitch, -aprilCameraYaw))), new CameraInfo("Apriltag_RearLeft_Camera", "AprilTagRearLeft", new Transform3d(new Translation3d( - -13.48 / PoseSubsystem.INCHES_IN_A_METER, - 13.09 / PoseSubsystem.INCHES_IN_A_METER, - 9.25 / PoseSubsystem.INCHES_IN_A_METER), - new Rotation3d(0, Math.toRadians(-30.5), Math.toRadians(180 - 14)))), + -aprilCameraXDisplacement, + aprilCameraYDisplacement, + aprilCameraZDisplacement), + new Rotation3d(0, aprilCameraPitch, Math.toRadians(180) - aprilCameraYaw))), new CameraInfo("Apriltag_RearRight_Camera", "AprilTagRearRight", new Transform3d(new Translation3d( - -13.48 / PoseSubsystem.INCHES_IN_A_METER, - -13.09 / PoseSubsystem.INCHES_IN_A_METER, - 9.25 / PoseSubsystem.INCHES_IN_A_METER), - new Rotation3d(0, Math.toRadians(-30.5), Math.toRadians(180 + 14)))) + -aprilCameraXDisplacement, + -aprilCameraYDisplacement, + aprilCameraZDisplacement), + new Rotation3d(0, aprilCameraPitch, Math.toRadians(180) + aprilCameraYaw))) }; } diff --git a/src/main/java/competition/subsystems/oracle/DynamicOracle.java b/src/main/java/competition/subsystems/oracle/DynamicOracle.java index e7f156bc..72afa9c3 100644 --- a/src/main/java/competition/subsystems/oracle/DynamicOracle.java +++ b/src/main/java/competition/subsystems/oracle/DynamicOracle.java @@ -295,16 +295,16 @@ public ScoringSubGoals getScoringSubgoal() { } private void determineScoringSubgoal() { - double acceptableRangeBeforeScoring = 0; + double acceptableRangeBeforeScoringMeters = 0; if (currentHighLevelGoal == HighLevelGoal.ScoreInSpeaker) { - acceptableRangeBeforeScoring = 2; + acceptableRangeBeforeScoringMeters = 2; } else if (currentHighLevelGoal == HighLevelGoal.ScoreInAmp) { // in the future we'll do something more like "get near amp, then drive into the wall for a few moments // before scoring" - acceptableRangeBeforeScoring = 0.05; + acceptableRangeBeforeScoringMeters = 0.05; } - if (isTerminatingPointWithinDistance(acceptableRangeBeforeScoring)) { + if (isTerminatingPointWithinDistance(acceptableRangeBeforeScoringMeters)) { currentScoringSubGoal = ScoringSubGoals.EarnestlyLaunchNote; } else { currentScoringSubGoal = ScoringSubGoals.MoveToScoringRange; From 4a7dbaa7597b022a2fa084d6ff06310d13952404 Mon Sep 17 00:00:00 2001 From: Stephen Just Date: Tue, 27 Feb 2024 19:49:12 -0800 Subject: [PATCH 2/3] Make the SCL camera bits generic again (#138) * Make the SCL camera bits generic again * More refactoring * SCL to main --- SeriouslyCommonLib | 2 +- .../CompetitionContract.java | 34 ++++++++------ .../ElectricalContract.java | 4 +- .../subsystems/vision/AprilTagCamera.java | 47 ------------------- .../subsystems/vision/NoteCamera.java | 16 ++----- .../subsystems/vision/SimpleCamera.java | 13 ----- .../subsystems/vision/VisionSubsystem.java | 34 +++++++++----- 7 files changed, 47 insertions(+), 103 deletions(-) delete mode 100644 src/main/java/competition/subsystems/vision/AprilTagCamera.java delete mode 100644 src/main/java/competition/subsystems/vision/SimpleCamera.java 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(); } From de30dd3864709e34015138d81c2bcb2d0dfbfb98 Mon Sep 17 00:00:00 2001 From: JohnGilb Date: Tue, 27 Feb 2024 21:16:01 -0800 Subject: [PATCH 3/3] Update contract for note sensor --- .../competition/electrical_contract/CompetitionContract.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/competition/electrical_contract/CompetitionContract.java b/src/main/java/competition/electrical_contract/CompetitionContract.java index 460c04ae..f87230ab 100644 --- a/src/main/java/competition/electrical_contract/CompetitionContract.java +++ b/src/main/java/competition/electrical_contract/CompetitionContract.java @@ -173,7 +173,7 @@ public DeviceInfo getInControlNoteSensorDio() { } @Override public DeviceInfo getReadyToFireNoteSensorDio() { - return new DeviceInfo("ReadyToFireNoteSensor", 5, true); + return new DeviceInfo("ReadyToFireNoteSensor", 9, true); } // ArmSubsystem