Skip to content

Commit

Permalink
spotless
Browse files Browse the repository at this point in the history
  • Loading branch information
amquake committed Jul 24, 2023
1 parent 1e393d9 commit eb74a45
Showing 1 changed file with 7 additions and 22 deletions.
29 changes: 7 additions & 22 deletions photon-lib/src/test/java/org/photonvision/VisionSystemSimTest.java
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,6 @@
import edu.wpi.first.util.CombinedRuntimeLoader;
import edu.wpi.first.util.RuntimeLoader;
import edu.wpi.first.util.WPIUtilJNI;

import java.util.ArrayList;
import java.util.List;
import java.util.stream.Stream;
Expand All @@ -61,7 +60,6 @@
import org.junit.jupiter.params.provider.MethodSource;
import org.junit.jupiter.params.provider.ValueSource;
import org.opencv.core.Core;
import org.opencv.highgui.HighGui;
import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.estimation.TargetModel;
import org.photonvision.simulation.PhotonCameraSim;
Expand Down Expand Up @@ -506,36 +504,23 @@ public void testPoseEstimation() {
double fieldWidth = Units.feetToMeters(27.0);
AprilTagFieldLayout layout = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth);
PhotonPoseEstimator estimator =
new PhotonPoseEstimator(
layout,
PoseStrategy.MULTI_TAG_PNP,
camera,
new Transform3d());
new PhotonPoseEstimator(layout, PoseStrategy.MULTI_TAG_PNP, camera, new Transform3d());
Pose2d robotPose = new Pose2d(5, 1, Rotation2d.fromDegrees(5));

visionSysSim.addVisionTargets(
new VisionTargetSim(
tagList.get(0).pose,
TargetModel.kTag16h5,
0));

new VisionTargetSim(tagList.get(0).pose, TargetModel.kTag16h5, 0));

visionSysSim.update(robotPose);
Pose3d pose = estimator.update().get().estimatedPose;
assertEquals(5, pose.getX(), .01);
assertEquals(1, pose.getY(), .01);
assertEquals(0, pose.getZ(), .01);

visionSysSim.addVisionTargets(
new VisionTargetSim(
tagList.get(1).pose,
TargetModel.kTag16h5,
1));
new VisionTargetSim(tagList.get(1).pose, TargetModel.kTag16h5, 1));
visionSysSim.addVisionTargets(
new VisionTargetSim(
tagList.get(2).pose,
TargetModel.kTag16h5,
2));

new VisionTargetSim(tagList.get(2).pose, TargetModel.kTag16h5, 2));

visionSysSim.update(robotPose);
pose = estimator.update().get().estimatedPose;
assertEquals(5, pose.getX(), .01);
Expand Down

0 comments on commit eb74a45

Please sign in to comment.