Clean up Vision

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

@@ -200,16 +200,19 @@ public void periodic() {
 				m_backLeft.getModuleState(), m_backRight.getModuleState() };
 		m_currentModuleStatePublisher.set(states);
 		m_poseEstimator.update(getHeading(), getModulePositions());
-		var estPose = m_vision.getEstimatedGlobalPose();
-		estPose.ifPresent(
-				est -> {
-					System.out.println("Vision update");
-					m_visionPosePublisher.set(est.estimatedPose.toPose2d());
-					m_poseEstimator.addVisionMeasurement(
-							est.estimatedPose.toPose2d(),
-							est.timestampSeconds,
-							m_vision.currentSTD);
-				});
+		var results = m_vision.refreshResults();
+		if (results.size() > 0) {
+			var estPose = m_vision.getEstimatedGlobalPose(results);
+			estPose.ifPresent(
+					est -> {
+						System.out.println("Vision update");
+						m_visionPosePublisher.set(est.estimatedPose.toPose2d());
+						m_poseEstimator.addVisionMeasurement(
+								est.estimatedPose.toPose2d(),
+								est.timestampSeconds,
+								m_vision.currentSTD);
+					});
+		}
 		m_posePublisher.set(m_poseEstimator.getEstimatedPosition());
 	}
 

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

@@ -29,7 +29,6 @@
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class Vision {
 	private List<PhotonPipelineResult> m_results;
@@ -62,54 +61,18 @@ public Vision() {
 
 	}
 
-	/**
-	 * Gets the ID of the best target seen by the camera.
-	 * 
-	 * @return The Fiducial ID of the viewed April Tag.
-	 */
-	public int getTargetId() {
-		List<PhotonPipelineResult> results = this.m_results;
-
-		if (results.size() < 1) {
-			return -1;
-		}
-
-		PhotonPipelineResult result = results.get(0);
-
-		SmartDashboard.putBoolean("Has Targets", result.hasTargets());
-
-		if (result.hasTargets()) {
-			return result.getBestTarget().getFiducialId();
-		}
-
-		else {
-			return -1;
-		}
-	}
-
-	public List<PhotonPipelineResult> getTargets() {
+	public List<PhotonPipelineResult> refreshResults() {
 		m_results = m_camera.getAllUnreadResults();
 		return m_results;
 	}
 
-	/**
-	 * Identifies the best target seen by the camera
-	 * (should be run periodically for up-to-date results).
-	 * 
-	 * @return The best target (April Tag)
-	 */
-	public PhotonTrackedTarget getBestTarget() {
-		return this.m_results.get(0).getBestTarget();
-	}
-
 	/**
 	 * Gets the field-relative pose of the target.
 	 * 
 	 * @param target The target (April Tag) viewed by the camera.
 	 * @return The target's pose
 	 */
 	public Pose3d getTargetPose(PhotonTrackedTarget target) {
-		target = (target == null) ? null : target; // TODO check for null in frontend instead
 		return aprilTagFieldLayout.getTagPose(target.getFiducialId()).get();
 	}
 
@@ -120,14 +83,14 @@ public Pose3d getTargetPose(PhotonTrackedTarget target) {
 	 * @return The robot's pose
 	 */
 	public Pose3d getVisionPose(PhotonTrackedTarget target) {
-		target = (target == null) ? null : target;
 		if (aprilTagFieldLayout.getTagPose(target.getFiducialId()).isPresent()) {
 			return PhotonUtils.estimateFieldToRobotAprilTag(
 					target.getBestCameraToTarget(),
 					getTargetPose(target),
 					new Transform3d(-0.5, -0.5, -0.5, new Rotation3d())); // TODO fix camera to robot
-		} else
+		} else {
 			return null;
+		}
 	}
 
 	/**
@@ -197,9 +160,9 @@ public void updateEstSTD(Optional<EstimatedRobotPose> estPose, List<PhotonTracke
 		}
 	}
 
-	public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
+	public Optional<EstimatedRobotPose> getEstimatedGlobalPose(List<PhotonPipelineResult> results) {
 		Optional<EstimatedRobotPose> estimation = Optional.empty();
-		for (var r : m_camera.getAllUnreadResults()) {
+		for (var r : results) {
 			estimation = m_poseEstimator.update(r);
 			updateEstSTD(estimation, r.getTargets());