Skip to content

Commit

Permalink
fix tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@mcm001
mcm001 committed Sep 22, 2024
1 parent c510134 commit d020419
Show file tree
Hide file tree
Showing 6 changed files with 84 additions and 62 deletions.
2 changes: 1 addition & 1 deletion photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
8 changes: 8 additions & 0 deletions photon-lib/src/main/java/org/photonvision/simulation/VisionSystemSim.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Expand Down
59 changes: 28 additions & 31 deletions photon-lib/src/test/java/org/photonvision/PhotonCameraTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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();
// }
}
38 changes: 15 additions & 23 deletions photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,8 +64,8 @@ void testLowestAmbiguityStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
0,
0,
11 * 1000000,
1100000,
List.of(
new PhotonTrackedTarget(
3.0,
Expand Down Expand Up @@ -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());
Expand All @@ -150,8 +149,8 @@ void testClosestToCameraHeightStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
0,
0,
4000000,
1100000,
List.of(
new PhotonTrackedTarget(
3.0,
Expand Down Expand Up @@ -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,
Expand All @@ -240,8 +237,8 @@ void closestToReferencePoseStrategy() {
cameraOne.result =
new PhotonPipelineResult(
0,
0,
0,
17000000,
1100000,
List.of(
new PhotonTrackedTarget(
3.0,
Expand Down Expand Up @@ -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(
Expand All @@ -330,8 +326,8 @@ void closestToLastPose() {
cameraOne.result =
new PhotonPipelineResult(
0,
0,
0,
1000000,
1100000,
List.of(
new PhotonTrackedTarget(
3.0,
Expand Down Expand Up @@ -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(
Expand All @@ -412,8 +407,8 @@ void closestToLastPose() {
cameraOne.result =
new PhotonPipelineResult(
0,
0,
0,
7000000,
1100000,
List.of(
new PhotonTrackedTarget(
3.0,
Expand Down Expand Up @@ -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;
Expand All @@ -495,8 +489,8 @@ void cacheIsInvalidated() {
var result =
new PhotonPipelineResult(
0,
0,
0,
20000000,
1100000,
List.of(
new PhotonTrackedTarget(
3.0,
Expand All @@ -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(
Expand All @@ -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<EstimatedRobotPose> estimatedPose = estimator.update(cameraOne.result);
assertFalse(estimatedPose.isPresent());

Expand Down Expand Up @@ -563,8 +556,8 @@ void averageBestPoses() {
cameraOne.result =
new PhotonPipelineResult(
0,
0,
0,
20 * 1000000,
1100000,
List.of(
new PhotonTrackedTarget(
3.0,
Expand Down Expand Up @@ -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(
Expand Down
Loading

0 comments on commit d020419

Please sign in to comment.