diff --git a/build.gradle b/build.gradle index f9d19a018..5ca8e439f 100644 --- a/build.gradle +++ b/build.gradle @@ -4,7 +4,7 @@ plugins { id "com.github.node-gradle.node" version "3.1.1" apply false id "edu.wpi.first.GradleJni" version "1.0.0" id "edu.wpi.first.GradleVsCode" version "1.1.0" - id "edu.wpi.first.NativeUtils" version "2023.10.0" apply false + id "edu.wpi.first.NativeUtils" version "2023.11.1" apply false id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2" id "org.hidetake.ssh" version "2.10.1" id 'edu.wpi.first.WpilibTools' version '1.0.0' diff --git a/photon-lib/build.gradle b/photon-lib/build.gradle index 631be2778..6bedea719 100644 --- a/photon-lib/build.gradle +++ b/photon-lib/build.gradle @@ -32,6 +32,8 @@ dependencies { implementation wpilibTools.deps.wpilibJava("hal") implementation wpilibTools.deps.wpilibJava("ntcore") implementation wpilibTools.deps.wpilibJava("wpilibj") + implementation wpilibTools.deps.wpilibJava("apriltag") + // Junit testImplementation("org.junit.jupiter:junit-jupiter-api:5.8.2") diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java index 7a4946ece..63152043d 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java @@ -280,7 +280,7 @@ public class PhotonCamera { prevHeartbeatValue = curHeartbeat; } - return ((now - prevHeartbeatChangeTime) > HEARBEAT_DEBOUNCE_SEC); + return ((now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC); } private void verifyVersion() { diff --git a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java index 772942c1f..daf2aefcd 100644 --- a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java @@ -24,7 +24,9 @@ package org.photonvision; +import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.Pair; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; @@ -33,7 +35,7 @@ import edu.wpi.first.wpilibj.DriverStation; import java.util.ArrayList; import java.util.HashSet; import java.util.List; -import java.util.Map; +import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; public class RobotPoseEstimator { @@ -58,7 +60,7 @@ public class RobotPoseEstimator { AVERAGE_BEST_TARGETS } - private Map aprilTags; + private AprilTagFieldLayout aprilTags; private PoseStrategy strategy; private ArrayList> cameras; private Pose3d lastPose; @@ -72,13 +74,14 @@ public class RobotPoseEstimator { *

Example: {@code

Map map = new HashMap<>();

map.put(1, new * Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is at (1.0,2.0,3.0) } * - * @param aprilTags A Map linking AprilTag IDs to Pose3ds with respect to the FIRST field. + * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with respect to the + * FIRST field. * @param strategy The strategy it should use to determine the best pose. * @param cameras An ArrayList of Pairs of PhotonCameras and their respective Transform3ds from * the center of the robot to the cameras. */ public RobotPoseEstimator( - Map aprilTags, + AprilTagFieldLayout aprilTags, PoseStrategy strategy, ArrayList> cameras) { this.aprilTags = aprilTags; @@ -91,7 +94,8 @@ public class RobotPoseEstimator { /** * Update the estimated pose using the selected strategy. * - * @return The updated estimated pose and the latency in milliseconds + * @return The updated estimated pose and the latency in milliseconds Estimated pose may be null + * if no targets were seen */ public Pair update() { if (cameras.isEmpty()) { @@ -123,18 +127,25 @@ public class RobotPoseEstimator { return pair; default: DriverStation.reportError("[RobotPoseEstimator] Invalid pose strategy!", false); - return Pair.of(lastPose, 0.); + return Pair.of(null, 0.); } } private Pair lowestAmbiguityStrategy() { - // Loop over each ambiguity of all the cameras int lowestAI = -1; int lowestAJ = -1; double lowestAmbiguityScore = 10; + ArrayList results = new ArrayList(cameras.size()); + + // Sample result from each camera for (int i = 0; i < cameras.size(); i++) { Pair p = cameras.get(i); - List targets = p.getFirst().getLatestResult().targets; + results.add(p.getFirst().getLatestResult()); + } + + // Loop over each ambiguity of all the cameras + for (int i = 0; i < cameras.size(); i++) { + List targets = results.get(i).targets; for (int j = 0; j < targets.size(); j++) { if (targets.get(j).getPoseAmbiguity() < lowestAmbiguityScore) { lowestAI = i; @@ -144,17 +155,17 @@ public class RobotPoseEstimator { } } - // No targets, return the last pose + // No targets, return null if (lowestAI == -1 || lowestAJ == -1) { - return Pair.of(lastPose, 0.); + return Pair.of(null, 0.); } // Pick the lowest and do the heavy calculations - PhotonTrackedTarget bestTarget = - cameras.get(lowestAI).getFirst().getLatestResult().targets.get(lowestAJ); + PhotonTrackedTarget bestTarget = results.get(lowestAI).targets.get(lowestAJ); // If the map doesn't contain the ID fail - if (!aprilTags.containsKey(bestTarget.getFiducialId())) { + var tmp = aprilTags.getTagPose(bestTarget.getFiducialId()); + if (tmp.isEmpty()) { if (!reportedErrors.contains(bestTarget.getFiducialId())) { DriverStation.reportError( "[RobotPoseEstimator] Tried to get pose of unknown April Tag: " @@ -165,26 +176,28 @@ public class RobotPoseEstimator { return Pair.of(lastPose, 0.); } + var tagPose = tmp.get(); + return Pair.of( - aprilTags - .get(bestTarget.getFiducialId()) + tagPose .transformBy(bestTarget.getBestCameraToTarget().inverse()) .transformBy(cameras.get(lowestAI).getSecond().inverse()), - cameras.get(lowestAI).getFirst().getLatestResult().getLatencyMillis()); + results.get(lowestAI).getLatencyMillis()); } private Pair closestToCameraHeightStrategy() { double smallestHeightDifference = 10e9; double mili = 0; - Pose3d pose = lastPose; + Pose3d pose = null; for (int i = 0; i < cameras.size(); i++) { Pair p = cameras.get(i); - List targets = p.getFirst().getLatestResult().targets; + var result = p.getFirst().getLatestResult(); + List targets = result.targets; for (int j = 0; j < targets.size(); j++) { PhotonTrackedTarget target = targets.get(j); - // If the map doesn't contain the ID fail - if (!aprilTags.containsKey(target.getFiducialId())) { + var tmp = aprilTags.getTagPose(target.getFiducialId()); + if (tmp.isEmpty()) { if (!reportedErrors.contains(target.getFiducialId())) { DriverStation.reportWarning( "[RobotPoseEstimator] Tried to get pose of unknown April Tag: " @@ -194,7 +207,7 @@ public class RobotPoseEstimator { } continue; } - Pose3d targetPose = aprilTags.get(target.getFiducialId()); + Pose3d targetPose = tmp.get(); double alternativeDifference = Math.abs( p.getSecond().getZ() @@ -205,16 +218,23 @@ public class RobotPoseEstimator { - targetPose.transformBy(target.getBestCameraToTarget().inverse()).getZ()); if (alternativeDifference < smallestHeightDifference) { smallestHeightDifference = alternativeDifference; - pose = targetPose.transformBy(target.getAlternateCameraToTarget().inverse()); - mili = p.getFirst().getLatestResult().getLatencyMillis(); + pose = + targetPose + .transformBy(target.getAlternateCameraToTarget().inverse()) + .transformBy(p.getSecond().inverse()); + mili = result.getLatencyMillis(); } if (bestDifference < smallestHeightDifference) { smallestHeightDifference = bestDifference; - pose = targetPose.transformBy(target.getBestCameraToTarget().inverse()); - mili = p.getFirst().getLatestResult().getLatencyMillis(); + pose = + targetPose + .transformBy(target.getBestCameraToTarget().inverse()) + .transformBy(p.getSecond().inverse()); + mili = result.getLatencyMillis(); } } } + return Pair.of(pose, mili); } @@ -223,18 +243,20 @@ public class RobotPoseEstimator { DriverStation.reportError( "[RobotPoseEstimator] Tried to use reference pose strategy without setting the reference!", false); - return Pair.of(lastPose, 0.); + return Pair.of(null, 0.); } double smallestDifference = 10e9; double mili = 0; - Pose3d pose = lastPose; + Pose3d pose = null; for (int i = 0; i < cameras.size(); i++) { Pair p = cameras.get(i); - List targets = p.getFirst().getLatestResult().targets; + var result = p.getFirst().getLatestResult(); + List targets = result.targets; for (int j = 0; j < targets.size(); j++) { PhotonTrackedTarget target = targets.get(j); // If the map doesn't contain the ID fail - if (!aprilTags.containsKey(target.getFiducialId())) { + var tmp = aprilTags.getTagPose(target.getFiducialId()); + if (tmp.isEmpty()) { if (!reportedErrors.contains(target.getFiducialId())) { DriverStation.reportWarning( "[RobotPoseEstimator] Tried to get pose of unknown April Tag: " @@ -244,26 +266,26 @@ public class RobotPoseEstimator { } continue; } - Pose3d targetPose = aprilTags.get(target.getFiducialId()); - double alternativeDifference = - Math.abs( - calculateDifference( - referencePose, - targetPose.transformBy(target.getAlternateCameraToTarget().inverse()))); - double bestDifference = - Math.abs( - calculateDifference( - referencePose, - targetPose.transformBy(target.getBestCameraToTarget().inverse()))); + Pose3d targetPose = tmp.get(); + Pose3d botBestPose = + targetPose + .transformBy(target.getAlternateCameraToTarget().inverse()) + .transformBy(p.getSecond().inverse()); + Pose3d botAltPose = + targetPose + .transformBy(target.getBestCameraToTarget().inverse()) + .transformBy(p.getSecond().inverse()); + double alternativeDifference = Math.abs(calculateDifference(referencePose, botAltPose)); + double bestDifference = Math.abs(calculateDifference(referencePose, botBestPose)); if (alternativeDifference < smallestDifference) { smallestDifference = alternativeDifference; - pose = targetPose.transformBy(target.getAlternateCameraToTarget().inverse()); - mili = p.getFirst().getLatestResult().getLatencyMillis(); + pose = botAltPose; + mili = result.getLatencyMillis(); } if (bestDifference < smallestDifference) { smallestDifference = bestDifference; - pose = targetPose.transformBy(target.getBestCameraToTarget().inverse()); - mili = p.getFirst().getLatestResult().getLatencyMillis(); + pose = botBestPose; + mili = result.getLatencyMillis(); } } } @@ -277,11 +299,13 @@ public class RobotPoseEstimator { double totalAmbiguity = 0; for (int i = 0; i < cameras.size(); i++) { Pair p = cameras.get(i); - List targets = p.getFirst().getLatestResult().targets; + var result = p.getFirst().getLatestResult(); + List targets = result.targets; for (int j = 0; j < targets.size(); j++) { PhotonTrackedTarget target = targets.get(j); // If the map doesn't contain the ID fail - if (!aprilTags.containsKey(target.getFiducialId())) { + var tmp = aprilTags.getTagPose(target.getFiducialId()); + if (tmp.isEmpty()) { if (!reportedErrors.contains(target.getFiducialId())) { DriverStation.reportWarning( "[RobotPoseEstimator] Tried to get pose of unknown April Tag: " @@ -291,20 +315,23 @@ public class RobotPoseEstimator { } continue; } - Pose3d targetPose = aprilTags.get(target.getFiducialId()); + Pose3d targetPose = tmp.get(); try { totalAmbiguity += 1. / target.getPoseAmbiguity(); } catch (ArithmeticException e) { // A total ambiguity of zero exists, using that pose instead!", return Pair.of( - targetPose.transformBy(target.getBestCameraToTarget().inverse()), - p.getFirst().getLatestResult().getLatencyMillis()); + targetPose + .transformBy(target.getBestCameraToTarget().inverse()) + .transformBy(p.getSecond().inverse()), + result.getLatencyMillis()); } tempPoses.add( Pair.of( - targetPose.transformBy(target.getBestCameraToTarget().inverse()), - Pair.of( - target.getPoseAmbiguity(), p.getFirst().getLatestResult().getLatencyMillis()))); + targetPose + .transformBy(target.getBestCameraToTarget().inverse()) + .transformBy(p.getSecond().inverse()), + Pair.of(target.getPoseAmbiguity(), result.getLatencyMillis()))); } } @@ -312,20 +339,25 @@ public class RobotPoseEstimator { Rotation3d rotation = new Rotation3d(); double latency = 0; - for (Pair> pair : tempPoses) { - try { - double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity; - transform = transform.plus(pair.getFirst().getTranslation().times(weight)); - rotation = rotation.plus(pair.getFirst().getRotation().times(weight)); - latency += pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well - } catch (ArithmeticException e) { - DriverStation.reportWarning( - "[RobotPoseEstimator] A total ambiguity of zero exists, using that pose instead!", - false); - return Pair.of(pair.getFirst(), pair.getSecond().getSecond()); + if (tempPoses.size() > 0) { + for (Pair> pair : tempPoses) { + try { + double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity; + transform = transform.plus(pair.getFirst().getTranslation().times(weight)); + rotation = rotation.plus(pair.getFirst().getRotation().times(weight)); + latency += + pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well + } catch (ArithmeticException e) { + DriverStation.reportWarning( + "[RobotPoseEstimator] A total ambiguity of zero exists, using that pose instead!", + false); + return Pair.of(pair.getFirst(), pair.getSecond().getSecond()); + } } + return Pair.of(new Pose3d(transform, rotation), latency); + } else { + return new Pair(null, -1.0); } - return Pair.of(new Pose3d(transform, rotation), latency); } /** @@ -338,12 +370,12 @@ public class RobotPoseEstimator { } /** @param aprilTags the aprilTags to set */ - public void setAprilTags(Map aprilTags) { + public void setAprilTags(AprilTagFieldLayout aprilTags) { this.aprilTags = aprilTags; } /** @return the aprilTags */ - public Map getAprilTags() { + public AprilTagFieldLayout getAprilTags() { return aprilTags; } @@ -371,6 +403,15 @@ public class RobotPoseEstimator { this.referencePose = referencePose; } + /** + * Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE + * + * @param referencePose the referencePose to set + */ + public void setReferencePose(Pose2d referencePose) { + this.referencePose = new Pose3d(referencePose); + } + /** * UPdate the stored last pose. Useful for setting the initial estimate with CLOSEST_TO_LAST_POSE * diff --git a/photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java index 2d7904079..4345a0be4 100644 --- a/photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/RobotPoseEstimatorTest.java @@ -26,21 +26,22 @@ package org.photonvision; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.hal.JNIWrapper; import edu.wpi.first.math.Pair; 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.geometry.Translation3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.net.WPINetJNI; import edu.wpi.first.networktables.NetworkTablesJNI; import edu.wpi.first.util.CombinedRuntimeLoader; import edu.wpi.first.util.WPIUtilJNI; import java.io.IOException; import java.util.ArrayList; -import java.util.HashMap; import java.util.List; -import java.util.Map; import org.junit.jupiter.api.BeforeAll; import org.junit.jupiter.api.Test; import org.photonvision.RobotPoseEstimator.PoseStrategy; @@ -49,6 +50,8 @@ import org.photonvision.targeting.PhotonTrackedTarget; import org.photonvision.targeting.TargetCorner; class RobotPoseEstimatorTest { + static AprilTagFieldLayout aprilTags; + @BeforeAll public static void init() { JNIWrapper.Helper.setExtractOnStaticLoad(false); @@ -63,14 +66,17 @@ class RobotPoseEstimatorTest { // TODO Auto-generated catch block e.printStackTrace(); } + + List atList = new ArrayList(2); + atList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d()))); + atList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d()))); + var fl = Units.feetToMeters(54.0); + var fw = Units.feetToMeters(27.0); + aprilTags = new AprilTagFieldLayout(atList, fl, fw); } @Test void testLowestAmbiguityStrategy() { - Map aprilTags = new HashMap<>(); - aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d())); - aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d())); - ArrayList> cameras = new ArrayList<>(); PhotonCameraInjector cameraOne = new PhotonCameraInjector(); @@ -143,10 +149,6 @@ class RobotPoseEstimatorTest { @Test void testClosestToCameraHeightStrategy() { - Map aprilTags = new HashMap<>(); - aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d())); - aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d())); - ArrayList> cameras = new ArrayList<>(); PhotonCameraInjector cameraOne = new PhotonCameraInjector(); @@ -214,15 +216,11 @@ class RobotPoseEstimatorTest { assertEquals(2, estimatedPose.getSecond()); assertEquals(4, pose.getX(), .01); assertEquals(4, pose.getY(), .01); - assertEquals(4, pose.getZ(), .01); + assertEquals(0, pose.getZ(), .01); } @Test void closestToReferencePoseStrategy() { - Map aprilTags = new HashMap<>(); - aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d())); - aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d())); - ArrayList> cameras = new ArrayList<>(); PhotonCameraInjector cameraOne = new PhotonCameraInjector(); @@ -296,10 +294,6 @@ class RobotPoseEstimatorTest { @Test void closestToLastPose() { - Map aprilTags = new HashMap<>(); - aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d())); - aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d())); - ArrayList> cameras = new ArrayList<>(); PhotonCameraInjector cameraOne = new PhotonCameraInjector(); @@ -428,10 +422,6 @@ class RobotPoseEstimatorTest { @Test void averageBestPoses() { - Map aprilTags = new HashMap<>(); - aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d())); - aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d())); - ArrayList> cameras = new ArrayList<>(); PhotonCameraInjector cameraOne = new PhotonCameraInjector();