diff --git a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java index 01975444..dcbf4461 100644 --- a/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java +++ b/src/main/java/frc/team2412/robot/sensors/AprilTagsProcessor.java @@ -46,7 +46,7 @@ public class AprilTagsProcessor { Units.inchesToMeters(27.0 / 2.0 - 0.94996), 0, Units.inchesToMeters(8.12331), - new Rotation3d(0, Units.degreesToRadians(-30), 0)); + new Rotation3d(Units.degreesToRadians(90), Units.degreesToRadians(-30), 0)); // TODO Measure these private static final Vector STANDARD_DEVS = VecBuilder.fill(1, 1, Units.degreesToRadians(30)); @@ -61,14 +61,15 @@ private static PhotonPipelineResult copy(PhotonPipelineResult result) { private static final double MAX_POSE_AMBIGUITY = 0.1; - // Both of these are in radians - // Negate pitch to go from robot/target to cam pitch to cam to robot/target pitch - private static final double EXPECTED_CAM_TO_TARGET_PITCH = -ROBOT_TO_CAM.getRotation().getY(); - private static final double CAM_TO_TARGET_PITCH_TOLERANCE = 0.1; + // Radians + private static final double ROBOT_TO_TARGET_ROLL_TOLERANCE = 0.1; + private static final double ROBOT_TO_TARGET_PITCH_TOLERANCE = 0.1; - private static boolean hasCorrectPitch(Transform3d camToTarget) { - return Math.abs(camToTarget.getRotation().getY() - EXPECTED_CAM_TO_TARGET_PITCH) - < CAM_TO_TARGET_PITCH_TOLERANCE; + private static boolean hasValidRotation(Transform3d camToTarget) { + // Intrinsic robot to cam + cam to target = extrinsic cam to target + robot to cam + var robotToTargetRotation = camToTarget.getRotation().plus(ROBOT_TO_CAM.getRotation()); + return (Math.abs(robotToTargetRotation.getX()) < ROBOT_TO_TARGET_ROLL_TOLERANCE) + && (Math.abs(robotToTargetRotation.getY()) < ROBOT_TO_TARGET_PITCH_TOLERANCE); } private static PhotonTrackedTarget swapBestAndAltTransforms(PhotonTrackedTarget target) { @@ -93,8 +94,8 @@ private static PhotonPipelineResult filteredPipelineResult(PhotonPipelineResult copy.targets.remove(i); continue; } - if (!hasCorrectPitch(target.getBestCameraToTarget())) { - if (hasCorrectPitch(target.getAlternateCameraToTarget())) { + if (!hasValidRotation(target.getBestCameraToTarget())) { + if (hasValidRotation(target.getAlternateCameraToTarget())) { target = swapBestAndAltTransforms(target); copy.targets.set(i, target); } else { @@ -112,12 +113,13 @@ private static PhotonPipelineResult filteredPipelineResult(PhotonPipelineResult private final FieldObject2d rawVisionFieldObject; // These are always set with every pipeline result + private double lastRawTimestampSeconds = 0; private PhotonPipelineResult latestResult = null; private PhotonPipelineResult latestFilteredResult = null; private Optional latestPose = Optional.empty(); // These are only set when there's a valid pose - private double lastTimestampSeconds = 0; + private double lastValidTimestampSeconds = 0; private Pose2d lastFieldPose = new Pose2d(-1, -1, new Rotation2d()); private static final AprilTagFieldLayout fieldLayout = @@ -150,44 +152,70 @@ public AprilTagsProcessor(DrivebaseWrapper aprilTagsHelper) { event -> update()); ShuffleboardTab shuffleboardTab = Shuffleboard.getTab("AprilTags"); - shuffleboardTab.addBoolean("Has targets", this::hasTargets).withPosition(0, 0).withSize(1, 1); shuffleboardTab - .addInteger("Num targets", this::getNumTargets) + .addDouble("Last raw timestamp", this::getLastRawTimestampSeconds) + .withPosition(0, 0) + .withSize(1, 1); + shuffleboardTab + .addInteger("Raw num targets", this::getRawNumTargets) .withPosition(0, 1) .withSize(1, 1); shuffleboardTab - .addDouble("Last timestamp", this::getLastTimestampSeconds) + .addInteger("Filtered num targets", this::getFilteredNumTargets) + .withPosition(1, 1) + .withSize(1, 1); + shuffleboardTab + .addBoolean("Has valid targets", this::hasTargets) + .withPosition(1, 2) + .withSize(1, 1); + shuffleboardTab + .addDouble("Last timestamp", this::getLastValidTimestampSeconds) .withPosition(1, 0) .withSize(1, 1); shuffleboardTab .add("3d pose on field", new SendablePose3d(this::getRobotPose)) .withPosition(2, 0) - .withSize(1, 6); + .withSize(2, 2); } public void update() { latestResult = photonCamera.getLatestResult(); latestFilteredResult = filteredPipelineResult(latestResult); + lastRawTimestampSeconds = latestResult.getTimestampSeconds(); latestPose = photonPoseEstimator.update(latestFilteredResult); if (latestPose.isPresent()) { - lastTimestampSeconds = latestPose.get().timestampSeconds; + lastValidTimestampSeconds = latestPose.get().timestampSeconds; lastFieldPose = latestPose.get().estimatedPose.toPose2d(); rawVisionFieldObject.setPose(lastFieldPose); - aprilTagsHelper.addVisionMeasurement(lastFieldPose, lastTimestampSeconds, STANDARD_DEVS); + aprilTagsHelper.addVisionMeasurement(lastFieldPose, lastValidTimestampSeconds, STANDARD_DEVS); var estimatedPose = aprilTagsHelper.getEstimatedPosition(); aprilTagsHelper.getField().setRobotPose(estimatedPose); photonPoseEstimator.setLastPose(estimatedPose); } } - public boolean hasTargets() { - return latestPose.isPresent(); + /** + * Returns the timestamp of the last result we got (regardless of whether it has any valid + * targets) + * + * @return The timestamp of the last result we got in seconds since FPGA startup. + */ + public double getLastRawTimestampSeconds() { + return lastRawTimestampSeconds; } - public int getNumTargets() { + public int getRawNumTargets() { return latestResult == null ? -1 : latestResult.getTargets().size(); } + public int getFilteredNumTargets() { + return latestFilteredResult == null ? -1 : latestFilteredResult.getTargets().size(); + } + + public boolean hasTargets() { + return latestPose.isPresent(); + } + /** * Calculates the robot pose using the best target. Returns null if there is no known robot pose. * @@ -205,7 +233,7 @@ public Pose3d getRobotPose() { * * @return The time we last saw an AprilTag in seconds since FPGA startup. */ - public double getLastTimestampSeconds() { - return lastTimestampSeconds; + public double getLastValidTimestampSeconds() { + return lastValidTimestampSeconds; } } diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 46211fc1..77641e45 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.4", + "version": "v2024.2.9", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.4", + "version": "v2024.2.9", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.4", + "version": "v2024.2.9", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.4" + "version": "v2024.2.9" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.4" + "version": "v2024.2.9" } ] } \ No newline at end of file