From dee9806c48aab9e6f246ff7387690693580d073c Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu, 9 Jan 2025 00:54:51 +0200 Subject: [PATCH] T265 (#13) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Reworked `PoseEstimator6328` as `SwervePoseEstimator` * Removed `PoseEstimator6328` * Removed unused variable * Removed unnecessary records and added new ones 😁 * Moved it all to one file * The Copilot thing aint too smart * Jd * Update src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * 🤦‍♂️ * ALWAYS MAKE SURE TO UPDATE BEFORE READING!!!!!!!!!!! * Added T265 * Small cleaning * Removed command that shouldn't be here * Removed whitespace * Updated logging * Logic changes * Optimization * Moved T265 to CameraConstants * Cleaned AprilTagCamera stddevs * Update CameraConstants.java * Finally fixed logic!!!!!!!!!!!!!!! (I think) * LLL * Small clean * Swerve isn't working, but the other stuff should be * Removed stupid indent that looks like a change * Logic works!!! Swerve rotation has no hope * IT IS NOT WORKING RIGHT NOW, JUST COMMITING * Fixed half of problem * Fixed other problem * Actually fixed logic * Added letency prevention logci i have no idea what im doingggggggggggggggggggg * Updated jd * Squashed commit of the following: commit 68756eeaeb49db42821f31903f0f9c096a94447f Author: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon Dec 23 12:58:26 2024 +0200 Updated for quickorttrigonlib commit 04cc3614b62873e90f6cf38a80017b6302b3287f Author: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Mon Dec 23 09:09:39 2024 +0200 Small cleaning commit de329a9ab0edb9a88ec044ec1a2589c119a9ccec Author: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat Dec 21 18:58:55 2024 +0200 Cleaned null translation check commit e4fa9861c174327849f3dd72bfff7b9e87272efe Author: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu Dec 19 12:55:32 2024 +0200 Fixed swerve problem and odometry update problem commit b297f612113f1d511af1dd03a14abf1118791dd5 Author: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Thu Dec 19 12:50:43 2024 +0200 Cleaning * Fixed error * Use Stddevs record function * Fixed stddevs bug * Lowered tolerance * jd + small stuff * jd robotcentertocam cleaning * refactor * Added stuff -JSON * jd * Removed Pinhole * Generalized and cleaned * Json stuff * Maybe fixed json stuff? * Update RelativeRobotPoseSourceT265IO.java * Update RelativeRobotPoseSourceConstants.java * Update JSON stuff * Added replay support + fixes * 😧 * Lots * Correct JsonDump logic * Fixed bugs (updated) * nnnnnnnnnnnnn * me made fix * We don't need this * Reverted logic (conflicts suck ig) * oops * jd * Simplified * Update PoseEstimator.java * Update PoseEstimator.java * Resolve error * Made variable that can be final final * Remove unused command * Be more efficient with cameraToRobot and explain with a jdoc * Clean, optimize, and fix logic * jolbijlb * quick * oops * fps * Update photonlib.json * Update TRIGONLib * Cleaned some of the camera logic * Extra newline * Added proper getters * Fixed pose estimator logic * Removed non-existent camera * Cleaned, and added more checks --------- Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> Co-authored-by: Yishai Levy <96019039+levyishai@users.noreply.github.com> --- build.gradle | 2 +- .../java/frc/trigon/robot/RobotContainer.java | 1 - .../robot/commands/CommandConstants.java | 1 - .../robot/constants/CameraConstants.java | 3 - .../robot/constants/OperatorConstants.java | 1 - .../robot/constants/RobotConstants.java | 4 +- .../apriltagcamera/AprilTagCamera.java | 216 +++++------------- .../AprilTagCameraConstants.java | 6 +- .../apriltagcamera/AprilTagCameraIO.java | 13 +- .../io/AprilTagLimelightIO.java | 29 +-- .../io/AprilTagPhotonCameraIO.java | 82 +++---- .../poseestimator/PoseEstimator.java | 186 ++++++++++----- .../poseestimator/PoseEstimatorConstants.java | 6 +- .../poseestimator/StandardDeviations.java | 20 +- .../RelativeRobotPoseSource.java | 72 ++++++ .../RelativeRobotPoseSourceConstants.java | 7 + .../RelativeRobotPoseSourceIO.java | 29 +++ .../RelativeRobotPoseSourceSimulationIO.java | 17 ++ .../io/RelativeRobotPoseSourceT265IO.java | 67 ++++++ vendordeps/photonlib.json | 12 +- 20 files changed, 455 insertions(+), 319 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSource.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceIO.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceSimulationIO.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceT265IO.java diff --git a/build.gradle b/build.gradle index 530506de..5fd20ca8 100644 --- a/build.gradle +++ b/build.gradle @@ -100,7 +100,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:v2025.0.10' + implementation 'com.github.Programming-TRIGON:TRIGONLib:v2025.0.11' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 952cdff8..f337a085 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -60,7 +60,6 @@ private void initializeGeneralSystems() { private void bindControllerCommands() { OperatorConstants.RESET_HEADING_TRIGGER.onTrue(CommandConstants.RESET_HEADING_COMMAND); - OperatorConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER.onTrue(CommandConstants.SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND); OperatorConstants.DRIVE_FROM_DPAD_TRIGGER.whileTrue(CommandConstants.SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND); OperatorConstants.TOGGLE_ROTATION_MODE_TRIGGER.onTrue(GeneralCommands.getToggleRotationModeCommand()); OperatorConstants.TOGGLE_BRAKE_TRIGGER.onTrue(GeneralCommands.getToggleBrakeCommand()); diff --git a/src/main/java/frc/trigon/robot/commands/CommandConstants.java b/src/main/java/frc/trigon/robot/commands/CommandConstants.java index 259f9788..7a7f5017 100644 --- a/src/main/java/frc/trigon/robot/commands/CommandConstants.java +++ b/src/main/java/frc/trigon/robot/commands/CommandConstants.java @@ -46,7 +46,6 @@ public class CommandConstants { () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX()) ), RESET_HEADING_COMMAND = new InstantCommand(() -> RobotContainer.POSE_ESTIMATOR.resetPose(changeRotation(new FlippablePose2d(RobotContainer.POSE_ESTIMATOR.getCurrentEstimatedPose(), false), new Rotation2d()).get())), - SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_COMMAND = new InstantCommand(RobotContainer.POSE_ESTIMATOR::setGyroHeadingToBestSolvePNPHeading).ignoringDisable(true), SELF_RELATIVE_DRIVE_FROM_DPAD_COMMAND = SwerveCommands.getClosedLoopSelfRelativeDriveCommand( () -> getXPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), () -> getYPowerFromPov(DRIVER_CONTROLLER.getPov()) / OperatorConstants.POV_DIVIDER / calculateShiftModeValue(MINIMUM_TRANSLATION_SHIFT_POWER), diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index 6008bd56..232f430c 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -1,8 +1,5 @@ package frc.trigon.robot.constants; public class CameraConstants { - public static final double - TRANSLATIONS_STD_EXPONENT = 0.02, - THETA_STD_EXPONENT = 0.02; //TODO: implement CameraConstants } diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index 6b7ad123..f3c3b457 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -21,7 +21,6 @@ public class OperatorConstants { public static final Trigger RESET_HEADING_TRIGGER = DRIVER_CONTROLLER.y(), - SET_GYRO_HEADING_TO_SOLVE_PNP_HEADING_TRIGGER = OPERATOR_CONTROLLER.r().or(DRIVER_CONTROLLER.a()), TOGGLE_BRAKE_TRIGGER = OPERATOR_CONTROLLER.g().or(RobotController::getUserButton), TOGGLE_ROTATION_MODE_TRIGGER = DRIVER_CONTROLLER.b(), DRIVE_FROM_DPAD_TRIGGER = new Trigger(() -> DRIVER_CONTROLLER.getPov() != -1), diff --git a/src/main/java/frc/trigon/robot/constants/RobotConstants.java b/src/main/java/frc/trigon/robot/constants/RobotConstants.java index 32507541..86be644d 100644 --- a/src/main/java/frc/trigon/robot/constants/RobotConstants.java +++ b/src/main/java/frc/trigon/robot/constants/RobotConstants.java @@ -5,10 +5,10 @@ import org.trigon.utilities.FilesHandler; public class RobotConstants { - private static final RobotHardwareStats.ReplayType REPLAY_TYPE = RobotHardwareStats.ReplayType.NONE; - private static final double PERIODIC_TIME_SECONDS = 0.02; public static final String CANIVORE_NAME = "CANivore"; public static final String LOGGING_PATH = Robot.IS_REAL ? "/media/sda1/akitlogs/" : FilesHandler.DEPLOY_PATH + "logs/"; + private static final RobotHardwareStats.ReplayType REPLAY_TYPE = RobotHardwareStats.ReplayType.NONE; + private static final double PERIODIC_TIME_SECONDS = 0.02; public static void init() { RobotHardwareStats.setCurrentRobotStats(Robot.IS_REAL, REPLAY_TYPE); diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java index 60982c16..959d57fb 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -1,7 +1,6 @@ package frc.trigon.robot.poseestimation.apriltagcamera; import edu.wpi.first.math.geometry.*; -import frc.trigon.robot.RobotContainer; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.poseestimator.StandardDeviations; import org.littletonrobotics.junction.Logger; @@ -9,63 +8,50 @@ /** * An april tag camera is a class that provides the robot's pose from a camera using one or multiple apriltags. - * An april tag is like a 2D barcode used to find the robot's position on the field. + * An april tag is like a 2d QR-code used to find the robot's position on the field. * Since the tag's position on the field is known, we can calculate our position relative to it, therefore estimating our position on the field. */ public class AprilTagCamera { protected final String name; private final AprilTagCameraInputsAutoLogged inputs = new AprilTagCameraInputsAutoLogged(); - private final Transform3d robotCenterToCamera; - private final double - thetaStandardDeviationExponent, - translationStandardDeviationExponent; + private final Transform2d cameraToRobotCenter; + private final StandardDeviations standardDeviations; private final AprilTagCameraIO aprilTagCameraIO; - private double lastUpdatedTimestamp; - private Pose2d robotPose = null; + private Pose2d estimatedRobotPose = null; /** * Constructs a new AprilTagCamera. * - * @param aprilTagCameraType the type of camera - * @param name the camera's name - * @param robotCenterToCamera the transform of the robot's origin point to the camera - * @param thetaStandardDeviationExponent a calibrated gain for the standard theta deviations of the estimated robot pose - * @param translationStandardDeviationExponent a calibrated gain for the standard translation deviations of the estimated robot pose + * @param aprilTagCameraType the type of camera + * @param name the camera's name + * @param robotCenterToCamera the transform of the robot's origin point to the camera. + * only the x, y and yaw values will be used for transforming the camera pose to the robot's center, + * to avoid more inaccuracies like pitch and roll. + * The reset will be used for creating a camera in simulation + * @param standardDeviations the initial calibrated standard deviations for the camera's estimated pose, + * will be changed as the distance from the tag(s) changes and the number of tags changes */ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name, Transform3d robotCenterToCamera, - double thetaStandardDeviationExponent, - double translationStandardDeviationExponent) { + StandardDeviations standardDeviations) { this.name = name; - this.robotCenterToCamera = robotCenterToCamera; - this.thetaStandardDeviationExponent = thetaStandardDeviationExponent; - this.translationStandardDeviationExponent = translationStandardDeviationExponent; + this.standardDeviations = standardDeviations; + this.cameraToRobotCenter = toTransform2d(robotCenterToCamera).inverse(); - if (RobotHardwareStats.isSimulation()) { - aprilTagCameraIO = AprilTagCameraConstants.AprilTagCameraType.SIMULATION_CAMERA.createIOFunction.apply(name); + aprilTagCameraIO = AprilTagCameraIO.generateIO(aprilTagCameraType, name); + if (RobotHardwareStats.isSimulation()) aprilTagCameraIO.addSimulatedCameraToVisionSimulation(robotCenterToCamera); - return; - } - aprilTagCameraIO = aprilTagCameraType.createIOFunction.apply(name); } public void update() { aprilTagCameraIO.updateInputs(inputs); - robotPose = calculateBestRobotPose(); + estimatedRobotPose = calculateRobotPose(); logCameraInfo(); } - public boolean hasNewResult() { - return (inputs.hasResult && inputs.distanceFromBestTag != Double.POSITIVE_INFINITY) && isNewTimestamp(); - } - public Pose2d getEstimatedRobotPose() { - return robotPose; - } - - public Rotation2d getSolvePNPHeading() { - return inputs.cameraSolvePNPPose.getRotation().toRotation2d().minus(robotCenterToCamera.getRotation().toRotation2d()); + return estimatedRobotPose; } public String getName() { @@ -76,151 +62,76 @@ public double getLatestResultTimestampSeconds() { return inputs.latestResultTimestampSeconds; } - /** - * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calibrated gain. - * The theta deviation is infinity when we assume the robot's heading because we already assume that the heading is correct. - * - * @return the standard deviations for the pose estimation strategy used - */ - public StandardDeviations calculateStandardDeviations() { - final double translationStandardDeviation = calculateStandardDeviations(translationStandardDeviationExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length); - final double thetaStandardDeviation = isWithinBestTagRangeForSolvePNP() ? calculateStandardDeviations(thetaStandardDeviationExponent, inputs.distanceFromBestTag, inputs.visibleTagIDs.length) : Double.POSITIVE_INFINITY; - - return new StandardDeviations(translationStandardDeviation, thetaStandardDeviation); - } - - public double getDistanceToBestTagMeters() { - return inputs.distanceFromBestTag; - } - - /** - * If the robot is close enough to the tag, it calculates the pose using the solve PNP heading. - * If it's too far, it assumes the robot's heading from the gyro and calculates its position from there. - * Assuming the robot's heading from the gyro is more robust, but won't fix current wrong heading. - * To fix this, we use solve PNP to reset the gyro when we are close enough for an accurate result. - * - * @return the robot's pose - */ - private Pose2d calculateBestRobotPose() { - final Rotation2d gyroHeadingAtTimestamp = RobotHardwareStats.isSimulation() ? RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose().getRotation() : RobotContainer.POSE_ESTIMATOR.getPoseAtTimestamp(inputs.latestResultTimestampSeconds).getRotation(); - return calculateAssumedRobotHeadingPose(gyroHeadingAtTimestamp); + public boolean hasValidResult() { + return inputs.hasResult && inputs.poseAmbiguity < AprilTagCameraConstants.MAXIMUM_AMBIGUITY && estimatedRobotPose != null; } /** - * Calculates the robot's pose by assuming its heading, the apriltag's position, and the camera's position on the robot. - * This method of pose estimation is more robust than solve PNP, but relies on knowing the robot's heading beforehand. + * Calculates the range of how inaccurate the estimated pose could be using the distance from the target, the number of targets, and a calibrated gain. * - * @return the robot's pose + * @return the standard deviations of the current estimated pose */ - private Pose2d calculateAssumedRobotHeadingPose(Rotation2d gyroHeading) { - if (inputs.visibleTagIDs.length == 0 || !inputs.hasResult || inputs.poseAmbiguity > AprilTagCameraConstants.MAXIMUM_AMBIGUITY) - return null; - - final Rotation2d fieldRelativeRobotHeading = isWithinBestTagRangeForSolvePNP() ? getSolvePNPHeading() : gyroHeading; - final Translation2d fieldRelativeRobotTranslation = getFieldRelativeRobotTranslation(fieldRelativeRobotHeading); - - if (fieldRelativeRobotTranslation == null) - return null; - return new Pose2d(fieldRelativeRobotTranslation, fieldRelativeRobotHeading); - } - - private Translation2d getFieldRelativeRobotTranslation(Rotation2d currentHeading) { - final Pose3d bestTagPose = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]); - if (bestTagPose == null) - return null; - - correctTargetRelativeRotation(); + public StandardDeviations calculateStandardDeviations() { + final double averageDistanceFromTags = calculateAverageDistanceFromTags(); + final double translationStandardDeviation = calculateStandardDeviation(standardDeviations.getThetaStandardDeviation(), averageDistanceFromTags, inputs.visibleTagIDs.length); + final double thetaStandardDeviation = calculateStandardDeviation(standardDeviations.getThetaStandardDeviation(), averageDistanceFromTags, inputs.visibleTagIDs.length); - final Translation2d tagRelativeCameraTranslation = calculateTagRelativeCameraTranslation(currentHeading, bestTagPose); - final Translation2d fieldRelativeRobotPose = getFieldRelativeRobotPose(tagRelativeCameraTranslation, bestTagPose); - final Translation2d fieldRelativeCameraToRobotTranslation = robotCenterToCamera.getTranslation().toTranslation2d().rotateBy(currentHeading); - return fieldRelativeRobotPose.minus(fieldRelativeCameraToRobotTranslation); - } + Logger.recordOutput("StandardDeviations/" + name + "/translations", translationStandardDeviation); + Logger.recordOutput("StandardDeviations/" + name + "/theta", thetaStandardDeviation); - /** - * When the roll of the camera changes, the target pitch and yaw are also affected. - * This method corrects the yaw and pitch based on the camera's mount position roll. - */ - private void correctTargetRelativeRotation() { - final Translation2d targetRotation = new Translation2d(inputs.bestTargetRelativePitchRadians, inputs.bestTargetRelativeYawRadians); - targetRotation.rotateBy(Rotation2d.fromRadians(robotCenterToCamera.getRotation().getX())); - inputs.bestTargetRelativePitchRadians = targetRotation.getX(); - inputs.bestTargetRelativeYawRadians = targetRotation.getY(); - } - - private Translation2d calculateTagRelativeCameraTranslation(Rotation2d gyroHeading, Pose3d tagPose) { - final double robotPlaneTargetYawRadians = getRobotPlaneTargetYawRadians(); - final double robotPlaneCameraDistanceToUsedTagMeters = calculateRobotPlaneXYDistanceToTag(tagPose, robotPlaneTargetYawRadians); - final double headingOffsetToUsedTagRadians = gyroHeading.getRadians() - robotPlaneTargetYawRadians + robotCenterToCamera.getRotation().getZ(); - return new Translation2d(robotPlaneCameraDistanceToUsedTagMeters, Rotation2d.fromRadians(headingOffsetToUsedTagRadians)); + return new StandardDeviations(translationStandardDeviation, thetaStandardDeviation); } /** - * Gets the target yaw relative to the robot from the target yaw relative to the camera and the camera's mount position. + * Solve PNP is inaccurate the further the camera is from the tag. + * Because of this, there are some things we might want to do only if we are close enough to get an accurate enough result. + * This method checks if the current distance from the tag is less than the maximum distance for an accurate result, which is defined as the variable {@link AprilTagCameraConstants#MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_SOLVE_PNP_RESULT_METERS}. * - * @return the target yaw in radians + * @return if the camera is close enough to the tag to get an accurate result from solve PNP */ - private double getRobotPlaneTargetYawRadians() { - double targetYawRadians = -inputs.bestTargetRelativeYawRadians; - for (int i = 0; i < AprilTagCameraConstants.CALCULATE_YAW_ITERATIONS; i++) { - final double projectedPitch = projectToPlane(-robotCenterToCamera.getRotation().getY(), targetYawRadians + Math.PI / 2); - targetYawRadians = -inputs.bestTargetRelativeYawRadians - Math.tan(projectedPitch) * -inputs.bestTargetRelativePitchRadians; - } - return projectToPlane(targetYawRadians, robotCenterToCamera.getRotation().getY()); - } - - private double projectToPlane(double targetAngleRadians, double cameraAngleRadians) { - if (cameraAngleRadians < 0) - return Math.atan(Math.tan(targetAngleRadians) / Math.cos(cameraAngleRadians)); - return Math.atan(Math.tan(targetAngleRadians) * Math.cos(cameraAngleRadians)); - } - - private double calculateRobotPlaneXYDistanceToTag(Pose3d usedTagPose, double robotPlaneTargetYaw) { - final double zDistanceToUsedTagMeters = Math.abs(usedTagPose.getZ() - robotCenterToCamera.getTranslation().getZ()); - final double robotPlaneDistanceFromUsedTagMeters = zDistanceToUsedTagMeters / Math.tan(-robotCenterToCamera.getRotation().getY() - inputs.bestTargetRelativePitchRadians); - return robotPlaneDistanceFromUsedTagMeters / Math.cos(robotPlaneTargetYaw); - } - - private Translation2d getFieldRelativeRobotPose(Translation2d tagRelativeCameraTranslation, Pose3d tagPose) { - return tagPose.getTranslation().toTranslation2d().minus(tagRelativeCameraTranslation); - } - - private boolean isNewTimestamp() { - if (lastUpdatedTimestamp == getLatestResultTimestampSeconds()) - return false; - - lastUpdatedTimestamp = getLatestResultTimestampSeconds(); - return true; + public boolean isWithinBestTagRangeForAccurateSolvePNPResult() { + return hasValidResult() && inputs.distancesFromTags[0] < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_SOLVE_PNP_RESULT_METERS; } /** - * Calculates the standard deviation of the estimated pose using a formula. + * Calculates an aspect of the standard deviations of the estimated pose using a formula. * As we get further from the tag(s), this will return a less trusting (higher deviation) result. * - * @param exponent a calibrated gain, different for each pose estimating strategy + * @param exponent a calibrated gain * @param distance the distance from the tag(s) * @param numberOfVisibleTags the number of visible tags * @return the standard deviation */ - private double calculateStandardDeviations(double exponent, double distance, int numberOfVisibleTags) { + private double calculateStandardDeviation(double exponent, double distance, int numberOfVisibleTags) { return exponent * (distance * distance) / numberOfVisibleTags; } - private boolean isWithinBestTagRangeForSolvePNP() { - return inputs.distanceFromBestTag < AprilTagCameraConstants.MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS; + private Pose2d calculateRobotPose() { + return cameraPoseToRobotPose(inputs.cameraSolvePNPPose.toPose2d()); + } + + private Pose2d cameraPoseToRobotPose(Pose2d cameraPose) { + return cameraPose.transformBy(cameraToRobotCenter); + } + + private double calculateAverageDistanceFromTags() { + double totalDistance = 0; + for (int visibleTagID : inputs.visibleTagIDs) + totalDistance += FieldConstants.TAG_ID_TO_POSE.get(visibleTagID).getTranslation().getDistance(inputs.cameraSolvePNPPose.getTranslation()); + return totalDistance / inputs.visibleTagIDs.length; } private void logCameraInfo() { Logger.processInputs("Cameras/" + name, inputs); if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) logUsedTags(); - if (!inputs.hasResult || inputs.distanceFromBestTag == Double.POSITIVE_INFINITY || robotPose == null) { - logEstimatedRobotPose(); - logSolvePNPPose(); - } else { - Logger.recordOutput("Poses/Robot/" + name + "/Pose", AprilTagCameraConstants.EMPTY_POSE_LIST); - Logger.recordOutput("Poses/Robot/" + name + "/SolvePNPPose", AprilTagCameraConstants.EMPTY_POSE_LIST); + + if (hasValidResult()) { + Logger.recordOutput("Poses/Robot/" + name + "/Pose", estimatedRobotPose); + return; } + + Logger.recordOutput("Poses/Robot/" + name + "/Pose", AprilTagCameraConstants.EMPTY_POSE_LIST); } private void logUsedTags() { @@ -229,17 +140,16 @@ private void logUsedTags() { return; } - final Pose3d[] usedTagPoses = isWithinBestTagRangeForSolvePNP() ? new Pose3d[inputs.visibleTagIDs.length] : new Pose3d[1]; + final Pose3d[] usedTagPoses = isWithinBestTagRangeForAccurateSolvePNPResult() ? new Pose3d[inputs.visibleTagIDs.length] : new Pose3d[1]; for (int i = 0; i < usedTagPoses.length; i++) usedTagPoses[i] = FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[i]); Logger.recordOutput("UsedTags/" + this.getName(), usedTagPoses); } - private void logEstimatedRobotPose() { - Logger.recordOutput("Poses/Robot/" + name + "/Pose", robotPose); - } + private Transform2d toTransform2d(Transform3d transform3d) { + final Translation2d robotCenterToCameraTranslation = transform3d.getTranslation().toTranslation2d(); + final Rotation2d robotCenterToCameraRotation = transform3d.getRotation().toRotation2d(); - private void logSolvePNPPose() { - Logger.recordOutput("Poses/Robot/" + name + "/SolvePNPPose", inputs.cameraSolvePNPPose.plus(robotCenterToCamera.inverse())); + return new Transform2d(robotCenterToCameraTranslation, robotCenterToCameraRotation); } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java index 960a6ee0..b862100a 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -13,11 +13,9 @@ import java.util.function.Function; public class AprilTagCameraConstants { - static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_SOLVE_PNP_METERS = 2; - static final int CALCULATE_YAW_ITERATIONS = 3; + static final double MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_SOLVE_PNP_RESULT_METERS = 2; static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; - - public static final double MAXIMUM_AMBIGUITY = 0.4; + static final double MAXIMUM_AMBIGUITY = 0.4; public static final VisionSystemSim VISION_SIMULATION = new VisionSystemSim("VisionSimulation"); private static final int diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java index 5ad54f5b..6341e2d2 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -3,8 +3,17 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform3d; import org.littletonrobotics.junction.AutoLog; +import org.trigon.hardware.RobotHardwareStats; public class AprilTagCameraIO { + static AprilTagCameraIO generateIO(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraType, String name) { + if (RobotHardwareStats.isReplay()) + return new AprilTagCameraIO(); + if (RobotHardwareStats.isSimulation()) + return AprilTagCameraConstants.AprilTagCameraType.SIMULATION_CAMERA.createIOFunction.apply(name); + return aprilTagCameraType.createIOFunction.apply(name); + } + protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { } @@ -22,9 +31,7 @@ public static class AprilTagCameraInputs { public double latestResultTimestampSeconds = 0; public Pose3d cameraSolvePNPPose = new Pose3d(); public int[] visibleTagIDs = new int[0]; - public double bestTargetRelativeYawRadians = 0; - public double bestTargetRelativePitchRadians = 0; - public double distanceFromBestTag = 0; public double poseAmbiguity = 0; + public double[] distancesFromTags = new double[0]; } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java index 1f10f356..c0d859b4 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -1,13 +1,11 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.util.Units; -import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; import org.trigon.utilities.LimelightHelpers; +// TODO: Fully implement this class, Limelight currently not supported. public class AprilTagLimelightIO extends AprilTagCameraIO { private final String hostname; @@ -29,20 +27,14 @@ protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { } private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, LimelightHelpers.LimelightResults results) { - final Rotation3d bestTagRelativeRotation = getBestTargetRelativeRotation(results); - inputs.cameraSolvePNPPose = results.getBotPose3d_wpiBlue(); inputs.latestResultTimestampSeconds = results.timestamp_RIOFPGA_capture; inputs.visibleTagIDs = getVisibleTagIDs(results); - inputs.bestTargetRelativeYawRadians = bestTagRelativeRotation.getZ(); - inputs.bestTargetRelativePitchRadians = bestTagRelativeRotation.getY(); - inputs.distanceFromBestTag = getDistanceFromBestTag(results); } private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { inputs.cameraSolvePNPPose = new Pose3d(); inputs.visibleTagIDs = new int[0]; - inputs.distanceFromBestTag = Double.POSITIVE_INFINITY; } private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) { @@ -64,25 +56,6 @@ private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) { return visibleTagIDs; } - /** - * Estimates the camera's rotation relative to the apriltag. - * - * @param results the camera's pipeline result - * @return the estimated rotation - */ - private Rotation3d getBestTargetRelativeRotation(LimelightHelpers.LimelightResults results) { - final LimelightHelpers.LimelightTarget_Fiducial bestTag = getBestTarget(results); - return new Rotation3d(0, -Units.degreesToRadians(bestTag.tx), -Units.degreesToRadians(bestTag.ty)); - } - - private double getDistanceFromBestTag(LimelightHelpers.LimelightResults results) { - return getDistanceFromTag((int) getBestTarget(results).fiducialID, results.getBotPose3d_wpiBlue()); - } - - private double getDistanceFromTag(int fiducialID, Pose3d estimatedRobotPose) { - return FieldConstants.TAG_ID_TO_POSE.get(fiducialID).getTranslation().getDistance(estimatedRobotPose.getTranslation()); - } - private LimelightHelpers.LimelightTarget_Fiducial getBestTarget(LimelightHelpers.LimelightResults results) { return results.targets_Fiducials[0]; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java index ea9a39f6..eba04b9b 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -1,9 +1,7 @@ package frc.trigon.robot.poseestimation.apriltagcamera.io; 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.util.Units; import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraInputsAutoLogged; @@ -24,11 +22,12 @@ public AprilTagPhotonCameraIO(String cameraName) { protected void updateInputs(AprilTagCameraInputsAutoLogged inputs) { final PhotonPipelineResult latestResult = getLatestPipelineResult(); - inputs.hasResult = latestResult != null && latestResult.hasTargets() && !latestResult.getTargets().isEmpty(); + inputs.hasResult = latestResult != null && latestResult.hasTargets(); if (inputs.hasResult) { updateHasResultInputs(inputs, latestResult); return; } + updateNoResultInputs(inputs); } @@ -38,48 +37,16 @@ private PhotonPipelineResult getLatestPipelineResult() { } private void updateHasResultInputs(AprilTagCameraInputsAutoLogged inputs, PhotonPipelineResult latestResult) { - final PhotonTrackedTarget bestTarget = getBestTarget(latestResult); - final Rotation3d bestTargetRelativeRotation3d = getBestTargetRelativeRotation(bestTarget); - - inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult, bestTarget); + inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult); inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); - inputs.bestTargetRelativePitchRadians = bestTargetRelativeRotation3d.getY(); - inputs.bestTargetRelativeYawRadians = bestTargetRelativeRotation3d.getZ(); - inputs.visibleTagIDs = getVisibleTagIDs(latestResult, bestTarget); - inputs.distanceFromBestTag = getDistanceFromBestTag(bestTarget); - inputs.poseAmbiguity = bestTarget.getPoseAmbiguity(); + inputs.visibleTagIDs = getVisibleTagIDs(latestResult); + inputs.poseAmbiguity = latestResult.getMultiTagResult().isPresent() ? 0 : latestResult.getBestTarget().getPoseAmbiguity(); + inputs.distancesFromTags = getDistancesFromTags(latestResult); } private void updateNoResultInputs(AprilTagCameraInputsAutoLogged inputs) { inputs.cameraSolvePNPPose = new Pose3d(); inputs.visibleTagIDs = new int[0]; - inputs.distanceFromBestTag = Double.POSITIVE_INFINITY; - } - - /** - * Calculates the best tag from the pipeline result based on the area that the tag takes up. - * - * @param result the camera's pipeline result - * @return the best target - */ - private PhotonTrackedTarget getBestTarget(PhotonPipelineResult result) { - PhotonTrackedTarget bestTarget = result.getBestTarget(); - for (PhotonTrackedTarget target : result.getTargets()) { - if (target.getArea() > bestTarget.area) - bestTarget = target; - } - return bestTarget; - } - - /** - * Estimates the camera's rotation relative to the apriltag. - * - * @param bestTag the best apriltag visible to the camera - * @return the estimated rotation - */ - private Rotation3d getBestTargetRelativeRotation(PhotonTrackedTarget bestTag) { - final Rotation3d cameraRotation = bestTag.getBestCameraToTarget().getRotation(); - return new Rotation3d(0, Units.degreesToRadians(cameraRotation.getY()), Units.degreesToRadians(cameraRotation.getZ())); } /** @@ -88,35 +55,38 @@ private Rotation3d getBestTargetRelativeRotation(PhotonTrackedTarget bestTag) { * @param result the camera's pipeline result * @return the estimated pose */ - private Pose3d getSolvePNPPose(PhotonPipelineResult result, PhotonTrackedTarget bestTarget) { + private Pose3d getSolvePNPPose(PhotonPipelineResult result) { if (result.getMultiTagResult().isPresent()) { final Transform3d cameraPoseTransform = result.getMultiTagResult().get().estimatedPose.best; return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); } - final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(bestTarget.getFiducialId()); - final Transform3d targetToCamera = bestTarget.getBestCameraToTarget().inverse(); + final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(result.getBestTarget().getFiducialId()); + final Transform3d targetToCamera = result.getBestTarget().getBestCameraToTarget().inverse(); return tagPose.transformBy(targetToCamera); } - private int[] getVisibleTagIDs(PhotonPipelineResult result, PhotonTrackedTarget bestTarget) { + private int[] getVisibleTagIDs(PhotonPipelineResult result) { final List targets = result.getTargets(); final int[] visibleTagIDs = new int[targets.size()]; - visibleTagIDs[0] = bestTarget.getFiducialId(); - - boolean hasSeenBestTarget = false; - for (int i = 0; i < visibleTagIDs.length; i++) { - final int targetID = targets.get(i).getFiducialId(); - if (targetID == visibleTagIDs[0]) { - hasSeenBestTarget = true; - continue; - } - visibleTagIDs[hasSeenBestTarget ? i : i + 1] = targetID; - } + + for (int i = 0; i < targets.size(); i++) + visibleTagIDs[i] = targets.get(i).getFiducialId(); + return visibleTagIDs; } - private double getDistanceFromBestTag(PhotonTrackedTarget bestTag) { - return bestTag.getBestCameraToTarget().getTranslation().getNorm(); + private double[] getDistancesFromTags(PhotonPipelineResult result) { + final List targets = result.getTargets(); + final double[] distances = new double[targets.size()]; + + for (int i = 0; i < targets.size(); i++) + distances[i] = getDistanceFromTarget(targets.get(i)); + + return distances; + } + + private double getDistanceFromTarget(PhotonTrackedTarget target) { + return target.getBestCameraToTarget().getTranslation().getNorm(); } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java index c0d36cf6..2e7ae33e 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -3,6 +3,7 @@ import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -10,6 +11,8 @@ import frc.trigon.robot.constants.FieldConstants; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSource; +import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceConstants; import frc.trigon.robot.subsystems.swerve.SwerveConstants; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -18,6 +21,7 @@ import java.util.Arrays; import java.util.Map; +import java.util.NoSuchElementException; import java.util.Optional; /** @@ -27,6 +31,8 @@ public class PoseEstimator implements AutoCloseable { private final Field2d field = new Field2d(); private final AprilTagCamera[] aprilTagCameras; private final TimeInterpolatableBuffer previousOdometryPoses = TimeInterpolatableBuffer.createBuffer(PoseEstimatorConstants.POSE_BUFFER_SIZE_SECONDS); + private final RelativeRobotPoseSource relativeRobotPoseSource; + private final boolean shouldUseRelativeRobotPoseSource; private Pose2d odometryPose = new Pose2d(), @@ -39,17 +45,33 @@ public class PoseEstimator implements AutoCloseable { }; private Rotation2d lastGyroHeading = new Rotation2d(); + /** + * Constructs a new PoseEstimator and sets the relativeRobotPoseSource. + * This constructor enables usage of a relative robot pose source and disables the use of april tags for pose estimation, and instead uses them to reset the relative robot pose source's offset. + * + * @param relativeRobotPoseSource the relative robot pose source that should be used to update the pose estimator + * @param aprilTagCameras the apriltag cameras that should be used to update the relative robot pose source + */ + public PoseEstimator(RelativeRobotPoseSource relativeRobotPoseSource, AprilTagCamera... aprilTagCameras) { + this.aprilTagCameras = aprilTagCameras; + this.relativeRobotPoseSource = relativeRobotPoseSource; + this.shouldUseRelativeRobotPoseSource = true; + + initialize(); + } + /** * Constructs a new PoseEstimator. + * This constructor disables the use of a relative robot pose source and instead uses april tags cameras for pose estimation. * - * @param aprilTagCameras the cameras that should be used to update the pose estimator. + * @param aprilTagCameras the cameras that should be used to update the pose estimator */ public PoseEstimator(AprilTagCamera... aprilTagCameras) { this.aprilTagCameras = aprilTagCameras; - putAprilTagsOnFieldWidget(); - SmartDashboard.putData("Field", field); + this.relativeRobotPoseSource = null; + this.shouldUseRelativeRobotPoseSource = false; - logTargetPath(); + initialize(); } @Override @@ -58,7 +80,11 @@ public void close() { } public void periodic() { - updateFromVision(); + if (shouldUseRelativeRobotPoseSource) + updateFromRelativeRobotPoseSource(); + else + updateFromAprilTagCameras(); + field.setRobotPose(getCurrentEstimatedPose()); if (RobotHardwareStats.isSimulation()) AprilTagCameraConstants.VISION_SIMULATION.update(RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose()); @@ -75,6 +101,8 @@ public void resetPose(Pose2d newPose) { estimatedPose = newPose; lastGyroHeading = newPose.getRotation(); previousOdometryPoses.clear(); + if (shouldUseRelativeRobotPoseSource) + relativeRobotPoseSource.resetOffset(newPose); } /** @@ -106,32 +134,21 @@ public void updatePoseEstimatorStates(SwerveModulePosition[][] swerveWheelPositi addOdometryObservation(swerveWheelPositions[i], gyroRotations[i], timestamps[i]); } - public void setGyroHeadingToBestSolvePNPHeading() { - if (aprilTagCameras.length == 0) - return; - int closestCameraToTag = 0; - for (int i = 0; i < aprilTagCameras.length; i++) { - if (aprilTagCameras[i].getDistanceToBestTagMeters() < aprilTagCameras[closestCameraToTag].getDistanceToBestTagMeters()) - closestCameraToTag = i; - } - - final Rotation2d bestRobotHeading = aprilTagCameras[closestCameraToTag].getSolvePNPHeading(); - resetPose(new Pose2d(getCurrentEstimatedPose().getTranslation(), bestRobotHeading)); - } - /** * Gets the estimated pose of the robot at the target timestamp. * * @param timestamp the target timestamp * @return the robot's estimated pose at the timestamp */ - public Pose2d getPoseAtTimestamp(double timestamp) { - final Optional odometryPoseAtTimestamp = previousOdometryPoses.getSample(timestamp); - if (odometryPoseAtTimestamp.isEmpty()) - return null; + public Pose2d getEstimatedPoseAtTimestamp(double timestamp) { + final Pose2d odometryPoseAtTimestamp = getOdometryPoseAtTimestamp(timestamp); + return calculateEstimatedPoseAtTimestamp(odometryPoseAtTimestamp); + } - final Transform2d currentOdometryPoseToOdometryPoseAtTimestampTransform = new Transform2d(odometryPose, odometryPoseAtTimestamp.get()); - return estimatedPose.plus(currentOdometryPoseToOdometryPoseAtTimestampTransform); + private void initialize() { + putAprilTagsOnFieldWidget(); + SmartDashboard.putData("Field", field); + logTargetPath(); } private void putAprilTagsOnFieldWidget() { @@ -153,7 +170,7 @@ private void logTargetPath() { } /** - * Sets the estimated pose from the odometry at the given timestamp. + * Sets the estimated robot pose from the odometry at the given timestamp. * * @param swerveModulePositions the positions of each swerve module * @param gyroHeading the heading of the gyro @@ -166,23 +183,56 @@ private void addOdometryObservation(SwerveModulePosition[] swerveModulePositions updateOdometryPositions(swerveModulePositions, gyroHeading); } - /** - * Updates the estimated pose from the cameras. - */ - private void updateFromVision() { - final AprilTagCamera[] newResultCameras = getCamerasWithNewResult(); + private void updateFromRelativeRobotPoseSource() { + for (AprilTagCamera aprilTagCamera : aprilTagCameras) { + aprilTagCamera.update(); + + if (aprilTagCamera.isWithinBestTagRangeForAccurateSolvePNPResult() && isUnderMaximumSpeedForOffsetResetting()) + relativeRobotPoseSource.resetOffset(aprilTagCamera.getEstimatedRobotPose()); + } + + relativeRobotPoseSource.update(); + + if (relativeRobotPoseSource.hasNewResult()) + addPoseSourceObservation( + relativeRobotPoseSource.getEstimatedRobotPose(), + relativeRobotPoseSource.getLatestResultTimestampSeconds(), + RelativeRobotPoseSourceConstants.STANDARD_DEVIATIONS + ); + } + + private void updateFromAprilTagCameras() { + final AprilTagCamera[] newResultCameras = getCamerasWithResults(); sortCamerasByLatestResultTimestamp(newResultCameras); - updateEstimatedPoseFromVision(newResultCameras); + for (AprilTagCamera aprilTagCamera : newResultCameras) + addPoseSourceObservation( + aprilTagCamera.getEstimatedRobotPose(), + aprilTagCamera.getLatestResultTimestampSeconds(), + aprilTagCamera.calculateStandardDeviations() + ); + } + + /** + * Checks if the current velocity of the slow enough to get an accurate enough result to reset the offset of the {@link RelativeRobotPoseSource}. + * + * @return if the robot is moving slow enough to calculate an accurate offset result. + */ + private boolean isUnderMaximumSpeedForOffsetResetting() { + final ChassisSpeeds chassisSpeeds = RobotContainer.SWERVE.getSelfRelativeVelocity(); + final double currentTranslationVelocityMetersPerSecond = Math.hypot(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond); + final double currentThetaVelocityRadiansPerSecond = chassisSpeeds.omegaRadiansPerSecond; + return currentTranslationVelocityMetersPerSecond <= PoseEstimatorConstants.MAXIMUM_TRANSLATION_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_METERS_PER_SECOND && + currentThetaVelocityRadiansPerSecond <= PoseEstimatorConstants.MAXIMUM_THETA_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_RADIANS_PER_SECOND; } - private AprilTagCamera[] getCamerasWithNewResult() { + private AprilTagCamera[] getCamerasWithResults() { final AprilTagCamera[] camerasWithNewResult = new AprilTagCamera[aprilTagCameras.length]; int index = 0; for (AprilTagCamera aprilTagCamera : aprilTagCameras) { aprilTagCamera.update(); - if (aprilTagCamera.hasNewResult()) { + if (aprilTagCamera.hasValidResult()) { camerasWithNewResult[index] = aprilTagCamera; index++; } @@ -196,49 +246,75 @@ private void sortCamerasByLatestResultTimestamp(AprilTagCamera[] aprilTagCameras } /** - * Updates the estimated pose from each camera, each at the timestamp of their latest result. + * Sets the estimated pose from a pose source at the given timestamp. + * + * @param observationPose the estimated robot pose + * @param timestamp the timestamp of the observation */ - private void updateEstimatedPoseFromVision(AprilTagCamera[] aprilTagCameras) { - for (AprilTagCamera aprilTagCamera : aprilTagCameras) { - addVisionObservation( - aprilTagCamera.getEstimatedRobotPose(), - aprilTagCamera.calculateStandardDeviations(), - aprilTagCamera.getLatestResultTimestampSeconds() - ); - } + private void addPoseSourceObservation(Pose2d observationPose, double timestamp, StandardDeviations standardDeviations) { + final Pose2d odometryPoseAtTimestamp = getOdometryPoseAtTimestamp(timestamp); + final Pose2d estimatedPoseAtObservationTime = calculateEstimatedPoseAtTimestamp(odometryPoseAtTimestamp); + if (estimatedPoseAtObservationTime == null) + return; + + final Pose2d estimatedPoseAtTimestampWithAmbiguityCompensation = calculateEstimatedPoseAtTimestampWithAmbiguityCompensation(estimatedPoseAtObservationTime, observationPose, standardDeviations); + final Transform2d odometryPoseAtTimestampToCurrentOdometryPose = new Transform2d(odometryPoseAtTimestamp, odometryPose); + this.estimatedPose = estimatedPoseAtTimestampWithAmbiguityCompensation.plus(odometryPoseAtTimestampToCurrentOdometryPose); + } + + private Pose2d getOdometryPoseAtTimestamp(double timestamp) { + if (isTimestampOutOfHeldPosesRange(timestamp)) + return null; + + final Optional odometryPoseAtTimestamp = previousOdometryPoses.getSample(timestamp); + return odometryPoseAtTimestamp.orElse(null); } - private void addVisionObservation(Pose2d observationPose, StandardDeviations standardDeviations, double timestamp) { - final Pose2d estimatedPoseAtObservationTime = getPoseAtTimestamp(timestamp); - if (estimatedPoseAtObservationTime != null) - this.estimatedPose = calculateEstimatedPoseWithAmbiguityCompensation(estimatedPoseAtObservationTime, observationPose, standardDeviations); + private Pose2d calculateEstimatedPoseAtTimestamp(Pose2d odometryPoseAtTimestamp) { + if (odometryPoseAtTimestamp == null) + return null; + + final Transform2d currentPoseToSamplePose = new Transform2d(odometryPose, odometryPoseAtTimestamp); + return estimatedPose.plus(currentPoseToSamplePose); + } + + private boolean isTimestampOutOfHeldPosesRange(double timestamp) { + try { + final double oldestEstimatedRobotPoseTimestamp = previousOdometryPoses.getInternalBuffer().lastKey() - PoseEstimatorConstants.POSE_BUFFER_SIZE_SECONDS; + if (oldestEstimatedRobotPoseTimestamp > timestamp) + return true; + } catch (NoSuchElementException e) { + return true; + } + return false; } /** * Calculates the estimated pose of a vision observation with compensation for its ambiguity. * This is done by finding the difference between the estimated pose at the time of the observation and the estimated pose of the observation and scaling that down using the calibrated standard deviations. + * This will calculate for the pose at timestamp of `estimatedPoseAtObservationTime`. * * @param estimatedPoseAtObservationTime the estimated pose of the robot at the time of the observation * @param observationPose the estimated robot pose from the observation * @param observationStandardDeviations the ambiguity of the observation * @return the estimated pose with compensation for its ambiguity */ - private Pose2d calculateEstimatedPoseWithAmbiguityCompensation(Pose2d estimatedPoseAtObservationTime, Pose2d observationPose, StandardDeviations observationStandardDeviations) { - final Transform2d estimatedPoseAtObservationTimeToObservationPose = new Transform2d(estimatedPoseAtObservationTime, observationPose); - final Transform2d allowedMovement = calculateAllowedMovementFromAmbiguity(estimatedPoseAtObservationTimeToObservationPose, observationStandardDeviations); - return observationPose.plus(allowedMovement); + private Pose2d calculateEstimatedPoseAtTimestampWithAmbiguityCompensation(Pose2d estimatedPoseAtObservationTime, Pose2d observationPose, StandardDeviations observationStandardDeviations) { + final Transform2d observationDifference = new Transform2d(estimatedPoseAtObservationTime, observationPose); + final Transform2d allowedMovement = calculateAllowedMovementFromAmbiguity(observationDifference, observationStandardDeviations); + return estimatedPoseAtObservationTime.plus(allowedMovement); } /** * Calculates the scaling needed to reduce noise in the estimated pose from the standard deviations of the observation. * - * @param estimatedPoseAtObservationTimeToObservationPose the difference between the estimated pose of the robot at the time of the observation and the estimated pose of the observation - * @param cameraStandardDeviations the standard deviations of the camera's estimated pose + * @param observationDifference the difference between the estimated pose of the robot at the time of the observation and the estimated pose of the observation + * @param cameraStandardDeviations the standard deviations of the camera's estimated pose * @return the allowed movement of the estimated pose as a {@link Transform2d} */ - private Transform2d calculateAllowedMovementFromAmbiguity(Transform2d estimatedPoseAtObservationTimeToObservationPose, StandardDeviations cameraStandardDeviations) { - final StandardDeviations estimatedPoseStandardDeviations = cameraStandardDeviations.combineWith(PoseEstimatorConstants.ODOMETRY_STANDARD_DEVIATIONS); - return estimatedPoseStandardDeviations.scaleTransformFromStandardDeviations(estimatedPoseAtObservationTimeToObservationPose); + private Transform2d calculateAllowedMovementFromAmbiguity(Transform2d observationDifference, StandardDeviations cameraStandardDeviations) { + final StandardDeviations combinedStandardDeviations = PoseEstimatorConstants.ODOMETRY_STANDARD_DEVIATIONS.combineWith(cameraStandardDeviations); + return combinedStandardDeviations.scaleTransformFromStandardDeviations(observationDifference); } /** diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java index b65525e6..d3e2c287 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java @@ -1,10 +1,12 @@ package frc.trigon.robot.poseestimation.poseestimator; - public class PoseEstimatorConstants { public static final double ODOMETRY_FREQUENCY_HERTZ = 250; static final double POSE_BUFFER_SIZE_SECONDS = 2; - static final StandardDeviations ODOMETRY_STANDARD_DEVIATIONS = new StandardDeviations(0.003, 0.0002); + static final StandardDeviations ODOMETRY_STANDARD_DEVIATIONS = new StandardDeviations(Math.pow(0.003, 2), Math.pow(0.0002, 2)); + static final double + MAXIMUM_TRANSLATION_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_METERS_PER_SECOND = 0, + MAXIMUM_THETA_VELOCITY_FOR_RELATIVE_ROBOT_POSE_SOURCE_OFFSET_RESETTING_RADIANS_PER_SECOND = 0; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java index c570a16b..b578e637 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/StandardDeviations.java @@ -22,6 +22,14 @@ public StandardDeviations(double translationStandardDeviation, double thetaStand this.thetaStandardDeviation = thetaStandardDeviation; } + public double getTranslationStandardDeviation() { + return translationStandardDeviation; + } + + public double getThetaStandardDeviation() { + return thetaStandardDeviation; + } + /** * Combines this with another {@link StandardDeviations}. * This might be used when estimating a pose in relation to another estimated pose. @@ -30,14 +38,14 @@ public StandardDeviations(double translationStandardDeviation, double thetaStand * @param other the {@link StandardDeviations} to combine with * @return the combined {@link StandardDeviations} */ - StandardDeviations combineWith(StandardDeviations other) { + public StandardDeviations combineWith(StandardDeviations other) { return new StandardDeviations( combineStandardDeviation(translationStandardDeviation, other.translationStandardDeviation), combineStandardDeviation(thetaStandardDeviation, other.thetaStandardDeviation) ); } - Transform2d scaleTransformFromStandardDeviations(Transform2d transform) { + public Transform2d scaleTransformFromStandardDeviations(Transform2d transform) { return new Transform2d( transform.getX() * translationStandardDeviation, transform.getY() * translationStandardDeviation, @@ -53,6 +61,12 @@ Transform2d scaleTransformFromStandardDeviations(Transform2d transform) { * @return the combined standard deviation */ private double combineStandardDeviation(double firstStandardDeviation, double secondStandardDeviation) { - return firstStandardDeviation / (firstStandardDeviation + Math.sqrt(firstStandardDeviation * secondStandardDeviation)); + if (firstStandardDeviation == 0.0) + return 0.0; + + final double squaredSecondStandardDeviation = secondStandardDeviation * secondStandardDeviation; + final double combinedSquareRoot = Math.sqrt(firstStandardDeviation * squaredSecondStandardDeviation); + final double denominator = firstStandardDeviation + combinedSquareRoot; + return firstStandardDeviation / denominator; } } diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSource.java new file mode 100644 index 00000000..0d1e1b9f --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSource.java @@ -0,0 +1,72 @@ +package frc.trigon.robot.poseestimation.relativerobotposesource; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform2d; +import org.littletonrobotics.junction.AutoLogOutput; + +/** + * A relative robot pose source is a robot pose source that calculates its position externally. + * The origin point of the relative pose source doesn't necessarily match the origin point of the robot's estimated pose, so the estimated pose needs to be transformed by that difference. + */ +public class RelativeRobotPoseSource { + private final RelativeRobotPoseSourceInputsAutoLogged inputs = new RelativeRobotPoseSourceInputsAutoLogged(); + private final Transform2d cameraToRobotCenter; + private final RelativeRobotPoseSourceIO relativeRobotPoseSourceIO; + + private double lastResultTimestampSeconds = 0; + private Pose2d robotPoseAtSyncTime = new Pose2d(); + private Pose2d relativePoseSourceEstimatedPoseAtSyncTime = new Pose2d(); + + /** + * Constructs a new RelativeRobotPoseSource. + * + * @param cameraToRobotCenter the transform of the camera to the robot's origin point + * @param hostname the name of the camera in the NetworkTables + */ + public RelativeRobotPoseSource(Transform2d cameraToRobotCenter, String hostname) { + this.cameraToRobotCenter = cameraToRobotCenter; + this.relativeRobotPoseSourceIO = RelativeRobotPoseSourceIO.generateIO(hostname); + } + + public void update() { + relativeRobotPoseSourceIO.updateInputs(inputs); + } + + /** + * Resets the offset from the relative robot pose source to the robot pose. + * The offset is stored as two {@link Pose2d}s, one for the robot's pose and the other for the relative pose source's pose both at the same timestamp. + * With these two poses, we can calculate the total distance moved from the relative pose source's perspective and transform the robot's pose by the same amount. + * + * @param robotPose the current pose of the robot + */ + public void resetOffset(Pose2d robotPose) { + this.robotPoseAtSyncTime = robotPose; + this.relativePoseSourceEstimatedPoseAtSyncTime = getRobotPoseFromCameraPose(inputs.pose); + } + + @AutoLogOutput(key = "RelativeRobotPoseSource/EstimatedRobotPose") + public Pose2d getEstimatedRobotPose() { + final Transform2d movementFromSyncedPose = new Transform2d(relativePoseSourceEstimatedPoseAtSyncTime, getRobotPoseFromCameraPose(inputs.pose)); + return robotPoseAtSyncTime.transformBy(movementFromSyncedPose); + } + + public double getLatestResultTimestampSeconds() { + return inputs.resultTimestampSeconds; + } + + public boolean hasNewResult() { + return inputs.hasResult && isNewTimestamp(); + } + + private boolean isNewTimestamp() { + if (lastResultTimestampSeconds == getLatestResultTimestampSeconds()) + return false; + + lastResultTimestampSeconds = getLatestResultTimestampSeconds(); + return true; + } + + private Pose2d getRobotPoseFromCameraPose(Pose2d cameraPose) { + return cameraPose.transformBy(cameraToRobotCenter); + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java new file mode 100644 index 00000000..7c7a2812 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceConstants.java @@ -0,0 +1,7 @@ +package frc.trigon.robot.poseestimation.relativerobotposesource; + +import frc.trigon.robot.poseestimation.poseestimator.StandardDeviations; + +public class RelativeRobotPoseSourceConstants { + public static final StandardDeviations STANDARD_DEVIATIONS = new StandardDeviations(0.0000001, 0.0000001); +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceIO.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceIO.java new file mode 100644 index 00000000..7775d0e9 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/RelativeRobotPoseSourceIO.java @@ -0,0 +1,29 @@ +package frc.trigon.robot.poseestimation.relativerobotposesource; + +import edu.wpi.first.math.geometry.Pose2d; +import frc.trigon.robot.poseestimation.relativerobotposesource.io.RelativeRobotPoseSourceSimulationIO; +import frc.trigon.robot.poseestimation.relativerobotposesource.io.RelativeRobotPoseSourceT265IO; +import org.littletonrobotics.junction.AutoLog; +import org.trigon.hardware.RobotHardwareStats; + +public class RelativeRobotPoseSourceIO { + static RelativeRobotPoseSourceIO generateIO(String hostname) { + if (RobotHardwareStats.isReplay()) + return new RelativeRobotPoseSourceIO(); + if (RobotHardwareStats.isSimulation()) + return new RelativeRobotPoseSourceSimulationIO(); + return new RelativeRobotPoseSourceT265IO(hostname); + } + + protected void updateInputs(RelativeRobotPoseSourceInputsAutoLogged inputs) { + } + + @AutoLog + public static class RelativeRobotPoseSourceInputs { + public int framesPerSecond = 0; + public double batteryPercentage = 0; + public Pose2d pose = new Pose2d(); + public double resultTimestampSeconds = 0; + public boolean hasResult = false; + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceSimulationIO.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceSimulationIO.java new file mode 100644 index 00000000..be67f68f --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceSimulationIO.java @@ -0,0 +1,17 @@ +package frc.trigon.robot.poseestimation.relativerobotposesource.io; + +import edu.wpi.first.wpilibj.Timer; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceIO; +import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceInputsAutoLogged; + +public class RelativeRobotPoseSourceSimulationIO extends RelativeRobotPoseSourceIO { + @Override + protected void updateInputs(RelativeRobotPoseSourceInputsAutoLogged inputs) { + inputs.framesPerSecond = 60; + inputs.batteryPercentage = 100; + inputs.pose = RobotContainer.POSE_ESTIMATOR.getCurrentOdometryPose(); + inputs.resultTimestampSeconds = Timer.getTimestamp(); + inputs.hasResult = true; + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceT265IO.java b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceT265IO.java new file mode 100644 index 00000000..8079c473 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/relativerobotposesource/io/RelativeRobotPoseSourceT265IO.java @@ -0,0 +1,67 @@ +package frc.trigon.robot.poseestimation.relativerobotposesource.io; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceIO; +import frc.trigon.robot.poseestimation.relativerobotposesource.RelativeRobotPoseSourceInputsAutoLogged; +import org.trigon.utilities.JsonHandler; + +public class RelativeRobotPoseSourceT265IO extends RelativeRobotPoseSourceIO { + private final NetworkTableEntry jsonDumpEntry; + + public RelativeRobotPoseSourceT265IO(String hostname) { + jsonDumpEntry = NetworkTableInstance.getDefault().getTable(hostname).getEntry("JsonDump"); + } + + @Override + protected void updateInputs(RelativeRobotPoseSourceInputsAutoLogged inputs) { + final T265JsonDump jsonDump = readJsonDump(); + if (jsonDump == null) { + updateNoResultInputs(inputs); + return; + } + updateHasResultInputs(inputs, jsonDump); + } + + private void updateNoResultInputs(RelativeRobotPoseSourceInputsAutoLogged inputs) { + inputs.framesPerSecond = 0; + inputs.hasResult = false; + } + + private void updateHasResultInputs(RelativeRobotPoseSourceInputsAutoLogged inputs, T265JsonDump jsonDump) { + inputs.framesPerSecond = jsonDump.framesPerSecond; + inputs.batteryPercentage = jsonDump.batteryPercentage; + inputs.pose = extractPose(jsonDump); + inputs.resultTimestampSeconds = jsonDumpEntry.getLastChange(); + inputs.hasResult = true; + } + + private T265JsonDump readJsonDump() { + return JsonHandler.parseJsonStringToObject(jsonDumpEntry.getString(""), T265JsonDump.class); + } + + private Pose2d extractPose(T265JsonDump jsonDump) { + final Translation2d translation = extractTranslation(jsonDump); + final Rotation2d heading = extractHeading(jsonDump); + return new Pose2d(translation, heading); + } + + private Translation2d extractTranslation(T265JsonDump jsonDump) { + return new Translation2d(jsonDump.xPositionMeters, jsonDump.yPositionMeters); + } + + private Rotation2d extractHeading(T265JsonDump jsonDump) { + return Rotation2d.fromRadians(jsonDump.rotationRadians); + } + + private static class T265JsonDump { + private int framesPerSecond = 0; + private double batteryPercentage = 0; + private double xPositionMeters = 0; + private double yPositionMeters = 0; + private double rotationRadians = 0; + } +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 5a8b03b4..e44002cf 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "dev-v2025.0.0-beta-8-22-gfa2034d3", + "version": "dev-v2025.0.0-beta-8-23-g484e8d42", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "dev-v2025.0.0-beta-8-22-gfa2034d3", + "version": "dev-v2025.0.0-beta-8-23-g484e8d42", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "dev-v2025.0.0-beta-8-22-gfa2034d3", + "version": "dev-v2025.0.0-beta-8-23-g484e8d42", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "dev-v2025.0.0-beta-8-22-gfa2034d3", + "version": "dev-v2025.0.0-beta-8-23-g484e8d42", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "dev-v2025.0.0-beta-8-22-gfa2034d3" + "version": "dev-v2025.0.0-beta-8-23-g484e8d42" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "dev-v2025.0.0-beta-8-22-gfa2034d3" + "version": "dev-v2025.0.0-beta-8-23-g484e8d42" } ] }