feat: Add first draft of vision wrappers

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);
+    }
+}

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<CameraSource> visionSources;
+
+    public VisionMap(final List<CameraSource> visionSources) {
+        this.visionSources = visionSources;
+    }
+
+    public <T> void updateData(final PoseEstimator<T> 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);
+                });
+            }
+        }
+    }
+}