From bf4a4db8741ab2070e040e4afe6b4cb6532c4dd1 Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Tue, 14 Feb 2023 12:49:08 -0600 Subject: [PATCH] Docs corrections related to PhotonPoseEstimator (#804) --- .../java/org/photonvision/PhotonPoseEstimator.java | 12 ++++++------ .../src/main/java/frc/robot/Drivetrain.java | 3 --- .../src/main/java/frc/robot/PhotonCameraWrapper.java | 4 ++-- 3 files changed, 8 insertions(+), 11 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index b1df1eef9..b358ad73d 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -63,7 +63,7 @@ public class PhotonPoseEstimator { /** Choose the Pose which is closest to the last pose calculated */ CLOSEST_TO_LAST_POSE, - /** Choose the Pose with the lowest ambiguity. */ + /** Return the average of the best target poses using ambiguity as weight. */ AVERAGE_BEST_TARGETS, /** Use all visible tags to compute a single pose estimate.. */ @@ -89,9 +89,9 @@ public class PhotonPoseEstimator { * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/coordinate-systems.html#field-coordinate-system">Field * Coordinate System. * @param strategy The strategy it should use to determine the best pose. - * @param camera PhotonCameras and - * @param robotToCamera Transform3d from the center of the robot to the camera mount positions - * (ie, robot ➔ camera) in the Robot * Coordinate System. */ @@ -240,8 +240,8 @@ public class PhotonPoseEstimator { * Poll data from the configured cameras and update the estimated position of the robot. Returns * empty if there are no cameras set or no targets were found from the cameras. * - * @return an EstimatedRobotPose with an estimated pose, and information about the camera(s) and - * pipeline results used to create the estimate + * @return an EstimatedRobotPose with an estimated pose, the timestamp, and targets used to create + * the estimate */ public Optional update() { if (camera == null) { diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java index 1323d5108..b7e5c8899 100644 --- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java +++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java @@ -181,9 +181,6 @@ public class Drivetrain { m_poseEstimator.update( m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance()); - // Also apply vision measurements. We use 0.3 seconds in the past as an example - // -- on - // a real robot, this must be calculated based either on latency or timestamps. Optional result = pcw.getEstimatedGlobalPose(m_poseEstimator.getEstimatedPosition()); diff --git a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java index 84f52a018..9a5af0818 100644 --- a/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java +++ b/photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java @@ -79,8 +79,8 @@ public class PhotonCameraWrapper { /** * @param estimatedRobotPose The current best guess at robot pose - * @return A pair of the fused camera observations to a single Pose2d on the field, and the time - * of the observation. Assumes a planar field and the robot is always firmly on the ground + * @return an EstimatedRobotPose with an estimated pose, the timestamp, and targets used to create + * the estimate */ public Optional getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) { photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);