From d020419872e30845d0420bcdbd00ab71e23fce31 Mon Sep 17 00:00:00 2001 From: Matt Morley Date: Sun, 22 Sep 2024 14:49:35 -0700 Subject: [PATCH] fix tests --- .../simulation/PhotonCameraSim.java | 2 +- .../simulation/VisionSystemSim.java | 8 +++ .../org/photonvision/PhotonCameraTest.java | 59 +++++++++---------- .../photonvision/PhotonPoseEstimatorTest.java | 38 +++++------- .../org/photonvision/VisionSystemSimTest.java | 25 ++++++++ .../targeting/PhotonPipelineResult.java | 14 ++--- 6 files changed, 84 insertions(+), 62 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java index f5ab57e5c1..137fdd0aff 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java @@ -68,7 +68,7 @@ public class PhotonCameraSim implements AutoCloseable { private final PhotonCamera cam; NTTopicSet ts = new NTTopicSet(); - private long heartbeatCounter = 0; + private long heartbeatCounter = 1; /** This simulated camera's {@link SimCameraProperties} */ public final SimCameraProperties prop; diff --git a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java index b61b939465..5aae41fedc 100644 --- a/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java +++ b/photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java @@ -395,6 +395,14 @@ public void update(Pose3d robotPoseMeters) { // process a PhotonPipelineResult with visible targets var camResult = camSim.process(latencyMillis, lateCameraPose, allTargets); // publish this info to NT at estimated timestamp of receive + System.out.println( + "Camera " + // + camSim.ts.bestTargetPosX.getTopic().getName() + + camSim.getCamera().getName() + + ": result at " + + timestampNT + + ":\n" + + camResult); camSim.submitProcessedFrame(camResult, timestampNT); // display debug results for (var target : camResult.getTargets()) { diff --git a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java index 085eacfcfb..d3eb6c1089 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java @@ -27,22 +27,17 @@ import edu.wpi.first.apriltag.jni.AprilTagJNI; import edu.wpi.first.cscore.CameraServerCvJNI; import edu.wpi.first.cscore.CameraServerJNI; -import edu.wpi.first.hal.HAL; import edu.wpi.first.hal.JNIWrapper; import edu.wpi.first.math.WPIMathJNI; import edu.wpi.first.net.WPINetJNI; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.NetworkTablesJNI; import edu.wpi.first.util.CombinedRuntimeLoader; import edu.wpi.first.util.WPIUtilJNI; -import edu.wpi.first.wpilibj.Timer; import java.io.IOException; import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.Test; import org.opencv.core.Core; import org.photonvision.common.dataflow.structures.Packet; -import org.photonvision.jni.PhotonTargetingJniLoader; -import org.photonvision.jni.TimeSyncServer; import org.photonvision.targeting.PhotonPipelineResult; class PhotonCameraTest { @@ -83,38 +78,40 @@ public void testEmpty() { }); } - @Test - public void testMeme() throws InterruptedException, IOException { - load_wpilib(); - PhotonTargetingJniLoader.load(); + // @Test + // public void testMeme() throws InterruptedException, IOException { + // load_wpilib(); + // PhotonTargetingJniLoader.load(); - HAL.initialize(500, 0); + // // HAL.initialize(500, 0); - NetworkTableInstance.getDefault().stopClient(); - NetworkTableInstance.getDefault().startServer(); + // NetworkTableInstance.getDefault().stopClient(); + // NetworkTableInstance.getDefault().startServer(); - var server = new TimeSyncServer(5810); - server.start(); + // var server = new TimeSyncServer(5810); + // server.start(); - var camera = new PhotonCamera("Arducam_OV9281_USB_Camera"); - PhotonCamera.setVersionCheckEnabled(false); + // var camera = new PhotonCamera("Arducam_OV9281_USB_Camera"); + // PhotonCamera.setVersionCheckEnabled(false); - for (int i = 0; i < 20; i++) { - Thread.sleep(100); + // for (int i = 0; i < 20; i++) { + // Thread.sleep(100); - var res = camera.getLatestResult(); - var captureTime = res.getTimestampSeconds(); - var now = Timer.getFPGATimestamp(); + // var res = camera.getLatestResult(); + // var captureTime = res.getTimestampSeconds(); + // var now = Timer.getFPGATimestamp(); - // expectTrue(captureTime < now); + // // expectTrue(captureTime < now); - System.out.println( - "sequence " - + res.metadata.sequenceID - + " image capture " - + captureTime - + " recieved at " - + res.getNtReceiveTimestampMicros() / 1e6); - } - } + // System.out.println( + // "sequence " + // + res.metadata.sequenceID + // + " image capture " + // + captureTime + // + " recieved at " + // + res.getNtReceiveTimestampMicros() / 1e6); + // } + + // server.stop(); + // } } diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index 9ae688e7ed..a96329e20d 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java @@ -64,8 +64,8 @@ void testLowestAmbiguityStrategy() { cameraOne.result = new PhotonPipelineResult( 0, - 0, - 0, + 11 * 1000000, + 1100000, List.of( new PhotonTrackedTarget( 3.0, @@ -130,7 +130,6 @@ void testLowestAmbiguityStrategy() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setReceiveTimestampMicros((long) (11 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, new Transform3d()); @@ -150,8 +149,8 @@ void testClosestToCameraHeightStrategy() { cameraOne.result = new PhotonPipelineResult( 0, - 0, - 0, + 4000000, + 1100000, List.of( new PhotonTrackedTarget( 3.0, @@ -217,8 +216,6 @@ void testClosestToCameraHeightStrategy() { new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setReceiveTimestampMicros((long) (4 * 1e6)); - PhotonPoseEstimator estimator = new PhotonPoseEstimator( aprilTags, @@ -240,8 +237,8 @@ void closestToReferencePoseStrategy() { cameraOne.result = new PhotonPipelineResult( 0, - 0, - 0, + 17000000, + 1100000, List.of( new PhotonTrackedTarget( 3.0, @@ -306,7 +303,6 @@ void closestToReferencePoseStrategy() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setReceiveTimestampMicros((long) (17 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -330,8 +326,8 @@ void closestToLastPose() { cameraOne.result = new PhotonPipelineResult( 0, - 0, - 0, + 1000000, + 1100000, List.of( new PhotonTrackedTarget( 3.0, @@ -396,7 +392,6 @@ void closestToLastPose() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -412,8 +407,8 @@ void closestToLastPose() { cameraOne.result = new PhotonPipelineResult( 0, - 0, - 0, + 7000000, + 1100000, List.of( new PhotonTrackedTarget( 3.0, @@ -478,7 +473,6 @@ void closestToLastPose() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - cameraOne.result.setReceiveTimestampMicros((long) (7 * 1e6)); estimatedPose = estimator.update(cameraOne.result); pose = estimatedPose.get().estimatedPose; @@ -495,8 +489,8 @@ void cacheIsInvalidated() { var result = new PhotonPipelineResult( 0, - 0, - 0, + 20000000, + 1100000, List.of( new PhotonTrackedTarget( 3.0, @@ -519,7 +513,6 @@ void cacheIsInvalidated() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); - result.setReceiveTimestampMicros((long) (20 * 1e6)); PhotonPoseEstimator estimator = new PhotonPoseEstimator( @@ -529,7 +522,7 @@ void cacheIsInvalidated() { // Empty result, expect empty result cameraOne.result = new PhotonPipelineResult(); - cameraOne.result.setReceiveTimestampMicros((long) (1 * 1e6)); + cameraOne.result.metadata.captureTimestampMicros = (long) (1 * 1e6); Optional estimatedPose = estimator.update(cameraOne.result); assertFalse(estimatedPose.isPresent()); @@ -563,8 +556,8 @@ void averageBestPoses() { cameraOne.result = new PhotonPipelineResult( 0, - 0, - 0, + 20 * 1000000, + 1100000, List.of( new PhotonTrackedTarget( 3.0, @@ -629,7 +622,6 @@ void averageBestPoses() { new TargetCorner(3, 4), new TargetCorner(5, 6), new TargetCorner(7, 8))))); // 3 3 3 ambig .4 - cameraOne.result.setReceiveTimestampMicros(20 * 1000000); PhotonPoseEstimator estimator = new PhotonPoseEstimator( diff --git a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java index 859aaf1cb6..0e460b3a7c 100644 --- a/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java +++ b/photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java @@ -77,6 +77,7 @@ public void testEmpty() { @BeforeAll public static void setUp() { // NT live for debug purposes + NetworkTableInstance.getDefault().stopClient(); NetworkTableInstance.getDefault().startServer(); // No version check for testing @@ -117,42 +118,50 @@ public void testVisibilityCupidShuffle() { // To the right, to the right var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-70)); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // To the right, to the right robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-95)); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // To the left, to the left robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(90)); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // To the left, to the left robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(65)); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // now kick, now kick robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); // now kick, now kick robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-5)); visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); // now walk it by yourself robotPose = new Pose2d(new Translation2d(2, 0), Rotation2d.fromDegrees(-179)); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); // now walk it by yourself visionSysSim.adjustCamera( cameraSim, new Transform3d(new Translation3d(), new Rotation3d(0, 0, Math.PI))); visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); } @@ -169,11 +178,13 @@ public void testNotVisibleVert1() { var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); visionSysSim.adjustCamera( // vooop selfie stick cameraSim, new Transform3d(new Translation3d(0, 0, 5000), new Rotation3d(0, 0, Math.PI))); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); } @@ -192,11 +203,13 @@ public void testNotVisibleVert2() { var robotPose = new Pose2d(new Translation2d(13.98, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); // Pitched back camera should mean target goes out of view below the robot as distance increases robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); } @@ -214,10 +227,12 @@ public void testNotVisibleTgtSize() { var robotPose = new Pose2d(new Translation2d(12, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); } @@ -236,10 +251,12 @@ public void testNotVisibleTooFarForLEDs() { var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); + assertTrue(camera.getLatestResult().hasTargets()); robotPose = new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(5)); visionSysSim.update(robotPose); + assertFalse(camera.getLatestResult().hasTargets()); } @@ -259,6 +276,7 @@ public void testYawAngles(double testYaw) { // If the robot is rotated x deg (CCW+), the target yaw should be x deg (CW+) var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(testYaw)); visionSysSim.update(robotPose); + var res = camera.getLatestResult(); assertTrue(res.hasTargets()); var tgt = res.getBestTarget(); @@ -284,8 +302,11 @@ public void testPitchAngles(double testPitch) { cameraSim, new Transform3d( new Translation3d(), new Rotation3d(0, Units.degreesToRadians(testPitch), 0))); + visionSysSim.update(robotPose); + var res = camera.getLatestResult(); + System.out.println("Got result: " + res); assertTrue(res.hasTargets()); var tgt = res.getBestTarget(); @@ -349,6 +370,7 @@ public void testDistanceCalc(double testDist, double testPitch, double testHeigh // 1. These are calculated with the average of the minimum area rectangle, which does not // actually find the target center because of perspective distortion. // 2. Yaw and pitch are calculated separately which gives incorrect pitch values. + var res = camera.getLatestResult(); assertTrue(res.hasTargets()); var tgt = res.getBestTarget(); @@ -450,6 +472,7 @@ public void testMultipleTargets() { var robotPose = new Pose2d(new Translation2d(6.0, 0), Rotation2d.fromDegrees(0.25)); visionSysSim.update(robotPose); + var res = camera.getLatestResult(); assertTrue(res.hasTargets()); List tgtList; @@ -479,6 +502,7 @@ public void testPoseEstimation() { new VisionTargetSim(tagList.get(0).pose, TargetModel.kAprilTag16h5, 0)); visionSysSim.update(robotPose); + var results = VisionEstimation.estimateCamPosePNP( camera.getCameraMatrix().get(), @@ -499,6 +523,7 @@ public void testPoseEstimation() { new VisionTargetSim(tagList.get(2).pose, TargetModel.kAprilTag16h5, 2)); visionSysSim.update(robotPose); + results = VisionEstimation.estimateCamPosePNP( camera.getCameraMatrix().get(), diff --git a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java index 3936e1470d..e2adb61d9e 100644 --- a/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java +++ b/photon-targeting/src/main/java/org/photonvision/targeting/PhotonPipelineResult.java @@ -52,19 +52,19 @@ public PhotonPipelineResult() { * Constructs a pipeline result. * * @param sequenceID The number of frames processed by this camera since boot - * @param captureTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor - * captured the image this result contains the targeting info of - * @param publishTimestamp The time, in uS in the coprocessor's timebase, that the coprocessor - * published targeting info + * @param captureTimestampMicros The time, in uS in the coprocessor's timebase, that the + * coprocessor captured the image this result contains the targeting info of + * @param publishTimestampMicros The time, in uS in the coprocessor's timebase, that the + * coprocessor published targeting info * @param targets The list of targets identified by the pipeline. */ public PhotonPipelineResult( long sequenceID, - long captureTimestamp, - long publishTimestamp, + long captureTimestampMicros, + long publishTimestampMicros, List targets) { this( - new PhotonPipelineMetadata(captureTimestamp, publishTimestamp, sequenceID), + new PhotonPipelineMetadata(captureTimestampMicros, publishTimestampMicros, sequenceID), targets, Optional.empty()); }