new camera class

src/main/java/common/hardware/camera/Camera.java

@@ -1,6 +1,14 @@
 package common.hardware.camera;
 
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
 
 /**
  * Represents and holds the specs of a camera.
@@ -12,24 +20,43 @@ public class Camera {
 
     public final String name;
 
-    public final Transform2d offset;
+    public final double cameraPitch;
+
+    public final double xOffset;
 
-    public boolean enabled;
+    public final double yOffset;
 
-    public final double cameraPitch;
+    public final double pitchOffset;
+
+    public final double yawOffset;
+
+    public final AprilTagFields aprilTags;
+
+    public final PoseStrategy calc_strategy;
+
+    public final PhotonPoseEstimator estimator;
 
-    /**
-     * Creates a new object for a camera.
-     * 
-     * @param name Name of camera on photonvision.
-     * @param enabled Whether or not to return results.
-     * @param cameraOffset Offset from robot as a transform2d.
-     */
-    public Camera(String name, boolean enabled, Transform2d cameraOffset, double cameraPitch) {
+    public Camera(String name, double cameraPitch, double xOffset, double yOffset, double pitchOffset, double yawOffset,
+            AprilTagFields aprilTags, PoseStrategy calc_strategy) {
         this.name = name;
-        this.enabled = enabled;
-        this.offset = cameraOffset;
         this.cameraPitch = cameraPitch;
+        this.xOffset = xOffset;
+        this.yOffset = yOffset;
+        this.pitchOffset = pitchOffset;
+        this.yawOffset = yawOffset;
+        this.aprilTags = aprilTags;
+        this.calc_strategy = calc_strategy;
+
+        Transform3d offset = new Transform3d(xOffset, yOffset, 0, 
+        new Rotation3d(0.0, pitchOffset, 0.0).rotateBy(new Rotation3d(0.0,0.0,yawOffset))
+        );
+
+        estimator = new PhotonPoseEstimator(aprilTags.loadAprilTagLayoutField(), 
+        calc_strategy,
+        new PhotonCamera(name), offset
+        );
+
+        estimator.setReferencePose(new Pose2d());
     }
 
 }