Parsing out the vision into a separate subsystem

	modified:   src/main/java/frc/robot/Constants.java
	modified:   src/main/java/frc/robot/RobotContainer.java
	modified:   src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java
	renamed:    src/main/java/frc/robot/subsystems/swervedrive/Vision.java -> src/main/java/frc/robot/subsystems/vision/Vision.java
	new file:   src/main/java/frc/robot/subsystems/vision/VisionIO.java
	new file:   src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java

src/main/java/frc/robot/Constants.java

@@ -17,15 +17,21 @@
 
 package frc.robot;
 
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.ObjectMapper;
 import com.pathplanner.lib.util.PIDConstants;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.RobotBase;
 import frc.robot.util.Alert;
 import frc.robot.util.Alert.AlertType;
+import java.io.IOException;
+import java.nio.file.Path;
+import lombok.Getter;
 import swervelib.math.Matter;
 
 /**
@@ -153,4 +159,39 @@ public static class OperatorConstants {
     public static final double RIGHT_X_DEADBAND = 0.1;
     public static final double TURN_CONSTANT = 6;
   }
+
+  @Getter
+  public enum AprilTagLayoutType {
+    OFFICIAL("2024-official"),
+    SPEAKERS_ONLY("2024-speakers"),
+    AMPS_ONLY("2024-amps"),
+    WPI("2024-wpi");
+
+    private AprilTagLayoutType(String name) {
+      if (Constants.disableHAL) {
+        layout = null;
+      } else {
+        try {
+          layout =
+              new AprilTagFieldLayout(
+                  Path.of(Filesystem.getDeployDirectory().getPath(), "apriltags", name + ".json"));
+        } catch (IOException e) {
+          throw new RuntimeException(e);
+        }
+      }
+      if (layout == null) {
+        layoutString = "";
+      } else {
+        try {
+          layoutString = new ObjectMapper().writeValueAsString(layout);
+        } catch (JsonProcessingException e) {
+          throw new RuntimeException(
+              "Failed to serialize AprilTag layout JSON " + toString() + "for Northstar");
+        }
+      }
+    }
+
+    private final AprilTagFieldLayout layout;
+    private final String layoutString;
+  }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -18,6 +18,9 @@
 package frc.robot;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -176,4 +179,27 @@ public static class Ports {
     // Example:
     // public static final int INTAKE_SERVO = 0;
   }
+
+  /* Vision Constants and Camera Posses */
+  public static class VisionConstants {
+    public static final double ambiguityThreshold = 0.4;
+    public static final double targetLogTimeSecs = 0.1;
+    public static final double fieldBorderMargin = 0.5;
+    public static final double zMargin = 0.75;
+    public static final double xyStdDevCoefficient = 0.005;
+    public static final double thetaStdDevCoefficient = 0.01;
+
+    public static final Pose3d[] cameraPoses =
+        switch (Constants.getRobot()) {
+          case COMPBOT -> new Pose3d[] {
+            new Pose3d(
+                Units.inchesToMeters(-1.0),
+                Units.inchesToMeters(0),
+                Units.inchesToMeters(23.5),
+                new Rotation3d(0.0, Units.degreesToRadians(-20), 0.0)),
+          };
+          case DEVBOT -> new Pose3d[] {};
+          default -> new Pose3d[] {};
+        };
+  }
 }

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

@@ -42,6 +42,7 @@
 import frc.robot.Constants;
 import frc.robot.Constants.AutonConstants;
 import frc.robot.Constants.PhysicalConstants;
+import frc.robot.subsystems.vision.Vision;
 import java.io.File;
 import java.util.function.DoubleSupplier;
 import org.photonvision.PhotonCamera;

src/main/java/frc/robot/subsystems/swervedrive/Vision.java → src/main/java/frc/robot/subsystems/vision/Vision.java

@@ -17,7 +17,7 @@
 //
 // NOTE: This module based on the YAGSL Example Project
 
-package frc.robot.subsystems.swervedrive;
+package frc.robot.subsystems.vision;
 
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;

src/main/java/frc/robot/subsystems/vision/VisionIO.java

