From ee4955485c12aa582d906bd04a0b88927aaca21f Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Sun, 8 Feb 2026 11:23:27 -0500 Subject: [PATCH] Beginnings of a different solution for the PhotonVision cameras --- .../frc/robot/constants/PhotonConstants.java | 8 ++ .../PhotonVision.java | 105 ++++++++++-------- .../robot/utilities/PhotonVisionConfig.java | 10 ++ 3 files changed, 76 insertions(+), 47 deletions(-) rename src/main/java/frc/robot/{utilities => subsystems}/PhotonVision.java (55%) create mode 100644 src/main/java/frc/robot/utilities/PhotonVisionConfig.java diff --git a/src/main/java/frc/robot/constants/PhotonConstants.java b/src/main/java/frc/robot/constants/PhotonConstants.java index a90ec54..5fa37a3 100644 --- a/src/main/java/frc/robot/constants/PhotonConstants.java +++ b/src/main/java/frc/robot/constants/PhotonConstants.java @@ -1,6 +1,9 @@ package frc.robot.constants; +import java.util.List; + import edu.wpi.first.math.geometry.Transform3d; +import frc.robot.utilities.PhotonVisionConfig; public class PhotonConstants { public static final String kCamera1Name = "pv1"; @@ -14,4 +17,9 @@ public class PhotonConstants { public static final double kCamera1PitchRadians = 0; public static final double kCamera2HeightMeters = 0; public static final double kCamera2PitchRadians = 0; + + public static final List configs = List.of( + new PhotonVisionConfig(kCamera1Name, kCamera1RobotToCam, kCamera1HeightMeters, kCamera1PitchRadians), + new PhotonVisionConfig(kCamera2Name, kCamera2RobotToCam, kCamera2HeightMeters, kCamera2PitchRadians) + ); } \ No newline at end of file diff --git a/src/main/java/frc/robot/utilities/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java similarity index 55% rename from src/main/java/frc/robot/utilities/PhotonVision.java rename to src/main/java/frc/robot/subsystems/PhotonVision.java index 7e6d048..a3981c1 100644 --- a/src/main/java/frc/robot/utilities/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -1,9 +1,11 @@ -package frc.robot.utilities; +package frc.robot.subsystems; import java.io.IOException; +import java.util.ArrayList; import java.util.List; import java.util.Optional; import java.util.OptionalDouble; +import java.util.function.Consumer; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; @@ -15,65 +17,74 @@ import org.photonvision.targeting.PhotonTrackedTarget; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.CompetitionConstants; +import frc.robot.constants.PhotonConstants; import frc.robot.interfaces.IAprilTagProvider; import frc.robot.interfaces.IVisualPoseProvider; +import frc.robot.interfaces.IVisualPoseProvider.VisualPose; +import frc.robot.utilities.PhotonVisionConfig; -public class PhotonVision implements IAprilTagProvider,IVisualPoseProvider { +public class PhotonVision extends SubsystemBase implements IAprilTagProvider { + private PhotonCamera[] cameras; + private PhotonPoseEstimator[] estimators; + private PhotonPipelineResult[] latestResults; - private final PhotonCamera camera; + private ArrayList> poseEstimateConsumers; - private final PhotonPoseEstimator photonPoseEstimator; + public PhotonVision() { + cameras = new PhotonCamera[PhotonConstants.configs.size()]; + estimators = new PhotonPoseEstimator[PhotonConstants.configs.size()]; + latestResults = new PhotonPipelineResult[PhotonConstants.configs.size()]; - private final double cameraHeightMeters; - private final double cameraPitchRadians; - - private PhotonPipelineResult latestResult; - - public PhotonVision(String cameraName, Transform3d robotToCam, double cameraHeightMeters, double cameraPitchRadians) throws IOException { - camera = new PhotonCamera(cameraName); - - photonPoseEstimator = new PhotonPoseEstimator( - CompetitionConstants.kTagLayout, - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - robotToCam - ); - - this.cameraHeightMeters = cameraHeightMeters; - this.cameraPitchRadians = cameraPitchRadians; - - this.latestResult = null; - } - - // TODO This periodic method has to be called from somewhere, even though the cameras - // could be used in multiple other subsystems - public void periodic() { - // TODO Do we care about missed results? Probably not, if we're taking long enough to miss results something else is wrong - List results = camera.getAllUnreadResults(); - - if(!results.isEmpty()) { - latestResult = results.get(results.size() - 1); + for(int i = 0; i < PhotonConstants.configs.size(); i++) { + cameras[i] = new PhotonCamera(PhotonConstants.configs.get(i).cameraName()); + estimators[i] = new PhotonPoseEstimator( + CompetitionConstants.kTagLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + PhotonConstants.configs.get(i).robotToCamera() + ); + latestResults[i] = null; } + + poseEstimateConsumers = new ArrayList>(); } @Override - public Optional getVisualPose() { - if(latestResult == null) { - return Optional.empty(); + public void periodic() { + for(int i = 0; i < cameras.length; i++) { + List results = cameras[i].getAllUnreadResults(); + + if(!results.isEmpty()) { + latestResults[i] = results.get(results.size() - 1); + } + + Optional pose = estimators[i].update(latestResults[i]); + + if(!pose.isEmpty()) { + VisualPose visualPose = new VisualPose( + pose.get().estimatedPose.toPose2d(), + pose.get().timestampSeconds + ); + + for(Consumer consumer: poseEstimateConsumers) { + consumer.accept(visualPose); + } + } } + } - Optional pose = photonPoseEstimator.update(latestResult); - - if (pose.isEmpty()) { - return Optional.empty(); - } - - return Optional.of( - new VisualPose( - pose.get().estimatedPose.toPose2d(), - pose.get().timestampSeconds - ) - ); + public Trigger tagPrescenseTrigger(int targetTag) { + return new Trigger(() -> { + return List.of(latestResults).stream() + .filter((p) -> p != null) + .anyMatch((p) -> { + return p.getTargets().stream().map(PhotonTrackedTarget::getFiducialId).anyMatch((i) -> { + return i == targetTag; + }); + }); + }); } @Override diff --git a/src/main/java/frc/robot/utilities/PhotonVisionConfig.java b/src/main/java/frc/robot/utilities/PhotonVisionConfig.java new file mode 100644 index 0000000..576049a --- /dev/null +++ b/src/main/java/frc/robot/utilities/PhotonVisionConfig.java @@ -0,0 +1,10 @@ +package frc.robot.utilities; + +import edu.wpi.first.math.geometry.Transform3d; + +public record PhotonVisionConfig ( + String cameraName, + Transform3d robotToCamera, + double cameraHeightMeters, + double cameraPitchRadians +) {}