diff --git a/src/main/java/com/chopshop166/chopshoplib/maps/CameraSource.java b/src/main/java/com/chopshop166/chopshoplib/maps/CameraSource.java new file mode 100644 index 0000000..92c19de --- /dev/null +++ b/src/main/java/com/chopshop166/chopshoplib/maps/CameraSource.java @@ -0,0 +1,42 @@ +package com.chopshop166.chopshoplib.maps; + +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Transform3d; + +/** Simplified wrapper around a camera with a pose estimator. */ +public class CameraSource { + /** The default field, loaded for convenience. */ + public final AprilTagFieldLayout DEFAULT_FIELD = + AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + /** The camera object. */ + public final PhotonCamera camera; + /** The pose estimator. */ + public final PhotonPoseEstimator estimator; + + /** + * Constructor. + * + * @param cameraName The camera's name in Photon Vision. + * @param robotToCam The offset of the camera from the center of the robot. + */ + public CameraSource(final String cameraName, final Transform3d robotToCam) { + this(new PhotonCamera(cameraName), robotToCam); + } + + /** + * Constructor. + * + * @param cameraName The camera object. + * @param robotToCam The offset of the camera from the center of the robot. + */ + public CameraSource(final PhotonCamera camera, final Transform3d robotToCam) { + this.camera = camera; + this.estimator = new PhotonPoseEstimator(DEFAULT_FIELD, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCam); + this.estimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } +} diff --git a/src/main/java/com/chopshop166/chopshoplib/maps/VisionMap.java b/src/main/java/com/chopshop166/chopshoplib/maps/VisionMap.java new file mode 100644 index 0000000..64e0cd7 --- /dev/null +++ b/src/main/java/com/chopshop166/chopshoplib/maps/VisionMap.java @@ -0,0 +1,27 @@ +package com.chopshop166.chopshoplib.maps; + +import java.util.List; +import edu.wpi.first.math.estimator.PoseEstimator; + +public class VisionMap { + + /** Camera-based position estimators. */ + public final List visionSources; + + public VisionMap(final List visionSources) { + this.visionSources = visionSources; + } + + public void updateData(final PoseEstimator estimator) { + for (var source : this.visionSources) { + var results = source.camera.getAllUnreadResults(); + if (!results.isEmpty()) { + var estimate = source.estimator.update(results.get(results.size() - 1)); + estimate.ifPresent(est -> { + estimator.addVisionMeasurement(est.estimatedPose.toPose2d(), + est.timestampSeconds); + }); + } + } + } +}