@@ -0,0 +1,54 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.vision;
+
+import java.util.ArrayList;
+import java.util.List;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+public interface VisionIO {
+
+  class AprilTagVisionIOInputs implements LoggableInputs {
+    public String camname = "";
+    public double latency = 0.0;
+    public double timestamp = 0.0;
+    public List<PhotonTrackedTarget> targets = new ArrayList<PhotonTrackedTarget>() {};
+
+    public long fps = 0;
+
+    @Override
+    public void toLog(LogTable table) {
+      table.put("Latency", latency);
+      table.put("Timestamp", timestamp);
+      table.put("TargetCount", targets.size());
+      for (int i = 0; i < targets.size(); i++) {
+        table.put("Target/" + i, targets.get(i));
+      }
+      table.put("Fps", fps);
+    }
+
+    @Override
+    public void fromLog(LogTable table) {
+      latency = table.get("Latency", 0.0);
+      timestamp = table.get("Timestamp", 0.0);
+      int targetCount = table.get("TargetCount", 0);
+      targets = new ArrayList<PhotonTrackedTarget>(targetCount);
+      fps = table.get("Fps", 0);
+    }
+  }
+
+  default void updateInputs(AprilTagVisionIOInputs inputs) {}
+}

src/main/java/frc/robot/subsystems/vision/VisionIOPhoton.java

@@ -0,0 +1,77 @@
+// Copyright (c) 2024 Az-FIRST
+// http://github.com/AZ-First
+//
+// This program is free software; you can redistribute it and/or
+// modify it under the terms of the GNU General Public License
+// version 3 as published by the Free Software Foundation or
+// available in the root directory of this project.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+
+package frc.robot.subsystems.vision;
+
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.wpilibj.Timer;
+import frc.robot.Constants.AprilTagLayoutType;
+import frc.robot.util.Alert;
+import java.util.List;
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.Logger;
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+public class VisionIOPhoton implements VisionIO {
+
+  private final Supplier<AprilTagLayoutType> aprilTagTypeSupplier;
+  private AprilTagLayoutType lastAprilTagType = null;
+
+  private static final double disconnectedTimeout = 0.5;
+  private final Alert disconnectedAlert;
+  private final Timer disconnectedTimer = new Timer();
+  public final PhotonCamera camera;
+  public final String camname;
+
+  public VisionIOPhoton(Supplier<AprilTagLayoutType> aprilTagTypeSupplier, String camname) {
+    this.aprilTagTypeSupplier = aprilTagTypeSupplier;
+    this.camname = camname;
+    this.camera = new PhotonCamera(camname);
+
+    disconnectedAlert = new Alert("No data from \"" + camname + "\"", Alert.AlertType.ERROR);
+    disconnectedTimer.start();
+  }
+
+  public void updateInputs(AprilTagVisionIOInputs inputs) {
+    // Get observations
+    var result = camera.getLatestResult();
+
+    // Log the entire result for each camera to AdvantageKit
+    Logger.recordOutput("PhotonVision/" + camname, result); // result.getLatencyMillis());
+
+    // Put the relevant information into the `inputs`
+    inputs.camname = camname;
+    inputs.latency = result.getLatencyMillis();
+    inputs.timestamp = result.getTimestampSeconds();
+
+    if (result.hasTargets()) {
+      List<PhotonTrackedTarget> targets = result.getTargets();
+      inputs.targets = targets;
+      for (PhotonTrackedTarget target : targets) {
+        // Get information from target.
+        int targetID = target.getFiducialId();
+        double poseAmbiguity = target.getPoseAmbiguity();
+        Transform3d bestCameraToTarget = target.getBestCameraToTarget();
+        Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget();
+
+        String logTag = "PhotonVision/Tag" + Integer.toString(targetID);
+        Logger.recordOutput(logTag + "/PoseAmbiguity", poseAmbiguity);
+        Logger.recordOutput(logTag + "/BestCameraToTarget", bestCameraToTarget);
+        Logger.recordOutput(logTag + "/AlternateCameraToTarget", alternateCameraToTarget);
+      }
+    } else {
+      inputs.targets = null;
+    }
+  }
+}