From e0f272a0e597193613589c9b1dcec5c8f1367256 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sun, 29 Sep 2024 21:47:32 +0300 Subject: [PATCH] Reworked vision logic --- build.gradle | 2 +- simgui-window.json | 18 +- .../robot/constants/CameraConstants.java | 35 +- .../apriltagcamera/AprilTagCamera.java | 124 +++ .../AprilTagCameraConstants.java | 27 + .../apriltagcamera/AprilTagCameraIO.java | 19 + .../io/AprilTagLimelightIO.java | 81 ++ .../io/AprilTagPhotonCameraIO.java | 100 +++ .../EstimatedRobotPose.java | 72 -- .../PhotonPoseEstimator.java | 791 ------------------ .../poseestimator/PoseEstimator.java | 41 +- .../poseestimator/PoseEstimator6328.java | 7 + .../poseestimator/PoseEstimatorConstants.java | 2 +- .../robotposesources/AprilTagLimelightIO.java | 21 - .../AprilTagPhotonCameraIO.java | 91 -- .../robotposesources/RobotPoseSource.java | 82 -- .../RobotPoseSourceConstants.java | 40 - .../robotposesources/RobotPoseSourceIO.java | 18 - .../robotposesources/T265IO.java | 76 -- .../subsystems/swerve/SwerveConstants.java | 2 +- .../robot/subsystems/swerve/SwerveModule.java | 7 +- 21 files changed, 411 insertions(+), 1245 deletions(-) create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java create mode 100644 src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java delete mode 100644 src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java diff --git a/build.gradle b/build.gradle index b0d27e6b..a54357f9 100644 --- a/build.gradle +++ b/build.gradle @@ -88,7 +88,7 @@ dependencies { simulationRelease wpi.sim.enableRelease() implementation 'com.google.code.gson:gson:2.10.1' - implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.4' + implementation 'com.github.Programming-TRIGON:TRIGONLib:2024.2.9' def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" diff --git a/simgui-window.json b/simgui-window.json index d1d16c31..d30bc117 100644 --- a/simgui-window.json +++ b/simgui-window.json @@ -5,7 +5,7 @@ "MainWindow": { "GLOBAL": { "fps": "120", - "height": "1001", + "height": "974", "maximized": "1", "style": "0", "userScale": "2", @@ -71,22 +71,22 @@ "###/SmartDashboard/AutoChooser": { "Collapsed": "0", "Pos": "-3,891", - "Size": "307,60" + "Size": "361,66" }, "###/SmartDashboard/Field": { "Collapsed": "0", - "Pos": "1278,491", - "Size": "633,365" + "Pos": "350,392", + "Size": "1534,512" }, "###FMS": { "Collapsed": "0", "Pos": "-1,712", - "Size": "202,214" + "Size": "388,176" }, "###Joysticks": { "Collapsed": "0", "Pos": "320,888", - "Size": "976,278" + "Size": "1156,91" }, "###NetworkTables": { "Collapsed": "0", @@ -116,12 +116,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "1,346", - "Size": "232,254" + "Size": "273,290" }, "###Timing": { "Collapsed": "0", "Pos": "0,172", - "Size": "169,168" + "Size": "197,186" }, "Debug##Default": { "Collapsed": "0", @@ -131,7 +131,7 @@ "Robot State": { "Collapsed": "0", "Pos": "2,25", - "Size": "109,134" + "Size": "126,152" } } } diff --git a/src/main/java/frc/trigon/robot/constants/CameraConstants.java b/src/main/java/frc/trigon/robot/constants/CameraConstants.java index edbbf73d..8a60e232 100644 --- a/src/main/java/frc/trigon/robot/constants/CameraConstants.java +++ b/src/main/java/frc/trigon/robot/constants/CameraConstants.java @@ -5,8 +5,9 @@ import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import frc.trigon.robot.misc.objectdetectioncamera.ObjectDetectionCamera; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSource; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSourceConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.poseestimator.PoseEstimatorConstants; public class CameraConstants { public static final ObjectDetectionCamera NOTE_DETECTION_CAMERA = new ObjectDetectionCamera("Collection Camera"); @@ -24,28 +25,34 @@ public class CameraConstants { new Rotation3d(0, Units.degreesToRadians(-31.7), 0) ), REAR_MIDDLE_CENTER_TO_CAMERA = new Transform3d( - new Translation3d(0, 0, 0.632), - new Rotation3d(0, Units.degreesToRadians(-24), Units.degreesToRadians(180)) + new Translation3d(0, 0, 0.62), + new Rotation3d(0, Units.degreesToRadians(-23.5), Units.degreesToRadians(180)) ); - public static final RobotPoseSource - REAR_LEFT_CAMERA = new RobotPoseSource( - RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA, + public static final AprilTagCamera + REAR_LEFT_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.RobotPoseSourceType.PHOTON_CAMERA, "Rear Left Camera", - REAR_LEFT_CENTER_TO_CAMERA + REAR_LEFT_CENTER_TO_CAMERA, + PoseEstimatorConstants.THETA_STD_EXPONENT, + PoseEstimatorConstants.TRANSLATIONS_STD_EXPONENT ), - REAR_RIGHT_CAMERA = new RobotPoseSource( - RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA, + REAR_RIGHT_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.RobotPoseSourceType.PHOTON_CAMERA, "Rear Right Camera", - REAR_RIGHT_CENTER_TO_CAMERA + REAR_RIGHT_CENTER_TO_CAMERA, + PoseEstimatorConstants.THETA_STD_EXPONENT, + PoseEstimatorConstants.TRANSLATIONS_STD_EXPONENT ), // FRONT_MIDDLE_CAMERA = new RobotPoseSource( // RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA, // "Front Middle Camera", // FRONT_MIDDLE_CENTER_TO_CAMERA // ), - REAR_MIDDLE_CAMERA = new RobotPoseSource( - RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA, + REAR_MIDDLE_CAMERA = new AprilTagCamera( + AprilTagCameraConstants.RobotPoseSourceType.PHOTON_CAMERA, "Rear Middle Camera", - REAR_MIDDLE_CENTER_TO_CAMERA + REAR_MIDDLE_CENTER_TO_CAMERA, + PoseEstimatorConstants.THETA_STD_EXPONENT / 10, + PoseEstimatorConstants.TRANSLATIONS_STD_EXPONENT ); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java new file mode 100644 index 00000000..7ce34cfb --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java @@ -0,0 +1,124 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +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.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import frc.trigon.robot.Robot; +import frc.trigon.robot.constants.FieldConstants; +import org.littletonrobotics.junction.Logger; + +/** + * 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. + * 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 RobotPoseSourceInputsAutoLogged inputs = new RobotPoseSourceInputsAutoLogged(); + private final Transform3d robotCenterToCamera; + private final AprilTagCameraIO aprilTagCameraIO; + private final double + thetaStandardDeviationsExponent, + translationStandardDeviationsExponent; + private double lastUpdatedTimestamp; + private Pose2d robotPose = null; + + /** + * Constructs a new AprilTagCamera. + * + * @param robotPoseSourceType the type of camera + * @param name the camera's name + * @param robotCenterToCamera the transform of the robot's origin point to the camera + * @param thetaStandardDeviationsExponent the calibrated gain to calculate the theta deviation from the estimated pose when using solve PNP + * @param translationStandardDeviationsExponent the calibrated gain to calculate the translation deviation from the estimated pose when using solve PNP + */ + public AprilTagCamera(AprilTagCameraConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera, double thetaStandardDeviationsExponent, double translationStandardDeviationsExponent) { + this.name = name; + this.robotCenterToCamera = robotCenterToCamera; + this.thetaStandardDeviationsExponent = thetaStandardDeviationsExponent; + this.translationStandardDeviationsExponent = translationStandardDeviationsExponent; + + if (Robot.IS_REAL) + aprilTagCameraIO = robotPoseSourceType.createIOFunction.apply(name); + else + aprilTagCameraIO = new AprilTagCameraIO(); + } + + public void update() { + aprilTagCameraIO.updateInputs(inputs); + Logger.processInputs("Cameras/" + name, inputs); + robotPose = inputs.cameraSolvePNPPose.transformBy(robotCenterToCamera.inverse()).toPose2d(); + + logEstimatedRobotPose(); + if (!FieldConstants.TAG_ID_TO_POSE.isEmpty()) + logUsedTags(); + } + + public boolean hasNewResult() { + return (inputs.hasResult && inputs.averageDistanceFromAllTags != 0) && isNewTimestamp(); + } + + public Pose2d getEstimatedRobotPose() { + return robotPose; + } + + public String getName() { + return name; + } + + public double getLatestResultTimestampSeconds() { + return inputs.latestResultTimestampSeconds; + } + + private boolean isNewTimestamp() { + if (lastUpdatedTimestamp == getLatestResultTimestampSeconds()) + return false; + + lastUpdatedTimestamp = getLatestResultTimestampSeconds(); + return true; + } + + public Matrix getStandardDeviations() { + final int numberOfVisibleTags = inputs.visibleTagIDs.length; + final double translationStandardDeviation = calculateStandardDeviations(translationStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); + final double thetaStandardDeviation = calculateStandardDeviations(thetaStandardDeviationsExponent, inputs.averageDistanceFromAllTags, numberOfVisibleTags); + + return VecBuilder.fill(translationStandardDeviation, translationStandardDeviation, thetaStandardDeviation); + } + + /** + * Calculates the standard deviation 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 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) { + return exponent * (distance * distance) / numberOfVisibleTags; + } + + private void logEstimatedRobotPose() { + if (!inputs.hasResult || inputs.averageDistanceFromAllTags == 0 || robotPose == null) + Logger.recordOutput("Poses/Robot/" + name + "Pose", AprilTagCameraConstants.EMPTY_POSE_LIST); + else + Logger.recordOutput("Poses/Robot/" + name + "Pose", robotPose); + } + + private void logUsedTags() { + if (!inputs.hasResult) { + Logger.recordOutput("UsedTags/" + this.getName(), new Pose3d[0]); + return; + } + + final Pose3d[] usedTagPoses = new Pose3d[inputs.visibleTagIDs.length]; + 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); + } +} \ 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 new file mode 100644 index 00000000..8e09d6cb --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java @@ -0,0 +1,27 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.geometry.Pose2d; +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.poseestimation.apriltagcamera.io.AprilTagLimelightIO; +import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO; + +import java.util.function.Function; + +public class AprilTagCameraConstants { + public static final Transform3d TAG_OFFSET = new Transform3d(0, 0, -0.06, new Rotation3d(Units.degreesToRadians(-1.8), 0, 0)); + static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; + public static final double MAXIMUM_AMBIGUITY = 0.2; + + public enum RobotPoseSourceType { + PHOTON_CAMERA(AprilTagPhotonCameraIO::new), + LIMELIGHT(AprilTagLimelightIO::new); + + final Function createIOFunction; + + RobotPoseSourceType(Function createIOFunction) { + this.createIOFunction = createIOFunction; + } + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java new file mode 100644 index 00000000..d4f1f56f --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraIO.java @@ -0,0 +1,19 @@ +package frc.trigon.robot.poseestimation.apriltagcamera; + +import edu.wpi.first.math.geometry.Pose3d; +import org.littletonrobotics.junction.AutoLog; + +public class AprilTagCameraIO { + protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { + } + + @AutoLog + public static class RobotPoseSourceInputs { + public boolean hasResult = false; + public double latestResultTimestampSeconds = 0; + public Pose3d cameraSolvePNPPose = new Pose3d(); + public int[] visibleTagIDs = new int[0]; + public double averageDistanceFromAllTags = 0; + public double distanceFromBestTag = 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 new file mode 100644 index 00000000..5fe25a92 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagLimelightIO.java @@ -0,0 +1,81 @@ +package frc.trigon.robot.poseestimation.apriltagcamera.io; + +import edu.wpi.first.math.geometry.Pose3d; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.RobotPoseSourceInputsAutoLogged; +import org.trigon.utilities.LimelightHelpers; + +public class AprilTagLimelightIO extends AprilTagCameraIO { + private final String hostname; + + public AprilTagLimelightIO(String hostname) { + this.hostname = hostname; + } + + @Override + protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { + final LimelightHelpers.LimelightResults results = LimelightHelpers.getLatestResults(hostname); + + inputs.hasResult = results.targets_Fiducials.length > 0; + + if (inputs.hasResult) + updateHasResultInputs(inputs, results); + else + updateNoResultInputs(inputs); + } + + private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, LimelightHelpers.LimelightResults results) { + inputs.cameraSolvePNPPose = results.getBotPose3d_wpiBlue(); + inputs.latestResultTimestampSeconds = results.timestamp_RIOFPGA_capture; + inputs.visibleTagIDs = getVisibleTagIDs(results); + inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(results); + inputs.distanceFromBestTag = getDistanceFromBestTag(results); + } + + private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { + inputs.visibleTagIDs = new int[0]; + inputs.cameraSolvePNPPose = new Pose3d(); + } + + private int[] getVisibleTagIDs(LimelightHelpers.LimelightResults results) { + final LimelightHelpers.LimelightTarget_Fiducial[] visibleTags = results.targets_Fiducials; + final int[] visibleTagIDs = new int[visibleTags.length]; + visibleTagIDs[0] = (int) getBestTarget(results).fiducialID; + int idAddition = 1; + + for (int i = 0; i < visibleTagIDs.length; i++) { + final int currentID = (int) visibleTags[i].fiducialID; + + if (currentID == visibleTagIDs[0]) { + idAddition = 0; + continue; + } + + visibleTagIDs[i + idAddition] = currentID; + } + return visibleTagIDs; + } + + private double getAverageDistanceFromAllTags(LimelightHelpers.LimelightResults results) { + final LimelightHelpers.LimelightTarget_Fiducial[] targetFiducials = results.targets_Fiducials; + double totalDistanceFromTags = 0; + + for (LimelightHelpers.LimelightTarget_Fiducial targetFiducial : targetFiducials) + totalDistanceFromTags += getDistanceFromTag((int) targetFiducial.fiducialID, results.getBotPose3d_wpiBlue()); + + return totalDistanceFromTags / results.targets_Fiducials.length; + } + + 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]; + } +} \ No newline at end of file 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 new file mode 100644 index 00000000..771854d8 --- /dev/null +++ b/src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java @@ -0,0 +1,100 @@ +package frc.trigon.robot.poseestimation.apriltagcamera.io; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Transform3d; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCameraIO; +import frc.trigon.robot.poseestimation.apriltagcamera.RobotPoseSourceInputsAutoLogged; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PNPResult; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class AprilTagPhotonCameraIO extends AprilTagCameraIO { + private final PhotonCamera photonCamera; + + public AprilTagPhotonCameraIO(String cameraName) { + photonCamera = new PhotonCamera(cameraName); + } + + @Override + protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { + final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); + + inputs.hasResult = latestResult.hasTargets() && !latestResult.getTargets().isEmpty(); + if (inputs.hasResult) + updateHasResultInputs(inputs, latestResult); + else + updateNoResultInputs(inputs); + } + + private void updateHasResultInputs(RobotPoseSourceInputsAutoLogged inputs, PhotonPipelineResult latestResult) { + inputs.cameraSolvePNPPose = getSolvePNPPose(latestResult); + inputs.latestResultTimestampSeconds = latestResult.getTimestampSeconds(); + inputs.visibleTagIDs = getVisibleTagIDs(latestResult); + inputs.averageDistanceFromAllTags = getAverageDistanceFromAllTags(latestResult); + inputs.distanceFromBestTag = getDistanceFromBestTag(latestResult); + } + + private void updateNoResultInputs(RobotPoseSourceInputsAutoLogged inputs) { + inputs.visibleTagIDs = new int[]{}; + inputs.cameraSolvePNPPose = new Pose3d(); + } + + /** + * Estimates the camera's pose using Solve PNP using as many tags as possible. + * + * @param result the camera's pipeline result + * @return the estimated pose + */ + private Pose3d getSolvePNPPose(PhotonPipelineResult result) { + final PNPResult multitagPose = result.getMultiTagResult().estimatedPose; + if (multitagPose.isPresent && multitagPose.ambiguity < AprilTagCameraConstants.MAXIMUM_AMBIGUITY) { + final Transform3d cameraPoseTransform = multitagPose.best; + return new Pose3d().plus(cameraPoseTransform).relativeTo(FieldConstants.APRIL_TAG_FIELD_LAYOUT.getOrigin()); + } + + final PhotonTrackedTarget bestTarget = result.getBestTarget(); + if (bestTarget.getPoseAmbiguity() > AprilTagCameraConstants.MAXIMUM_AMBIGUITY) + return new Pose3d(); + + final Pose3d tagPose = FieldConstants.TAG_ID_TO_POSE.get(bestTarget.getFiducialId()); + final Transform3d targetToCamera = bestTarget.getBestCameraToTarget().inverse(); + return tagPose + .transformBy(targetToCamera) + .transformBy(AprilTagCameraConstants.TAG_OFFSET); + } + + private int[] getVisibleTagIDs(PhotonPipelineResult result) { + final int[] visibleTagIDs = new int[result.getTargets().size()]; + visibleTagIDs[0] = result.getBestTarget().getFiducialId(); + int idAddition = 1; + + for (int i = 0; i < visibleTagIDs.length; i++) { + final int currentID = result.getTargets().get(i).getFiducialId(); + + if (currentID == visibleTagIDs[0]) { + idAddition = 0; + continue; + } + visibleTagIDs[i + idAddition] = currentID; + } + + return visibleTagIDs; + } + + private double getAverageDistanceFromAllTags(PhotonPipelineResult result) { + final int tagsSeen = result.getTargets().size(); + double totalTagDistance = 0; + + for (int i = 0; i < tagsSeen; i++) + totalTagDistance += result.getTargets().get(i).getBestCameraToTarget().getTranslation().getNorm(); + + return totalTagDistance / tagsSeen; + } + + private double getDistanceFromBestTag(PhotonPipelineResult result) { + return result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm(); + } +} diff --git a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java b/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java deleted file mode 100644 index dcc605b7..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/EstimatedRobotPose.java +++ /dev/null @@ -1,72 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.trigon.robot.poseestimation.photonposeestimator; - -import edu.wpi.first.math.geometry.Pose3d; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.List; - -/** - * An estimated pose based on pipeline result - */ -public class EstimatedRobotPose { - /** - * The estimated pose - */ - public final Pose3d estimatedPose; - - /** - * The estimated time the frame used to derive the robot pose was taken - */ - public final double timestampSeconds; - - /** - * A list of the targets used to compute this pose - */ - public final List targetsUsed; - - /** - * The strategy actually used to produce this pose - */ - public final PhotonPoseEstimator.PoseStrategy strategy; - - /** - * Constructs an EstimatedRobotPose - * - * @param estimatedPose estimated pose - * @param timestampSeconds timestamp of the estimate - */ - public EstimatedRobotPose( - Pose3d estimatedPose, - double timestampSeconds, - List targetsUsed, - PhotonPoseEstimator.PoseStrategy strategy) { - this.estimatedPose = estimatedPose; - this.timestampSeconds = timestampSeconds; - this.targetsUsed = targetsUsed; - this.strategy = strategy; - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java b/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java deleted file mode 100644 index 9c932c21..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/photonposeestimator/PhotonPoseEstimator.java +++ /dev/null @@ -1,791 +0,0 @@ -/* - * MIT License - * - * Copyright (c) PhotonVision - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package frc.trigon.robot.poseestimation.photonposeestimator; - -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.hal.FRCNetComm.tResourceType; -import edu.wpi.first.hal.HAL; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; -import edu.wpi.first.math.numbers.N5; -import edu.wpi.first.wpilibj.DriverStation; -import frc.trigon.robot.RobotContainer; -import org.photonvision.PhotonCamera; -import org.photonvision.estimation.TargetModel; -import org.photonvision.estimation.VisionEstimation; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.HashSet; -import java.util.Optional; -import java.util.Set; - -/** - * The PhotonPoseEstimator class filters or combines readings from all the AprilTags visible at a - * given timestamp on the field to produce a single robot in field pose, using the strategy set - * below. Example usage can be found in our apriltagExample example project. - */ -public class PhotonPoseEstimator { - private static int InstanceCount = 0; - - /** - * Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. - */ - public enum PoseStrategy { - /** - * Choose the Pose with the lowest ambiguity. - */ - LOWEST_AMBIGUITY, - - /** - * Choose the Pose which is closest to the camera height. - */ - CLOSEST_TO_CAMERA_HEIGHT, - - /** - * Choose the Pose which is closest to a set Reference position. - */ - CLOSEST_TO_REFERENCE_POSE, - - /** - * Choose the Pose which is closest to the last pose calculated - */ - CLOSEST_TO_LAST_POSE, - - /** - * 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 on coprocessor. This option needs to - * be enabled on the PhotonVision web UI as well. - */ - MULTI_TAG_PNP_ON_COPROCESSOR, - - /** - * Use all visible tags to compute a single pose estimate. This runs on the RoboRIO, and can - * take a lot of time. - */ - MULTI_TAG_PNP_ON_RIO, - - CLOSEST_TO_HEADING - } - - private AprilTagFieldLayout fieldTags; - private TargetModel tagModel = TargetModel.kAprilTag16h5; - private PoseStrategy primaryStrategy; - private PoseStrategy multiTagFallbackStrategy = PoseStrategy.LOWEST_AMBIGUITY; - private final PhotonCamera camera; - private Transform3d robotToCamera; - - private Pose3d lastPose; - private Pose3d referencePose; - protected double poseCacheTimestampSeconds = -1; - private final Set reportedErrors = new HashSet<>(); - - /** - * Create a new PhotonPoseEstimator. - * - * @param fieldTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3d objects - * with respect to the FIRST field using the Field - * Coordinate System. Note that setting the origin of this layout object will affect the - * results from this class. - * @param strategy The strategy it should use to determine the best pose. - * @param camera PhotonCamera - * @param robotToCamera Transform3d from the center of the robot to the camera mount position (ie, - * robot -> camera) in the Robot - * Coordinate System. - */ - public PhotonPoseEstimator( - AprilTagFieldLayout fieldTags, - PoseStrategy strategy, - PhotonCamera camera, - Transform3d robotToCamera) { - this.fieldTags = fieldTags; - this.primaryStrategy = strategy; - this.camera = camera; - this.robotToCamera = robotToCamera; - - HAL.report(tResourceType.kResourceType_PhotonPoseEstimator, InstanceCount); - InstanceCount++; - } - - public PhotonPoseEstimator( - AprilTagFieldLayout fieldTags, PoseStrategy strategy, Transform3d robotToCamera) { - this(fieldTags, strategy, null, robotToCamera); - } - - /** - * Invalidates the pose cache. - */ - private void invalidatePoseCache() { - poseCacheTimestampSeconds = -1; - } - - private void checkUpdate(Object oldObj, Object newObj) { - if (oldObj != newObj && oldObj != null && !oldObj.equals(newObj)) { - invalidatePoseCache(); - } - } - - /** - * Get the AprilTagFieldLayout being used by the PositionEstimator. - * - *

Note: Setting the origin of this layout will affect the results from this class. - * - * @return the AprilTagFieldLayout - */ - public AprilTagFieldLayout getFieldTags() { - return fieldTags; - } - - /** - * Set the AprilTagFieldLayout being used by the PositionEstimator. - * - *

Note: Setting the origin of this layout will affect the results from this class. - * - * @param fieldTags the AprilTagFieldLayout - */ - public void setFieldTags(AprilTagFieldLayout fieldTags) { - checkUpdate(this.fieldTags, fieldTags); - this.fieldTags = fieldTags; - } - - /** - * Get the TargetModel representing the tags being detected. This is used for on-rio multitag. - * - *

By default, this is {@link TargetModel#kAprilTag16h5}. - */ - public TargetModel getTagModel() { - return tagModel; - } - - /** - * Set the TargetModel representing the tags being detected. This is used for on-rio multitag. - * - * @param tagModel E.g. {@link TargetModel#kAprilTag16h5}. - */ - public void setTagModel(TargetModel tagModel) { - this.tagModel = tagModel; - } - - /** - * Get the Position Estimation Strategy being used by the Position Estimator. - * - * @return the strategy - */ - public PoseStrategy getPrimaryStrategy() { - return primaryStrategy; - } - - /** - * Set the Position Estimation Strategy used by the Position Estimator. - * - * @param strategy the strategy to set - */ - public void setPrimaryStrategy(PoseStrategy strategy) { - checkUpdate(this.primaryStrategy, strategy); - this.primaryStrategy = strategy; - } - - /** - * Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. Must - * NOT be MULTI_TAG_PNP - * - * @param strategy the strategy to set - */ - public void setMultiTagFallbackStrategy(PoseStrategy strategy) { - checkUpdate(this.multiTagFallbackStrategy, strategy); - if (strategy == PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR - || strategy == PoseStrategy.MULTI_TAG_PNP_ON_RIO) { - DriverStation.reportWarning( - "Fallback cannot be set to MULTI_TAG_PNP! Setting to lowest ambiguity", false); - strategy = PoseStrategy.LOWEST_AMBIGUITY; - } - this.multiTagFallbackStrategy = strategy; - } - - /** - * Return the reference position that is being used by the estimator. - * - * @return the referencePose - */ - public Pose3d getReferencePose() { - return referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose3d referencePose) { - checkUpdate(this.referencePose, referencePose); - this.referencePose = referencePose; - } - - /** - * Update the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE - * strategy. - * - * @param referencePose the referencePose to set - */ - public void setReferencePose(Pose2d referencePose) { - setReferencePose(new Pose3d(referencePose)); - } - - /** - * Update the stored last pose. Useful for setting the initial estimate when using the - * CLOSEST_TO_LAST_POSE strategy. - * - * @param lastPose the lastPose to set - */ - public void setLastPose(Pose3d lastPose) { - this.lastPose = lastPose; - } - - /** - * Update the stored last pose. Useful for setting the initial estimate when using the - * CLOSEST_TO_LAST_POSE strategy. - * - * @param lastPose the lastPose to set - */ - public void setLastPose(Pose2d lastPose) { - setLastPose(new Pose3d(lastPose)); - } - - /** - * @return The current transform from the center of the robot to the camera mount position - */ - public Transform3d getRobotToCameraTransform() { - return robotToCamera; - } - - /** - * Useful for pan and tilt mechanisms and such. - * - * @param robotToCamera The current transform from the center of the robot to the camera mount - * position - */ - public void setRobotToCameraTransform(Transform3d robotToCamera) { - this.robotToCamera = robotToCamera; - } - - /** - * Poll data from the configured cameras and update the estimated position of the robot. Returns - * empty if: - * - *

    - *
  • New data has not been received since the last call to {@code update()}. - *
  • No targets were found from the camera - *
  • There is no camera set - *
- * - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update() { - if (camera == null) { - DriverStation.reportError("[PhotonPoseEstimator] Missing camera!", false); - return Optional.empty(); - } - - PhotonPipelineResult cameraResult = camera.getLatestResult(); - - return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraResult The latest pipeline result from the camera - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update(PhotonPipelineResult cameraResult) { - if (camera == null) return update(cameraResult, Optional.empty(), Optional.empty()); - return update(cameraResult, camera.getCameraMatrix(), camera.getDistCoeffs()); - } - - /** - * Updates the estimated position of the robot. Returns empty if: - * - *
    - *
  • The timestamp of the provided pipeline result is the same as in the previous call to - * {@code update()}. - *
  • No targets were found in the pipeline results. - *
- * - * @param cameraMatrix Camera calibration data that can be used in the case of no assigned - * PhotonCamera. - * @param distCoeffs Camera calibration data that can be used in the case of no assigned - * PhotonCamera - * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to - * create the estimate. - */ - public Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs) { - // Time in the past -- give up, since the following if expects times > 0 - if (cameraResult.getTimestampSeconds() < 0) { - return Optional.empty(); - } - - // If the pose cache timestamp was set, and the result is from the same - // timestamp, return an - // empty result -// - - // Remember the timestamp of the current result used - poseCacheTimestampSeconds = cameraResult.getTimestampSeconds(); - - // If no targets seen, trivial case -- return empty result - if (!cameraResult.hasTargets()) { - return Optional.empty(); - } - - return update(cameraResult, cameraMatrix, distCoeffs, this.primaryStrategy); - } - - private Optional update( - PhotonPipelineResult cameraResult, - Optional> cameraMatrix, - Optional> distCoeffs, - PoseStrategy strat) { - Optional estimatedPose; - switch (strat) { - case LOWEST_AMBIGUITY: - estimatedPose = lowestAmbiguityStrategy(cameraResult); - break; - case CLOSEST_TO_CAMERA_HEIGHT: - estimatedPose = closestToCameraHeightStrategy(cameraResult); - break; - case CLOSEST_TO_REFERENCE_POSE: - estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); - break; - case CLOSEST_TO_LAST_POSE: - setReferencePose(lastPose); - estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose); - break; - case AVERAGE_BEST_TARGETS: - estimatedPose = averageBestTargetsStrategy(cameraResult); - break; - case MULTI_TAG_PNP_ON_RIO: - estimatedPose = multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs); - break; - case MULTI_TAG_PNP_ON_COPROCESSOR: - estimatedPose = multiTagOnCoprocStrategy(cameraResult, cameraMatrix, distCoeffs); - break; - case CLOSEST_TO_HEADING: - estimatedPose = closestToHeadingStrategy(cameraResult); - break; - default: - DriverStation.reportError( - "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false); - return Optional.empty(); - } - - if (estimatedPose.isPresent()) { - lastPose = estimatedPose.get().estimatedPose; - } - - return estimatedPose; - } - - private Optional closestToHeadingStrategy(PhotonPipelineResult result) { - double smallestAngleDifferenceRadians; - EstimatedRobotPose closestAngleTarget; - double currentHeadingRadians = RobotContainer.POSE_ESTIMATOR.getCurrentPose().getRotation().getRadians(); - - PhotonTrackedTarget target = result.getBestTarget(); - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) return Optional.empty(); - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - return Optional.empty(); - } - - Transform3d bestCameraToTarget = target.getBestCameraToTarget(); - - Pose3d bestPose = - targetPosition - .get() - .transformBy(bestCameraToTarget.inverse()) - .transformBy(robotToCamera.inverse()); - double bestTransformAngle = bestPose.getRotation().getZ(); - smallestAngleDifferenceRadians = Math.abs(currentHeadingRadians - bestTransformAngle); - closestAngleTarget = - new EstimatedRobotPose( - bestPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_HEADING); - - Transform3d alternateCameraToTarget = target.getAlternateCameraToTarget(); - if (alternateCameraToTarget.getRotation().getZ() != 0) { - Pose3d altPose = - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - double alternateTransformAngle = altPose.getRotation().getZ(); - double alternateTransformDelta = Math.abs(currentHeadingRadians - alternateTransformAngle); - - if (alternateTransformDelta < smallestAngleDifferenceRadians) { - closestAngleTarget = - new EstimatedRobotPose( - altPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_HEADING); - } - } - - // Need to null check here in case none of the provided targets are fiducial. - return Optional.of(closestAngleTarget); - } - - private Optional multiTagOnCoprocStrategy( - PhotonPipelineResult result, - Optional> cameraMatrixOpt, - Optional> distCoeffsOpt) { - if (result.getMultiTagResult().estimatedPose.isPresent) { - var best_tf = result.getMultiTagResult().estimatedPose.best; - var cam = new Pose3d() - .plus(best_tf) // field-to-camera - .relativeTo(fieldTags.getOrigin()); - var best = cam.plus(robotToCamera.inverse()); // field-to-robot -// Logger.recordOutput(camera.getName(), Math.toDegrees(cam.getRotation().getY())); - - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR)); - } else { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - } - } - - private Optional multiTagOnRioStrategy( - PhotonPipelineResult result, - Optional> cameraMatrixOpt, - Optional> distCoeffsOpt) { - boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent(); - // cannot run multitagPNP, use fallback strategy - if (!hasCalibData || result.getTargets().size() < 2) { - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - } - - var pnpResult = - VisionEstimation.estimateCamPosePNP( - cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel); - // try fallback strategy if solvePNP fails for some reason - if (!pnpResult.isPresent) - return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy); - var best = - new Pose3d() - .plus(pnpResult.best) // field-to-camera - .plus(robotToCamera.inverse()); // field-to-robot - - return Optional.of( - new EstimatedRobotPose( - best, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.MULTI_TAG_PNP_ON_RIO)); - } - - /** - * Return the estimated position of the robot with the lowest position ambiguity from a List of - * pipeline results. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional lowestAmbiguityStrategy(PhotonPipelineResult result) { - PhotonTrackedTarget lowestAmbiguityTarget = null; - - double lowestAmbiguityScore = 10; - - for (PhotonTrackedTarget target : result.targets) { - double targetPoseAmbiguity = target.getPoseAmbiguity(); - // Make sure the target is a Fiducial target. - if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) { - lowestAmbiguityScore = targetPoseAmbiguity; - lowestAmbiguityTarget = target; - } - } - - // Although there are confirmed to be targets, none of them may be fiducial - // targets. - if (lowestAmbiguityTarget == null) return Optional.empty(); - - int targetFiducialId = lowestAmbiguityTarget.getFiducialId(); - - Optional targetPosition = fieldTags.getTagPose(targetFiducialId); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - return Optional.empty(); - } - - return Optional.of( - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.LOWEST_AMBIGUITY)); - } - - /** - * Return the estimated position of the robot using the target with the lowest delta height - * difference between the estimated and actual height of the camera. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional closestToCameraHeightStrategy(PhotonPipelineResult result) { - double smallestHeightDifference = 10e9; - EstimatedRobotPose closestHeightTarget = null; - - for (PhotonTrackedTarget target : result.targets) { - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) continue; - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(target.getFiducialId()); - continue; - } - - - double alternateTransformDelta = - Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .getZ()); - double bestTransformDelta = - Math.abs( - robotToCamera.getZ() - - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .getZ()); - - if (alternateTransformDelta < smallestHeightDifference) { - smallestHeightDifference = alternateTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); - } - - if (bestTransformDelta < smallestHeightDifference) { - smallestHeightDifference = bestTransformDelta; - closestHeightTarget = - new EstimatedRobotPose( - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()), - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT); - } - } - - // Need to null check here in case none of the provided targets are fiducial. - return Optional.ofNullable(closestHeightTarget); - } - - /** - * Return the estimated position of the robot using the target with the lowest delta in the vector - * magnitude between it and the reference pose. - * - * @param result pipeline result - * @param referencePose reference pose to check vector magnitude difference against. - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional closestToReferencePoseStrategy( - PhotonPipelineResult result, Pose3d referencePose) { - if (referencePose == null) { -// DriverStation.reportError( -// "[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!", -// false); - return Optional.empty(); - } - - double smallestPoseDelta = 10e9; - EstimatedRobotPose lowestDeltaPose = null; - - for (PhotonTrackedTarget target : result.targets) { - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) continue; - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - continue; - } - - Pose3d altTransformPosition = - targetPosition - .get() - .transformBy(target.getAlternateCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - Pose3d bestTransformPosition = - targetPosition - .get() - .transformBy(target.getBestCameraToTarget().inverse()) - .transformBy(robotToCamera.inverse()); - - double altDifference = Math.abs(calculateDifference(referencePose, altTransformPosition)); - double bestDifference = Math.abs(calculateDifference(referencePose, bestTransformPosition)); - - if (altDifference < smallestPoseDelta) { - smallestPoseDelta = altDifference; - lowestDeltaPose = - new EstimatedRobotPose( - altTransformPosition, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - } - if (bestDifference < smallestPoseDelta) { - smallestPoseDelta = bestDifference; - lowestDeltaPose = - new EstimatedRobotPose( - bestTransformPosition, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.CLOSEST_TO_REFERENCE_POSE); - } - } - return Optional.ofNullable(lowestDeltaPose); - } - - /** - * Return the average of the best target poses using ambiguity as weight. - * - * @param result pipeline result - * @return the estimated position of the robot in the FCS and the estimated timestamp of this - * estimation. - */ - private Optional averageBestTargetsStrategy(PhotonPipelineResult result) { - if (!result.hasTargets()) - return Optional.empty(); - final PhotonTrackedTarget target = result.getBestTarget(); - int targetFiducialId = target.getFiducialId(); - - // Don't report errors for non-fiducial targets. This could also be resolved by - // adding -1 to - // the initial HashSet. - if (targetFiducialId == -1) return Optional.empty(); - - Optional targetPosition = fieldTags.getTagPose(target.getFiducialId()); - - if (targetPosition.isEmpty()) { - reportFiducialPoseError(targetFiducialId); - return Optional.empty(); - } - - final Pose3d cameraPose = targetPosition.get().transformBy(target.getBestCameraToTarget().inverse()); -// Logger.recordOutput(camera.getName(), Math.toDegrees(cameraPose.getRotation().getY())); - final Pose3d robotPose = cameraPose.transformBy(robotToCamera.inverse()); - - return Optional.of( - new EstimatedRobotPose( - robotPose, - result.getTimestampSeconds(), - result.getTargets(), - PoseStrategy.AVERAGE_BEST_TARGETS - ) - ); - } - - /** - * Difference is defined as the vector magnitude between the two poses - * - * @return The absolute "difference" (>=0) between two Pose3ds. - */ - private double calculateDifference(Pose3d x, Pose3d y) { - return x.getTranslation().getDistance(y.getTranslation()); - } - - private void reportFiducialPoseError(int fiducialId) { - if (!reportedErrors.contains(fiducialId)) { - DriverStation.reportError( - "[PhotonPoseEstimator] Tried to get pose of unknown AprilTag: " + fiducialId, false); - reportedErrors.add(fiducialId); - } - } -} 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 1cc1deec..64f939ba 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator.java @@ -1,19 +1,15 @@ package frc.trigon.robot.poseestimation.poseestimator; import com.pathplanner.lib.util.PathPlannerLogging; -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.kinematics.SwerveDriveWheelPositions; -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 frc.trigon.robot.RobotContainer; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSource; -import frc.trigon.robot.poseestimation.robotposesources.RobotPoseSourceConstants; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.poseestimation.apriltagcamera.AprilTagCamera; import org.littletonrobotics.junction.Logger; import java.util.ArrayList; @@ -26,16 +22,16 @@ */ public class PoseEstimator implements AutoCloseable { private final Field2d field = new Field2d(); - private final RobotPoseSource[] robotPoseSources; + private final AprilTagCamera[] robotPoseSources; private final PoseEstimator6328 poseEstimator6328 = PoseEstimator6328.getInstance(); /** * Constructs a new PoseEstimator. * - * @param robotPoseSources the sources that should update the pose estimator apart from the odometry. This should be cameras etc. + * @param aprilTagCameras the sources that should update the pose estimator apart from the odometry. This should be cameras etc. */ - public PoseEstimator(RobotPoseSource... robotPoseSources) { - this.robotPoseSources = robotPoseSources; + public PoseEstimator(AprilTagCamera... aprilTagCameras) { + this.robotPoseSources = aprilTagCameras; putAprilTagsOnFieldWidget(); SmartDashboard.putData("Field", field); PathPlannerLogging.setLogActivePathCallback((pose) -> { @@ -92,38 +88,31 @@ private void updateFromVision() { private List getViableVisionObservations() { List viableVisionObservations = new ArrayList<>(); - for (RobotPoseSource robotPoseSource : robotPoseSources) { - final PoseEstimator6328.VisionObservation visionObservation = getVisionObservation(robotPoseSource); + for (AprilTagCamera aprilTagCamera : robotPoseSources) { + final PoseEstimator6328.VisionObservation visionObservation = getVisionObservation(aprilTagCamera); if (visionObservation != null) viableVisionObservations.add(visionObservation); } return viableVisionObservations; } - private PoseEstimator6328.VisionObservation getVisionObservation(RobotPoseSource robotPoseSource) { - robotPoseSource.update(); - if (!robotPoseSource.hasNewResult()) + private PoseEstimator6328.VisionObservation getVisionObservation(AprilTagCamera aprilTagCamera) { + aprilTagCamera.update(); + if (!aprilTagCamera.hasNewResult()) return null; - final Pose2d robotPose = robotPoseSource.getRobotPose(); + final Pose2d robotPose = aprilTagCamera.getEstimatedRobotPose(); if (robotPose == null) return null; return new PoseEstimator6328.VisionObservation( robotPose, - robotPoseSource.getLastResultTimestamp(), - averageDistanceToStdDevs(robotPoseSource.getAverageDistanceFromTags(), robotPoseSource.getVisibleTags()) + aprilTagCamera.getLatestResultTimestampSeconds(), + aprilTagCamera.getStandardDeviations() ); } - private Matrix averageDistanceToStdDevs(double averageDistance, int visibleTags) { - final double translationStd = PoseEstimatorConstants.TRANSLATIONS_STD_EXPONENT * Math.pow(averageDistance, 2) / (visibleTags * visibleTags); - final double thetaStd = PoseEstimatorConstants.THETA_STD_EXPONENT * Math.pow(averageDistance, 2) / visibleTags; - - return VecBuilder.fill(translationStd, translationStd, thetaStd); - } - private void putAprilTagsOnFieldWidget() { - for (Map.Entry entry : RobotPoseSourceConstants.TAG_ID_TO_POSE.entrySet()) { + for (Map.Entry entry : FieldConstants.TAG_ID_TO_POSE.entrySet()) { final Pose2d tagPose = entry.getValue().toPose2d(); field.getObject("Tag " + entry.getKey()).setPose(tagPose); } diff --git a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java index 6819d8ea..29c1f8df 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimator6328.java @@ -156,6 +156,13 @@ public void resetPose(Pose2d initialPose) { poseBuffer.clear(); } + public Pose2d samplePose(double timestamp) { + var sample = poseBuffer.getSample(timestamp); + var odometryToSampleTransform = new Transform2d(odometryPose, sample.get()); + // get old estimate by applying odometryToSample Transform + return estimatedPose.plus(odometryToSampleTransform); + } + @AutoLogOutput(key = "Poses/Robot/EstimatedPose") public Pose2d getEstimatedPose() { return estimatedPose; 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 a87aa90e..d6e27e57 100644 --- a/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java +++ b/src/main/java/frc/trigon/robot/poseestimation/poseestimator/PoseEstimatorConstants.java @@ -15,7 +15,7 @@ public class PoseEstimatorConstants { */ static final Vector ODOMETRY_AMBIGUITY = VecBuilder.fill(0.003, 0.003, 0.0002); - static final double + public static final double TRANSLATIONS_STD_EXPONENT = 0.005, THETA_STD_EXPONENT = 0.01; } diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java deleted file mode 100644 index aecd3b22..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagLimelightIO.java +++ /dev/null @@ -1,21 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import org.trigon.utilities.LimelightHelpers; - -public class AprilTagLimelightIO extends RobotPoseSourceIO { - private final String hostname; - - protected AprilTagLimelightIO(String hostname) { - this.hostname = hostname; - } - - @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - inputs.hasResult = LimelightHelpers.getTV(hostname); - if (inputs.hasResult) { - final LimelightHelpers.Results results = LimelightHelpers.getLatestResults(hostname).targetingResults; - inputs.cameraPose = results.getBotPose3d_wpiBlue(); - inputs.lastResultTimestamp = results.timestamp_LIMELIGHT_publish; - } - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java deleted file mode 100644 index 3e40add5..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/AprilTagPhotonCameraIO.java +++ /dev/null @@ -1,91 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; -import frc.trigon.robot.poseestimation.photonposeestimator.EstimatedRobotPose; -import frc.trigon.robot.poseestimation.photonposeestimator.PhotonPoseEstimator; -import org.littletonrobotics.junction.Logger; -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -import java.util.List; -import java.util.Optional; - -public class AprilTagPhotonCameraIO extends RobotPoseSourceIO { - private final PhotonCamera photonCamera; - private final PhotonPoseEstimator photonPoseEstimator; - - protected AprilTagPhotonCameraIO(String cameraName, Transform3d robotCenterToCamera) { - photonCamera = new PhotonCamera(cameraName); - - photonPoseEstimator = new PhotonPoseEstimator( - RobotPoseSourceConstants.APRIL_TAG_FIELD_LAYOUT, - RobotPoseSourceConstants.PRIMARY_POSE_STRATEGY, - photonCamera, - robotCenterToCamera - ); - - photonPoseEstimator.setMultiTagFallbackStrategy(RobotPoseSourceConstants.SECONDARY_POSE_STRATEGY); - } - - @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - final PhotonPipelineResult latestResult = photonCamera.getLatestResult(); - Optional optionalEstimatedRobotPose = photonPoseEstimator.update(latestResult); - - inputs.hasResult = hasResult(optionalEstimatedRobotPose); - if (inputs.hasResult) { - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - inputs.cameraPose = estimatedRobotPose.estimatedPose; - inputs.lastResultTimestamp = estimatedRobotPose.timestampSeconds; - inputs.visibleTags = estimatedRobotPose.targetsUsed.size(); - inputs.averageDistanceFromTags = getAverageDistanceFromTags(latestResult); - } else { - inputs.visibleTags = 0; - inputs.cameraPose = new Pose3d(); - } - - logVisibleTags(inputs.hasResult, optionalEstimatedRobotPose); - } - - private void logVisibleTags(boolean hasResult, Optional optionalEstimatedRobotPose) { - if (!hasResult) { - Logger.recordOutput("VisibleTags/" + photonCamera.getName(), new Pose2d[0]); - return; - } - - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - final Pose2d[] visibleTagPoses = new Pose2d[estimatedRobotPose.targetsUsed.size()]; - for (int i = 0; i < visibleTagPoses.length; i++) { - final int currentId = estimatedRobotPose.targetsUsed.get(i).getFiducialId(); - final Pose2d currentPose = RobotPoseSourceConstants.TAG_ID_TO_POSE.get(currentId).toPose2d(); - visibleTagPoses[i] = currentPose; - } - Logger.recordOutput("VisibleTags/" + photonCamera.getName(), visibleTagPoses); - } - - private boolean hasResult(Optional optionalEstimatedRobotPose) { - final boolean isEmpty = optionalEstimatedRobotPose.isEmpty(); - if (isEmpty) - return false; - final EstimatedRobotPose estimatedRobotPose = optionalEstimatedRobotPose.get(); - if (estimatedRobotPose.strategy == PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR) - return true; - return estimatedRobotPose.targetsUsed.get(0).getPoseAmbiguity() < RobotPoseSourceConstants.MAXIMUM_AMBIGUITY; - } - - private double getAverageDistanceFromTags(PhotonPipelineResult result) { - final List targets = result.targets; - double distanceSum = 0; - - for (PhotonTrackedTarget currentTarget : targets) { - final Translation2d distanceTranslation = currentTarget.getBestCameraToTarget().getTranslation().toTranslation2d(); - distanceSum += distanceTranslation.getNorm(); - } - - return distanceSum / targets.size(); - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java deleted file mode 100644 index 28a3fbd8..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSource.java +++ /dev/null @@ -1,82 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Transform3d; -import frc.trigon.robot.Robot; -import org.littletonrobotics.junction.Logger; - -/** - * A pose source is a class that provides the robot's pose, from a camera. - */ -public class RobotPoseSource { - protected final String name; - private final RobotPoseSourceInputsAutoLogged inputs = new RobotPoseSourceInputsAutoLogged(); - private final Transform3d robotCenterToCamera; - private final RobotPoseSourceIO robotPoseSourceIO; - private double lastUpdatedTimestamp; - private Pose2d cachedPose = null; - - public RobotPoseSource(RobotPoseSourceConstants.RobotPoseSourceType robotPoseSourceType, String name, Transform3d robotCenterToCamera) { - this.name = name; - if (robotPoseSourceType != RobotPoseSourceConstants.RobotPoseSourceType.PHOTON_CAMERA) - this.robotCenterToCamera = robotCenterToCamera; - else - this.robotCenterToCamera = new Transform3d(); - - if (Robot.IS_REAL) - robotPoseSourceIO = robotPoseSourceType.createIOFunction.apply(name, robotCenterToCamera); - else - robotPoseSourceIO = new RobotPoseSourceIO(); - } - - public void update() { - robotPoseSourceIO.updateInputs(inputs); - Logger.processInputs("Cameras/" + name, inputs); - cachedPose = getUnCachedRobotPose(); - if (!inputs.hasResult || inputs.averageDistanceFromTags == 0 || cachedPose == null) - Logger.recordOutput("Poses/Robot/" + name + "Pose", RobotPoseSourceConstants.EMPTY_POSE_LIST); - else - Logger.recordOutput("Poses/Robot/" + name + "Pose", cachedPose); - } - - public int getVisibleTags() { - return inputs.visibleTags; - } - - public double getAverageDistanceFromTags() { - return inputs.averageDistanceFromTags; - } - - public boolean hasNewResult() { - return (inputs.hasResult && inputs.averageDistanceFromTags != 0) && isNewTimestamp(); - } - - public Pose2d getRobotPose() { - return cachedPose; - } - - public String getName() { - return name; - } - - public double getLastResultTimestamp() { - return inputs.lastResultTimestamp; - } - - private Pose2d getUnCachedRobotPose() { - final Pose3d cameraPose = inputs.cameraPose; - if (cameraPose == null) - return null; - - return cameraPose.transformBy(robotCenterToCamera.inverse()).toPose2d(); - } - - private boolean isNewTimestamp() { - if (lastUpdatedTimestamp == getLastResultTimestamp()) - return false; - - lastUpdatedTimestamp = getLastResultTimestamp(); - return true; - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java deleted file mode 100644 index 8303aae5..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceConstants.java +++ /dev/null @@ -1,40 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.apriltag.AprilTag; -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 edu.wpi.first.math.geometry.Transform3d; -import frc.trigon.robot.poseestimation.photonposeestimator.PhotonPoseEstimator; - -import java.util.HashMap; -import java.util.function.BiFunction; - -public class RobotPoseSourceConstants { - public static final HashMap TAG_ID_TO_POSE = new HashMap<>(); - static final PhotonPoseEstimator.PoseStrategy - PRIMARY_POSE_STRATEGY = PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - SECONDARY_POSE_STRATEGY = PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_HEADING; - static AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); - - static { - for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags()) - TAG_ID_TO_POSE.put(aprilTag.ID, aprilTag.pose); - } - - static final double MAXIMUM_AMBIGUITY = 0.2; - static final Pose2d[] EMPTY_POSE_LIST = new Pose2d[0]; - - public enum RobotPoseSourceType { - PHOTON_CAMERA(AprilTagPhotonCameraIO::new), - LIMELIGHT((name, transform3d) -> new AprilTagLimelightIO(name)), - T265((name, transform3d) -> new T265IO(name)); - - final BiFunction createIOFunction; - - RobotPoseSourceType(BiFunction createIOFunction) { - this.createIOFunction = createIOFunction; - } - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java deleted file mode 100644 index a37f35d6..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/RobotPoseSourceIO.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.Pose3d; -import org.littletonrobotics.junction.AutoLog; - -public class RobotPoseSourceIO { - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - } - - @AutoLog - public static class RobotPoseSourceInputs { - public boolean hasResult = false; - public double lastResultTimestamp = 0; - public Pose3d cameraPose = new Pose3d(); - public double averageDistanceFromTags = 0; - public int visibleTags = 0; - } -} diff --git a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java b/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java deleted file mode 100644 index 8bc6ea0d..00000000 --- a/src/main/java/frc/trigon/robot/poseestimation/robotposesources/T265IO.java +++ /dev/null @@ -1,76 +0,0 @@ -package frc.trigon.robot.poseestimation.robotposesources; - -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import org.trigon.utilities.JsonHandler; - -public class T265IO extends RobotPoseSourceIO { - private static final NetworkTable NETWORK_TABLE = NetworkTableInstance.getDefault().getTable("T265"); - private static final short CONFIDENCE_THRESHOLD = 2; - private final NetworkTableEntry jsonDump; - - protected T265IO(String name) { - jsonDump = NETWORK_TABLE.getEntry(name + "/jsonDump"); - } - - @Override - protected void updateInputs(RobotPoseSourceInputsAutoLogged inputs) { - inputs.hasResult = canUseJsonDump(); - if (inputs.hasResult) - inputs.cameraPose = getCameraPose(); - inputs.lastResultTimestamp = (double) jsonDump.getLastChange() / 1000000; - } - - private Pose3d getCameraPose() { - if (!canUseJsonDump()) - return null; - - return getRobotPoseFromJsonDump(); - } - - private Pose3d getRobotPoseFromJsonDump() { - final T265JsonDump jsonDump = getJsonDump(); - final Translation3d translation = getTranslationFromDoubleArray(jsonDump.translation); - final Rotation3d rotation = getRotationFromDoubleArray(jsonDump.rotation); - - return t265PoseToWPIPose(new Pose3d(translation, rotation)); - } - - private Pose3d t265PoseToWPIPose(Pose3d t265Pose) { - final CoordinateSystem eusCoordinateSystem = new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.U(), CoordinateAxis.S()); - final Pose3d convertedPose = CoordinateSystem.convert(t265Pose, eusCoordinateSystem, CoordinateSystem.NWU()); - final Rotation3d convertedRotation = convertedPose.getRotation().plus(new Rotation3d(0, 0, Math.toRadians(90))); - - return new Pose3d(convertedPose.getTranslation(), convertedRotation); - } - - private Translation3d getTranslationFromDoubleArray(double[] xyz) { - return new Translation3d(xyz[0], xyz[1], xyz[2]); - } - - private Rotation3d getRotationFromDoubleArray(double[] wxyz) { - return new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3])); - } - - private boolean canUseJsonDump() { - final T265JsonDump jsonDump = getJsonDump(); - - try { - return jsonDump.confidence >= CONFIDENCE_THRESHOLD && jsonDump.translation.length == 3 && jsonDump.rotation.length == 4; - } catch (NullPointerException e) { - return false; - } - } - - private T265JsonDump getJsonDump() { - return JsonHandler.parseJsonStringToObject(jsonDump.getString(""), T265JsonDump.class); - } - - private static class T265JsonDump { - private double[] translation; - private double[] rotation; - private int confidence; - } -} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java index a3d44b29..1738d9dd 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveConstants.java @@ -119,6 +119,6 @@ public abstract class SwerveConstants { GYRO.applyConfiguration(config); GYRO.setSimulationYawVelocitySupplier(SIMULATION_YAW_VELOCITY_SUPPLIER); - GYRO.registerThreadedSignal(Pigeon2Signal.YAW, Pigeon2Signal.ANGULAR_VELOCITY_Z_WORLD, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + GYRO.registerThreadedSignal(Pigeon2Signal.YAW, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); } } diff --git a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java index fbc2f0b7..e3c8d53d 100644 --- a/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/trigon/robot/subsystems/swerve/SwerveModule.java @@ -155,11 +155,14 @@ private void configureHardware(double offsetRotations) { } private void configureSignals() { - driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, TalonFXSignal.VELOCITY, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + driveMotor.registerSignal(TalonFXSignal.VELOCITY, 100); driveMotor.registerSignal(TalonFXSignal.TORQUE_CURRENT, 100); driveMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); - steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, TalonFXSignal.VELOCITY, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + driveMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); + + steerMotor.registerSignal(TalonFXSignal.VELOCITY, 100); steerMotor.registerSignal(TalonFXSignal.MOTOR_VOLTAGE, 100); + steerMotor.registerThreadedSignal(TalonFXSignal.POSITION, PoseEstimatorConstants.ODOMETRY_FREQUENCY_HERTZ); steerEncoder.registerSignal(CANcoderSignal.POSITION, 100); steerEncoder.registerSignal(CANcoderSignal.VELOCITY, 100); }