Add PhotonVision implementation

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

@@ -4,6 +4,8 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Transform3d;
+
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide
  * numerical or boolean
@@ -31,6 +33,8 @@ public static class RobotConstants {
      */
     public static final double WHEEL_DIAMETER_INCHES = 4.0;
 
+    public static final Transform3d APRILTAG_CAMERA_TO_ROBOT = new Transform3d();
+
 
   /**
    * Defines operator (i.e. driver and manipulator) constants.

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

@@ -87,7 +87,8 @@ public void initShuffleboard(){
     m_autonomous.addShuffleboardLayout(operatorTab);
 
     RobotPreferences.addShuffleBoardTab();
-
+    
     m_subsystems.drivetrain.addShuffleboardTab();
+    m_subsystems.aprilTag.addShuffleboardTab();
   }
 }

src/main/java/frc/robot/subsystems/AprilTagSubsystem.java

@@ -0,0 +1,152 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import java.util.Optional;
+
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import com.nrg948.preferences.RobotPreferences;
+import com.nrg948.preferences.RobotPreferencesLayout;
+import com.nrg948.preferences.RobotPreferencesValue;
+
+import edu.wpi.first.cscore.HttpCamera;
+import edu.wpi.first.cscore.HttpCamera.HttpCameraKind;
+import edu.wpi.first.cscore.VideoSource;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import frc.robot.Constants.RobotConstants;
+
+/**
+ * This subsystem is responsible for getting target information from
+ * PhotonVision.
+ */
+
+@RobotPreferencesLayout(groupName = "AprilTag", row = 1, column = 4, width = 2, height = 1)
+public class AprilTagSubsystem extends PhotonVisionSubsystemBase {
+
+  @RobotPreferencesValue
+  public static final RobotPreferences.BooleanValue enableTab = new RobotPreferences.BooleanValue(
+      "AprilTag", "Enable Tab", false);
+
+  private DoubleLogEntry targetXLogger = new DoubleLogEntry(DataLogManager.getLog(), "AprilTag/Target X");
+  private DoubleLogEntry targetYLogger = new DoubleLogEntry(DataLogManager.getLog(), "AprilTag/Target Y");
+  private DoubleLogEntry targetAngleLogger = new DoubleLogEntry(DataLogManager.getLog(), "AprilTag/Target Angle");
+
+  private SendableChooser<Integer> aprilTagIdChooser = new SendableChooser<Integer>();
+
+  /** Creates a new PhotonVisionSubsystem. */
+  public AprilTagSubsystem() {
+    super("AprilTagCamera", RobotConstants.APRILTAG_CAMERA_TO_ROBOT);
+
+    for (int i = 1; i <= 16; i++) {
+      aprilTagIdChooser.addOption(String.valueOf(i), i);
+    }
+  }
+
+  @Override
+  public void updatePoseEstimate(SwerveDrivePoseEstimator estimator, Pose3d targetPose) {
+    if (hasTargets()) {
+      Transform3d cameraToRobot = getCameraToRobotTransform();
+      Transform3d targetToCamera = new Transform3d(
+          new Translation3d(
+              getDistanceToBestTarget(),
+              new Rotation3d(0, 0, Math.toRadians(-getAngleToBestTarget()))),
+          cameraToRobot.getRotation()).inverse();
+      Pose3d cameraPose = targetPose.transformBy(targetToCamera);
+      Pose3d robotPose = cameraPose.transformBy(cameraToRobot);
+
+      estimator.addVisionMeasurement(robotPose.toPose2d(), getTargetTimestamp());
+    }
+
+    targetXLogger.append(targetPose.getX());
+    targetYLogger.append(targetPose.getY());
+    targetAngleLogger.append(Math.toRadians(targetPose.getRotation().getAngle()));
+  }
+
+  /**
+   * Returns the AprilTag target of the input ID.
+   * 
+   * @param id The AprilTag ID.
+   * @return The target with the input ID.
+   */
+  public Optional<PhotonTrackedTarget> getTarget(int id) {
+    return getTargets().stream().filter(target -> target.getFiducialId() == id).findFirst();
+  }
+
+  /**
+   * Returns the transform from the camera to the AprilTag with input ID.
+   * @param id The AprilTag ID.
+   * @return The transform from the camera to the AprilTag with input ID.
+   */
+  public Transform3d getCameraToTarget(int id) {
+    return getTarget(id).get().getBestCameraToTarget();
+  }
+
+  /**
+   * Returns the distance to the target with the input ID. Returns 0 if target not found.
+   * 
+   * @param id The AprilTag ID.
+   * @return The distance to the target with the input ID.
+   */
+  public double getDistanceToTarget(int id) {
+    Optional<PhotonTrackedTarget> target = getTarget(id);
+    if (target.isEmpty()) {
+      return 0.0;
+    }
+    var bestCameraToTarget = target.get().getBestCameraToTarget();
+    return Math.hypot(bestCameraToTarget.getX(), bestCameraToTarget.getY());
+  }
+
+  /**
+   * Returns the angle to the target with the input ID. Returns 0 if target not found.
+   * 
+   * @param id The AprilTag ID.
+   * @return The angle to the target with the input ID.
+   */
+  public double getAngleToTarget(int id) {
+    Optional<PhotonTrackedTarget> target = getTarget(id);
+    if (target.isEmpty()) {
+      return 0.0;
+    }
+    return target.get().getYaw();
+  }
+
+    /**
+   * Adds a tab for April Tag in Shuffleboard.
+   */
+  public void addShuffleboardTab() {
+    if (!enableTab.getValue()) {
+      return;
+    }
+
+    ShuffleboardTab visionTab = Shuffleboard.getTab("April Tag");
+    ShuffleboardLayout targetLayout = visionTab.getLayout("Target Info", BuiltInLayouts.kList)
+        .withPosition(0, 0)
+        .withSize(2, 5);
+    targetLayout.add("Id Selection", aprilTagIdChooser).withWidget(BuiltInWidgets.kComboBoxChooser);
+    targetLayout.addBoolean("Has Target", this::hasTargets);
+    targetLayout.addDouble("Distance", () -> getDistanceToTarget(aprilTagIdChooser.getSelected()));
+    targetLayout.addDouble("Angle", () -> getAngleToTarget(aprilTagIdChooser.getSelected()));
+
+    VideoSource video = new HttpCamera("photonvision_Port_1182_MJPEG_Server", "http://10.9.48.11:1182/?action=stream",
+        HttpCameraKind.kMJPGStreamer);
+    visionTab.add("April Tag", video)
+        .withWidget(BuiltInWidgets.kCameraStream)
+        .withPosition(2, 0)
+        .withSize(4, 3);
+  }
+}

src/main/java/frc/robot/subsystems/PhotonVisionSubsystemBase.java

@@ -0,0 +1,158 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+import java.util.List;
+
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.util.datalog.BooleanLogEntry;
+import edu.wpi.first.util.datalog.DoubleLogEntry;
+import edu.wpi.first.wpilibj.DataLogManager;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * This is a base class for subsystems responsible for getting target
+ * information from PhotonVision.
+ */
+public abstract class PhotonVisionSubsystemBase extends SubsystemBase {
+
+  private final PhotonCamera camera;
+  private final Transform3d cameraToRobot;
+  private final Transform3d robotToCamera;
+  private PhotonPipelineResult result = new PhotonPipelineResult();
+
+  private BooleanLogEntry hasTargetLogger;
+  private DoubleLogEntry distanceLogger;
+  private DoubleLogEntry angleLogger;
+
+  /**
+   * Creates a new PhotonVisionSubsystemBase.
+   * 
+   * @param cameraName    The name of the PhotonVision camera.
+   * @param cameraToRobot The transform from the camera to center of the robot.
+   */
+  public PhotonVisionSubsystemBase(String cameraName, Transform3d cameraToRobot) {
+    this.camera = new PhotonCamera(cameraName);
+    this.cameraToRobot = cameraToRobot;
+    this.robotToCamera = cameraToRobot.inverse();
+
+    hasTargetLogger = new BooleanLogEntry(DataLogManager.getLog(), String.format("/%s/Has Target", cameraName));
+    distanceLogger = new DoubleLogEntry(DataLogManager.getLog(), String.format("/%s/Distance", cameraName));
+    angleLogger = new DoubleLogEntry(DataLogManager.getLog(), String.format("/%s/Angle", cameraName));
+  }
+
+  @Override
+  public void periodic() {
+    PhotonPipelineResult currentResult = camera.getLatestResult();
+
+    if (this.result.hasTargets() != currentResult.hasTargets()) {
+      hasTargetLogger.append(currentResult.hasTargets());
+    }
+
+    this.result = currentResult;
+
+    if (hasTargets()) {
+      distanceLogger.append(getDistanceToBestTarget());
+      angleLogger.append(-getAngleToBestTarget());
+    }
+  }
+
+  /**
+   * Updates the pose estimate based on vision information using the expected
+   * target pose.
+   * 
+   * @param estimator  The pose estimator.
+   * @param targetPose The expected pose of the target in the field of vision. The
+   *                   pose is specified in field-relative coordinates.
+   */
+  public abstract void updatePoseEstimate(SwerveDrivePoseEstimator estimator, Pose3d targetPose);
+
+  /**
+   * Returns the transform from the camera to center of the robot.
+   * 
+   * @return The transform from the camera to center of the robot.
+   */
+  public Transform3d getCameraToRobotTransform() {
+    return cameraToRobot;
+  }
+
+  /**
+   * Returns the transform from the center of the robot to the camera.
+   * 
+   * @return The transform from the center of the robot to the camera.
+   */
+  public Transform3d getRobotToCameraTransform() {
+    return robotToCamera;
+  }
+
+  /**
+   * Returns whether the result contains any targets.
+   * 
+   * @return Returns true if there are targets.
+   */
+  public boolean hasTargets() {
+    return result.hasTargets();
+  }
+
+  /**
+   * Returns information on the best target.
+   * 
+   * @return Information on the best target.
+   */
+  public PhotonTrackedTarget getBestTarget() {
+    return result.getBestTarget();
+  }
+
+  /**
+   * Returns the distance to the best target.
+   * 
+   * @return The distance, in meters, to the best target.
+   */
+  public double getDistanceToBestTarget() {
+    if (!hasTargets()) {
+      return 0;
+    }
+
+    Transform3d bestTarget = getBestTarget().getBestCameraToTarget();
+    return Math.hypot(bestTarget.getX(), bestTarget.getY());
+  }
+
+  /**
+   * Returns the angle to the best target.
+   * 
+   * @return The angle to the best target.
+   */
+  public double getAngleToBestTarget() {
+    if (!hasTargets()) {
+      return 0;
+    }
+
+    return getBestTarget().getYaw();
+  }
+
+  /**
+   * Returns the estimated time, in seconds, the target was detected.
+   * 
+   * @return The timestamp in seconds or -1 if no target was detected.
+   */
+  public double getTargetTimestamp() {
+    return result.getTimestampSeconds();
+  }
+
+  /**
+   * Returns a list of visible targets.
+   * 
+   * @return A list of visible targets.
+   */
+  public List<PhotonTrackedTarget> getTargets() {
+    return result.getTargets();
+  }
+}

src/main/java/frc/robot/subsystems/Subsystems.java

@@ -7,5 +7,5 @@
 /** Add your docs here. */
 public class Subsystems {
     public SwerveSubsystem drivetrain = new SwerveSubsystem();
-
+    public AprilTagSubsystem aprilTag = new AprilTagSubsystem();
 }