Incorrect pose estimation vision correction java example

[PGgit08]

In the PhotonLib pose estimation java example, old vision measurements are being ignored and only the most recent measurement from getAllUnreadResults() is being added to the WPILib pose estimator:

• getEstimatedGlobalPose() method (the issue is in this method):

  photonvision/photonlib-java-examples/poseest/src/main/java/frc/robot/Vision.java

  Lines 86 to 114 in e8efef4

  	/**
  	* The latest estimated robot pose on the field from vision data. This may be empty. This should
  	* only be called once per loop.
  	*
  	* <p>Also includes updates for the standard deviations, which can (optionally) be retrieved with
  	* {@link getEstimationStdDevs}
  	*
  	* @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets
  	* used for estimation.
  	*/
  	public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
  	Optional<EstimatedRobotPose> visionEst = Optional.empty();
  	for (var change : camera.getAllUnreadResults()) {
  	visionEst = photonEstimator.update(change);
  	updateEstimationStdDevs(visionEst, change.getTargets());
  	if (Robot.isSimulation()) {
  	visionEst.ifPresentOrElse(
  	est ->
  	getSimDebugField()
  	.getObject("VisionEstimation")
  	.setPose(est.estimatedPose.toPose2d()),
  	() -> {
  	getSimDebugField().getObject("VisionEstimation").setPoses();
  	});
  	}
  	}
  	return visionEst;
  	}

• SwerveDrivePoseEstimator vision update call using the getEstimatedGlobalPose() method:

  photonvision/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java

  Lines 64 to 73 in e8efef4

  	// Correct pose estimate with vision measurements
  	var visionEst = vision.getEstimatedGlobalPose();
  	visionEst.ifPresent(
  	est -> {
  	// Change our trust in the measurement based on the tags we can see
  	var estStdDevs = vision.getEstimationStdDevs();
  	drivetrain.addVisionMeasurement(
  	est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
  	});

[PGgit08]

i forgot to add the bug label

[mcm001]

Yeah agreed that that example looks like an anti pattern. Good catch!

[gerth2]

Yee. The example design predates getAllUnreadResults(), was just never updated.

[gerth2]

Fyi some overlap with #1632