diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7ee63a9..8e094a0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -5,6 +5,11 @@ package frc.robot; import static edu.wpi.first.units.Units.*; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import frc.robot.generated.TunerConstants; @@ -56,4 +61,34 @@ public static class DrivetrainConstants { public static final double ROTATION_KI = 0.0; public static final double ROTATION_KD = 0.0; } + + public static class VisionConstants { + + // --- vision utils --- + public static final double MAX_VISION_POSE_DISTANCE = 1; + public static final double MAX_VISION_POSE_Z = 0.1; + public static final double MAX_VISION_POSE_ROLL = 0.05; // in radians + public static final double MAX_VISION_POSE_PITCH = 0.05; // in radians + + // --- localization camera --- + // default vision standard deviation + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(6, 6, 4); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 0.3); + + public static final double VISION_DISTANCE_DISCARD = 10; + public static final double MAX_POSE_AMBIGUITY = 0.2; + public static final double MAX_AVG_DIST_BETWEEN_LAST_EST_POSES = 0.3; // in meters + public static final double MAX_AVG_SPEED_BETWEEN_LAST_EST_POSES = MAX_AVG_DIST_BETWEEN_LAST_EST_POSES * 50.; + public static final int NUM_LAST_EST_POSES = 3; + public static final double STD_DEV_SCALER = 30; + + + // --- vision subsystem --- + // (camera setup) + public static final String CAMERA1_NAME = null; + public static final String CAMERA2_NAME = null; + public static final Transform3d ROBOT_TO_CAM1_3D = null; + public static final Transform3d ROBOT_TO_CAM2_3D = null; + + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 47d3d5a..c72fc2f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -42,6 +42,9 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + + // Update pose estimate with vision measurements + m_robotContainer.updatePoseEst(); } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6882e8c..0cdbdd7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,16 +8,27 @@ import frc.robot.generated.TunerConstants; import frc.robot.subsystems.drivetrain.CommandSwerveDrivetrain; import frc.robot.subsystems.drivetrain.DrivetrainCommandFactory; +import frc.robot.subsystems.vision.LocalizationCamera; +import frc.robot.subsystems.vision.VisionSubsystem; import frc.robot.Constants.OperatorConstants; - +import frc.robot.Constants.VisionConstants; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.DataLogManager; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import java.util.List; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; + +import com.ctre.phoenix6.Utils; import com.pathplanner.lib.auto.AutoBuilder; /** @@ -33,6 +44,7 @@ public record JoystickVals(double x, double y) {} // Subsystems public final CommandSwerveDrivetrain m_drivetrain = TunerConstants.createDrivetrain(); + public final VisionSubsystem m_vision = new VisionSubsystem(); // Command factories private final DrivetrainCommandFactory m_drivetrainCommandFactory = new DrivetrainCommandFactory(m_drivetrain); @@ -40,6 +52,8 @@ public record JoystickVals(double x, double y) {} private final CommandXboxController m_driverJoystick = new CommandXboxController(OperatorConstants.kDriverControllerPort); + private final Field2d m_actualField = new Field2d(); // field simulation + /** The container for the robot. Contains subsystems and commands. */ public RobotContainer() { // Configure trigger bindings @@ -93,4 +107,61 @@ private void createNamedCommands() { public Command getAutonomousCommand() { return m_autoChooser.getSelected(); } + + + public void updatePoseEst() { + List cameras = m_vision.getLocalizationCameras(); + + for (LocalizationCamera cam : cameras){ + updatePoseEst(cam); + } + + m_actualField.setRobotPose(m_drivetrain.getState().Pose); + } + + public void updatePoseEst(LocalizationCamera camera){ + EstimatedRobotPose robotPose = camera.getRobotPose(); + + if (robotPose != null && camera.getTargetFound() && camera.getIsNewResult()) { + Pose3d estPose3d = robotPose.estimatedPose; // estimated robot pose of vision + Pose2d estPose2d = estPose3d.toPose2d(); + + // check if new estimated pose and previous pose are less than 2 meters apart (fused poseEst) + double distance = estPose2d.getTranslation().getDistance(m_drivetrain.getState().Pose.getTranslation()); + + SmartDashboard.putNumber("fusedVision/" + camera.getCameraName() + "/distanceBetweenVisionAndActualPose", distance); + // Only accept vision measurement if distance is reasonable (jumpy check already done in LocalizationCamera) + if (distance < VisionConstants.MAX_VISION_POSE_DISTANCE) { + m_drivetrain.setVisionMeasurementStdDevs(camera.getCurrentStdDevs()); + + // sample drivetrain fusedPose before updating + Optional samplePose = m_drivetrain.samplePoseAt(Utils.fpgaToCurrentTime(robotPose.timestampSeconds)); + + if (samplePose.isPresent()){ + SmartDashboard.putNumberArray("fusedVision/" + camera.getCameraName() + "/samplePose", new double [] { + samplePose.get().getX(), samplePose.get().getY(), samplePose.get().getRotation().getRadians()}); + } + + SmartDashboard.putNumberArray("fusedVision/" + camera.getCameraName() + "/drivetrainBeforeUpdate", new double [] { + m_drivetrain.getState().Pose.getX(), m_drivetrain.getState().Pose.getY(), m_drivetrain.getState().Pose.getRotation().getRadians()}); + + + m_drivetrain.addVisionMeasurement(estPose2d, Utils.fpgaToCurrentTime(robotPose.timestampSeconds)); + camera.updateField(estPose2d); + + // sample drivetrain fusedPose after updating + SmartDashboard.putNumberArray("fusedVision/" + camera.getCameraName() + "/drivetrainAfterUpdate", new double [] { + m_drivetrain.getState().Pose.getX(), m_drivetrain.getState().Pose.getY(), m_drivetrain.getState().Pose.getRotation().getRadians()}); + + SmartDashboard.putNumberArray("fusedVision/" + camera.getCameraName() + "/visionPose2dFiltered" + camera.getCameraName(), new double[] {estPose2d.getX(), estPose2d.getY(), estPose2d.getRotation().getRadians()}); + } + + SmartDashboard.putNumberArray("fusedVision/" + camera.getCameraName() + "/visionPose3D", new double[] { + estPose3d.getX(), + estPose3d.getY(), + estPose3d.getZ(), + estPose3d.getRotation().toRotation2d().getRadians() + }); // post vision 3d to smartdashboard + } + } } diff --git a/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java new file mode 100644 index 0000000..d92a71d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/LocalizationCamera.java @@ -0,0 +1,266 @@ +package frc.robot.subsystems.vision; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + + +import java.util.*; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +import frc.robot.Constants.VisionConstants; + +public class LocalizationCamera { + + private final PhotonCamera m_camera; + private final String m_cameraName; + + private boolean targetFound; // true if the translation2d was updated last periodic() call + private EstimatedRobotPose estimatedRobotPose; + private boolean isNewResult = false; + + private Matrix curStdDevs = VisionConstants.kSingleTagStdDevs; + + private AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + private final Field2d m_estPoseField = new Field2d(); // field pose estimator + private PhotonPoseEstimator poseEstimator; + + private PhotonTrackedTarget m_apriltagTarget; + private LinkedList m_lastEstPoses = new LinkedList<>(); + + public LocalizationCamera(String cameraName, Transform3d robotToCam) { + m_cameraName = cameraName; + m_camera = new PhotonCamera(m_cameraName); + poseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCam); + + SmartDashboard.putBoolean("isConnected/" + m_cameraName, m_camera.isConnected()); + } + + public String getCameraName() { + return m_cameraName; + } + + public PhotonTrackedTarget getTarget() { + return m_apriltagTarget; + } + + public Double getTargetPoseAmbiguity() { + return m_apriltagTarget.getPoseAmbiguity(); + } + + public boolean getTargetFound() { + return targetFound; + } + + public EstimatedRobotPose getRobotPose(){ + return estimatedRobotPose; + } + + public Field2d getEstField(){ + return m_estPoseField; + } + + public boolean getIsNewResult(){ + return isNewResult; + } + + public Matrix getCurrentStdDevs(){ + return curStdDevs; + } + + // Updates the field simulation in elastic + public void updateField(Pose2d newPos){ + m_estPoseField.setRobotPose(newPos); + + SmartDashboard.putData("est pose field/" + m_cameraName + "/", m_estPoseField); + } + + + // processes all targets + // uses area, standard deviation checks (outlier results?) and "sane-ness" (are readings > pre-determined maximum constants?) + public void findTarget() { + + ArrayList apriltagIDs = new ArrayList<>(); + ArrayList targetPoseAmbiguity = new ArrayList<>(); + List results; + + results = m_camera.getAllUnreadResults(); + + if (!results.isEmpty()){ + // Camera processed a new frame since last + // Get the last one in the list. + var result = results.get(results.size() - 1); + isNewResult = true; + + if (result.hasTargets()) { + double biggestTargetArea = 0; + + for (PhotonTrackedTarget sampleTarget : result.getTargets()){ + apriltagIDs.add(sampleTarget.getFiducialId()); + targetPoseAmbiguity.add(sampleTarget.getPoseAmbiguity()); + //loops through every sample target in results.getTargets() + //if the sample target's area is bigger than the biggestTargetArea, then the sample target + // is set to the target, and the biggest Target Area is set to the sample's target area + // we want the april tag with the biggest area since that means it is the closest + if (sampleTarget.getArea() > biggestTargetArea){ + biggestTargetArea = sampleTarget.getArea(); + m_apriltagTarget = sampleTarget; + } + } + + SmartDashboard.putNumber("vision/" + m_cameraName + "Alternate Target X", m_apriltagTarget.getBestCameraToTarget().getX()); + SmartDashboard.putNumber("vision/" + m_cameraName + "Alternate Target Y", m_apriltagTarget.getBestCameraToTarget().getY()); + + if (m_apriltagTarget.getPoseAmbiguity() > VisionConstants.MAX_POSE_AMBIGUITY) { + SmartDashboard.putString("vision/" + m_cameraName + "/targetState", "targetDiscardedAmbiguity"); + targetFound = false; + } else { + SmartDashboard.putString("vision/" + m_cameraName + "/targetState", "targetFound"); + targetFound = true; + } + + } else { + SmartDashboard.putString("vision/" + m_cameraName + "/targetState", "noTarget"); + targetFound = false; + } + + if (targetFound) { + // update the estimated robot pose if the pose estimator outputs something + Optional poseEstimatorOutput = poseEstimator.update(result); + + // update standard deviation based on dist + this.updateEstimationStdDevs(poseEstimatorOutput, result.getTargets()); + + if (poseEstimatorOutput.isPresent() && VisionUtils.poseIsSane(poseEstimatorOutput.get().estimatedPose)) { + // Temporarily add new pose to history to check if it makes the sequence jumpy + m_lastEstPoses.add(poseEstimatorOutput.get()); + + if (m_lastEstPoses.size() > VisionConstants.NUM_LAST_EST_POSES) { + m_lastEstPoses.removeFirst(); + } + + // Check if the pose estimate is jumpy (indicates bad data or camera shift) + if (isEstPoseJumpy()) { + // reject pose by removing from history m_lastEstPoses + m_lastEstPoses.removeLast(); + estimatedRobotPose = null; // resets estimatedRobotPose to null if jumpy; stops processing in updatePoseEst + SmartDashboard.putString("vision/" + m_cameraName + "/targetState", "targetDiscardedJumpy"); + } else { + // accept pose by changing instance var estimatedRobotPose + estimatedRobotPose = poseEstimatorOutput.get(); + SmartDashboard.putString("vision/" + m_cameraName + "/targetState", "targetFound"); + } + + SmartDashboard.putBoolean("vision/" + m_cameraName + "/zIsSane", VisionUtils.zIsSane(poseEstimatorOutput.get().estimatedPose)); + SmartDashboard.putBoolean("vision/" + m_cameraName + "/rollIsSane", VisionUtils.rollIsSane(poseEstimatorOutput.get().estimatedPose)); + SmartDashboard.putBoolean("vision/" + m_cameraName + "/pitchIsSane", VisionUtils.pitchIsSane(poseEstimatorOutput.get().estimatedPose)); + } + } + else{ + isNewResult = false; + } + } + SmartDashboard.putBoolean("vision/" + m_cameraName + "/isConnected", m_camera.isConnected()); + SmartDashboard.putBoolean("vision/" + m_cameraName + "/isNewResult", getIsNewResult()); + + SmartDashboard.putNumberArray("vision/" + m_cameraName + "/Targets Seen", apriltagIDs.stream().mapToDouble(Integer::doubleValue).toArray()); + SmartDashboard.putNumberArray("vision/" + m_cameraName + "/Target Pose Ambiguities", targetPoseAmbiguity.stream().mapToDouble(Double::doubleValue).toArray()); + SmartDashboard.putBoolean("vision/" + m_cameraName + "/Target Found", targetFound); + + SmartDashboard.putBoolean("vision/" + m_cameraName + "/isEstPoseJumpy", isEstPoseJumpy()); + SmartDashboard.putNumberArray("vision/" + m_cameraName + "/standardDeviations", curStdDevs.getData()); + + SmartDashboard.putBoolean("isConnected/" + m_cameraName, m_camera.isConnected()); + } + + // Standard deviation measures how "spread out" / accurate a vision reading is + private void updateEstimationStdDevs(Optional estimatedPose, List targets) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + SmartDashboard.putNumber("vision/" + m_cameraName + "/standardDeviation-average-distance", Double.MAX_VALUE); + SmartDashboard.putString("vision/" + m_cameraName + "/standardDeviation-state", "empty"); + } else { + // Pose present. Start running Heuristic + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an + // average-distance metric + for (var tgt : targets) { + var tagPose = poseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) + continue; + numTags++; + avgDist += tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + SmartDashboard.putString("vision/" + m_cameraName + "/standardDeviation-state", "no tags visible"); + } else if (numTags == 1 && avgDist > VisionConstants.VISION_DISTANCE_DISCARD) { + curStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + SmartDashboard.putString("vision/" + m_cameraName + "/standardDeviation-state", "target too far"); + } else { + var unscaledStdDevs = numTags > 1 ? VisionConstants.kMultiTagStdDevs : VisionConstants.kSingleTagStdDevs; + + avgDist /= numTags; + // increase std devs based on (average) distance + curStdDevs = unscaledStdDevs.times(1 + (avgDist * avgDist / VisionConstants.STD_DEV_SCALER)); + SmartDashboard.putString("vision/" + m_cameraName + "/standardDeviation-state", "good :)"); + } + SmartDashboard.putNumber("vision/" + m_cameraName + "/standardDeviation-average-distance", avgDist); + } + } + + // checks if the pose is jumpy based on avg speed since by calculating based on speed, + // the camera fps doesn't matter as the speed between readings will still be the same. this is based on last 3 readings + public boolean isEstPoseJumpy() { + if (m_lastEstPoses.size() < VisionConstants.NUM_LAST_EST_POSES) { + return true; + } + + double totalDistance = 0; + double totalTime = 0; + + for (int i = 0; i < m_lastEstPoses.size() - 1; i++) { + // add distance between ith pose and i+1th pose + totalDistance += Math.abs(m_lastEstPoses.get(i).estimatedPose.toPose2d().minus(m_lastEstPoses.get(i + 1).estimatedPose.toPose2d()).getTranslation().getNorm()); + totalTime += Math.abs(m_lastEstPoses.get(i).timestampSeconds - m_lastEstPoses.get(i+1).timestampSeconds); + } + + // divide by number of intervals (n-1) + double avgDist = totalDistance / (m_lastEstPoses.size() - 1); + double avgTime = totalTime / (m_lastEstPoses.size() - 1); + if (avgTime == 0){ + return true; + } + double avgSpeed = avgDist/avgTime; + + SmartDashboard.putNumber("vision/" + m_cameraName + "/avgDistBetweenLastEstPoses", avgDist); + SmartDashboard.putNumber("vision/" + m_cameraName + "/avgSpeedBetweenLastEstPoses", avgSpeed); + SmartDashboard.putNumber("vision/" + m_cameraName + "/avgTimeBetweenLastEstPoses", avgTime); + + return avgSpeed > VisionConstants.MAX_AVG_SPEED_BETWEEN_LAST_EST_POSES; + } +} + diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java new file mode 100644 index 0000000..bb9b811 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -0,0 +1,88 @@ +// 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.vision; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import java.util.ArrayDeque; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Comparator; +import java.util.List; +import java.util.Optional; +import java.util.function.Supplier; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.PhotonUtils; +import org.photonvision.targeting.PhotonTrackedTarget; + +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.hardware.Pigeon2; + +import frc.robot.Constants; +import frc.robot.Constants.VisionConstants; +import frc.robot.subsystems.vision.VisionUtils; + + + +public class VisionSubsystem extends SubsystemBase { + private ArrayList cameras = new ArrayList<>(); + private List camerasWithValidPose = new ArrayList<>(); + + /** Creates a new Vision Subsystem. */ + public VisionSubsystem() { + cameras.add(new LocalizationCamera(VisionConstants.CAMERA1_NAME, VisionConstants.ROBOT_TO_CAM1_3D)); + cameras.add(new LocalizationCamera(VisionConstants.CAMERA2_NAME, VisionConstants.ROBOT_TO_CAM2_3D)); + } + + public List getLocalizationCameras(){ + return camerasWithValidPose; + } + + @Override + public void periodic() { + for (LocalizationCamera cam : cameras){ + cam.findTarget(); + } + + // sorts the camera readings by time (care less about older readings) + camerasWithValidPose = cameras.stream() // turn the list into a stream + .filter((camera) -> { // only get the cameras with a valid EstimatedRobotPose + return camera.getRobotPose() != null && camera.getTargetFound(); + }) + .sorted((camera_a, camera_b) -> { // simplified comparator because we've filtered out invalid readings. + EstimatedRobotPose poseA = camera_a.getRobotPose(); + EstimatedRobotPose poseB = camera_b.getRobotPose(); + return Double.compare(poseA.timestampSeconds, poseB.timestampSeconds); + }) + .toList(); + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/VisionUtils.java b/src/main/java/frc/robot/subsystems/vision/VisionUtils.java new file mode 100644 index 0000000..3097c97 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionUtils.java @@ -0,0 +1,48 @@ +package frc.robot.subsystems.vision; + +import java.util.*; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import frc.robot.Constants.VisionConstants; + +public class VisionUtils { + + private static AprilTagFieldLayout aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + private static List reefAprilTagPoses = new ArrayList<>(); + + static { + // create the list of apriltag poses + List reefAprilTagIDs = new ArrayList<>(Arrays.asList(6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22)); + for (Integer id : reefAprilTagIDs) { + reefAprilTagPoses.add(aprilTagFieldLayout.getTagPose(id).get().toPose2d()); + } + } + + public static Pose2d getClosestReefAprilTag(Pose2d robotPose) { + return robotPose.nearest(reefAprilTagPoses); + } + + // sanity check: does the pose "read" by vision make sense? (based on predetermined maximum constants) + public static boolean poseIsSane(Pose3d pose) { + return zIsSane(pose) && rollIsSane(pose) && pitchIsSane(pose); + } + + public static boolean zIsSane(Pose3d pose) { + return pose.getZ() < VisionConstants.MAX_VISION_POSE_Z; + + } + + public static boolean rollIsSane(Pose3d pose) { + return pose.getRotation().getX() < VisionConstants.MAX_VISION_POSE_ROLL; + + } + + public static boolean pitchIsSane(Pose3d pose) { + return pose.getRotation().getY() < VisionConstants.MAX_VISION_POSE_PITCH; + + } + +}