mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
Sim Updates for 2023 (#512)
* WIP updating sim stuff for 2023 and pose3d's * vision system build fixups, but test not yet passing. * WIP Sim fixups and working on testcases * Still doesn't work, but closer * tests pass * removed C++ sim support * formatting update * adjusted target height above ground per review * Turns out its unused * missed example removal
This commit is contained in:
Chris Gerth
@@ -29,13 +29,19 @@ import static org.junit.jupiter.api.Assertions.assertFalse;
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Pose3d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Transform2d;
|
||||
import edu.wpi.first.math.geometry.Rotation3d;
|
||||
import edu.wpi.first.math.geometry.Transform3d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.geometry.Translation3d;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import java.util.List;
|
||||
import java.util.stream.Stream;
|
||||
import org.junit.jupiter.api.AfterAll;
|
||||
import org.junit.jupiter.api.Assertions;
|
||||
import org.junit.jupiter.api.BeforeAll;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.junit.jupiter.params.ParameterizedTest;
|
||||
import org.junit.jupiter.params.provider.Arguments;
|
||||
@@ -49,18 +55,30 @@ class SimVisionSystemTest {
|
||||
Assertions.assertDoesNotThrow(
|
||||
() -> {
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 320, 240, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(new Pose2d(), 0.0, 1.0, 1.0));
|
||||
new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 320, 240, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(new Pose3d(), 1.0, 1.0, 42));
|
||||
for (int loopIdx = 0; loopIdx < 100; loopIdx++) {
|
||||
sysUnderTest.processFrame(new Pose2d());
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
@BeforeAll
|
||||
public static void setUp() {
|
||||
// NT live for debug purposes
|
||||
NetworkTableInstance.getDefault().startServer();
|
||||
|
||||
// No version check for testing
|
||||
PhotonCamera.setVersionCheckEnabled(false);
|
||||
}
|
||||
|
||||
@AfterAll
|
||||
public static void shutDown() {}
|
||||
|
||||
// @ParameterizedTest
|
||||
// @ValueSource(doubles = {5, 10, 15, 20, 25, 30})
|
||||
// public void testDistanceAligned(double dist) {
|
||||
// final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
// final var targetPose = new Pose2d(new Translation2d(15.98, 0), new Rotation2d());
|
||||
// var sysUnderTest =
|
||||
// new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 320, 240, 0);
|
||||
// sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 1.0, 1.0));
|
||||
@@ -76,10 +94,10 @@ class SimVisionSystemTest {
|
||||
|
||||
@Test
|
||||
public void testVisibilityCupidShuffle() {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3.0));
|
||||
final var targetPose =
|
||||
new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI));
|
||||
var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3));
|
||||
|
||||
// To the right, to the right
|
||||
var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(-70));
|
||||
@@ -117,37 +135,40 @@ class SimVisionSystemTest {
|
||||
assertFalse(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
// now walk it by yourself
|
||||
sysUnderTest.moveCamera(
|
||||
new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180)), 0, 1.0);
|
||||
sysUnderTest.moveCamera(new Transform3d(new Translation3d(), new Rotation3d(0, 0, Math.PI)));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testNotVisibleVert1() {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3.0));
|
||||
final var targetPose =
|
||||
new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI));
|
||||
var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 3.0, 3));
|
||||
|
||||
PhotonCamera.setVersionCheckEnabled(false);
|
||||
var robotPose = new Pose2d(new Translation2d(5, 0), Rotation2d.fromDegrees(5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
sysUnderTest.moveCamera(new Transform2d(), 5000, 1.0); // vooop selfie stick
|
||||
sysUnderTest.moveCamera(
|
||||
new Transform3d(
|
||||
new Translation3d(0, 0, 5000), new Rotation3d(0, 0, Math.PI))); // vooop selfie stick
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertFalse(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testNotVisibleVert2() {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
final var targetPose =
|
||||
new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI));
|
||||
var robotToCamera =
|
||||
new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, Math.PI / 4, 0));
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 45.0, new Transform2d(), 1, 99999, 1234, 1234, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 3.0, 0.5, 0.5));
|
||||
new SimVisionSystem("Test", 80.0, robotToCamera.inverse(), 99999, 1234, 1234, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 3.0, 0.5, 1736));
|
||||
|
||||
var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(5));
|
||||
var robotPose = new Pose2d(new Translation2d(14.98, 0), Rotation2d.fromDegrees(5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
@@ -159,12 +180,12 @@ class SimVisionSystemTest {
|
||||
|
||||
@Test
|
||||
public void testNotVisibleTgtSize() {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 20.0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 0.25, 0.1));
|
||||
final var targetPose =
|
||||
new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI));
|
||||
var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 20.0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.1, 0.025, 24));
|
||||
|
||||
var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(5));
|
||||
var robotPose = new Pose2d(new Translation2d(12, 0), Rotation2d.fromDegrees(5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
@@ -175,12 +196,12 @@ class SimVisionSystemTest {
|
||||
|
||||
@Test
|
||||
public void testNotVisibleTooFarForLEDs() {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 10, 640, 480, 1.0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 0.25, 0.1));
|
||||
final var targetPose =
|
||||
new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI));
|
||||
var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 10, 640, 480, 1.0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 1.0, 0.25, 78));
|
||||
|
||||
var robotPose = new Pose2d(new Translation2d(28, 0), Rotation2d.fromDegrees(5));
|
||||
var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(5));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
assertTrue(sysUnderTest.cam.getLatestResult().hasTargets());
|
||||
|
||||
@@ -192,12 +213,12 @@ class SimVisionSystemTest {
|
||||
@ParameterizedTest
|
||||
@ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23})
|
||||
public void testYawAngles(double testYaw) {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d(Math.PI / 4));
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 1, 99999, 640, 480, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 0.5, 0.5));
|
||||
final var targetPose =
|
||||
new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, 3 * Math.PI / 4));
|
||||
var sysUnderTest = new SimVisionSystem("Test", 80.0, new Transform3d(), 99999, 640, 480, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 3));
|
||||
|
||||
var robotPose = new Pose2d(new Translation2d(32, 0), Rotation2d.fromDegrees(testYaw));
|
||||
var robotPose = new Pose2d(new Translation2d(10, 0), Rotation2d.fromDegrees(-1.0 * testYaw));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
var res = sysUnderTest.cam.getLatestResult();
|
||||
assertTrue(res.hasTargets());
|
||||
@@ -208,45 +229,48 @@ class SimVisionSystemTest {
|
||||
@ParameterizedTest
|
||||
@ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999})
|
||||
public void testCameraPitch(double testPitch) {
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d(Math.PI / 4));
|
||||
final var robotPose = new Pose2d(new Translation2d(30, 0), new Rotation2d(0));
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 80.0, 0.0, new Transform2d(), 0.0, 99999, 640, 480, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.0, 0.5, 0.5));
|
||||
final var targetPose =
|
||||
new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, 3 * Math.PI / 4));
|
||||
final var robotPose = new Pose2d(new Translation2d(10, 0), new Rotation2d(0));
|
||||
var sysUnderTest = new SimVisionSystem("Test", 120.0, new Transform3d(), 99999, 640, 480, 0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 23));
|
||||
|
||||
sysUnderTest.moveCamera(new Transform2d(), 0.0, testPitch);
|
||||
// Here, passing in a positive testPitch points the camera downward (since moveCamera takes the
|
||||
// camera->robot transform)
|
||||
sysUnderTest.moveCamera(
|
||||
new Transform3d(
|
||||
new Translation3d(), new Rotation3d(0, Units.degreesToRadians(testPitch), 0)));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
var res = sysUnderTest.cam.getLatestResult();
|
||||
assertTrue(res.hasTargets());
|
||||
var tgt = res.getBestTarget();
|
||||
// If the camera is pitched down by 10 degrees, the target should appear
|
||||
// in the upper part of the image (ie, pitch positive). Therefor,
|
||||
// pass/fail involves -1.0.
|
||||
assertEquals(tgt.getPitch(), -testPitch, 0.0001);
|
||||
|
||||
// Since the camera is level with the target, a downward point will mean the target is in the
|
||||
// upper half of the image
|
||||
// which should produce positive pitch.
|
||||
assertEquals(testPitch, tgt.getPitch(), 0.0001);
|
||||
}
|
||||
|
||||
private static Stream<Arguments> distCalCParamProvider() {
|
||||
// Arbitrary and fairly random assortment of distances, camera pitches, and heights
|
||||
return Stream.of(
|
||||
Arguments.of(5, 35, 0),
|
||||
Arguments.of(6, 35, 1),
|
||||
Arguments.of(10, 35, 0),
|
||||
Arguments.of(15, 35, 2),
|
||||
Arguments.of(19.95, 35, 0),
|
||||
Arguments.of(20, 35, 0),
|
||||
Arguments.of(5, 15.98, 0),
|
||||
Arguments.of(6, 15.98, 1),
|
||||
Arguments.of(10, 15.98, 0),
|
||||
Arguments.of(15, 15.98, 2),
|
||||
Arguments.of(19.95, 15.98, 0),
|
||||
Arguments.of(20, 15.98, 0),
|
||||
Arguments.of(5, 42, 1),
|
||||
Arguments.of(6, 42, 0),
|
||||
Arguments.of(10, 42, 2),
|
||||
Arguments.of(15, 42, 0.5),
|
||||
Arguments.of(19.42, 35, 0),
|
||||
Arguments.of(19.42, 15.98, 0),
|
||||
Arguments.of(20, 42, 0),
|
||||
Arguments.of(5, 55, 2),
|
||||
Arguments.of(6, 55, 0),
|
||||
Arguments.of(10, 54, 2.2),
|
||||
Arguments.of(15, 53, 0),
|
||||
Arguments.of(19.52, 35, 1.1),
|
||||
Arguments.of(20, 51, 2.87),
|
||||
Arguments.of(20, 55, 3));
|
||||
Arguments.of(19.52, 15.98, 1.1));
|
||||
}
|
||||
|
||||
@ParameterizedTest
|
||||
@@ -254,20 +278,25 @@ class SimVisionSystemTest {
|
||||
public void testDistanceCalc(double testDist, double testPitch, double testHeight) {
|
||||
// Assume dist along ground and tgt height the same. Iterate over other parameters.
|
||||
|
||||
final var targetPose = new Pose2d(new Translation2d(35, 0), new Rotation2d(Math.PI / 42));
|
||||
final var robotPose = new Pose2d(new Translation2d(35 - testDist, 0), new Rotation2d(0));
|
||||
final var targetPose =
|
||||
new Pose3d(new Translation3d(15.98, 0, 1), new Rotation3d(0, 0, Math.PI * 0.98));
|
||||
final var robotPose =
|
||||
new Pose3d(new Translation3d(15.98 - Units.feetToMeters(testDist), 0, 0), new Rotation3d());
|
||||
final var robotToCamera =
|
||||
new Transform3d(
|
||||
new Translation3d(0, 0, Units.feetToMeters(testHeight)),
|
||||
new Rotation3d(0, Units.degreesToRadians(testPitch), 0));
|
||||
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem(
|
||||
"absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!",
|
||||
160.0,
|
||||
testPitch,
|
||||
new Transform2d(),
|
||||
testHeight,
|
||||
robotToCamera.inverse(),
|
||||
99999,
|
||||
640,
|
||||
480,
|
||||
0);
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, testDist, 0.5, 0.5));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 0));
|
||||
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
var res = sysUnderTest.cam.getLatestResult();
|
||||
@@ -276,34 +305,102 @@ class SimVisionSystemTest {
|
||||
assertEquals(tgt.getYaw(), 0.0, 0.0001);
|
||||
double distMeas =
|
||||
PhotonUtils.calculateDistanceToTargetMeters(
|
||||
testHeight,
|
||||
testDist,
|
||||
robotToCamera.getZ(),
|
||||
targetPose.getZ(),
|
||||
Units.degreesToRadians(testPitch),
|
||||
Units.degreesToRadians(tgt.getPitch()));
|
||||
assertEquals(distMeas, testDist, 0.001);
|
||||
assertEquals(Units.feetToMeters(testDist), distMeas, 0.001);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testMultipleTargets() {
|
||||
final var targetPoseL = new Pose2d(new Translation2d(35, 2), new Rotation2d());
|
||||
final var targetPoseC = new Pose2d(new Translation2d(35, 0), new Rotation2d());
|
||||
final var targetPoseR = new Pose2d(new Translation2d(35, -2), new Rotation2d());
|
||||
var sysUnderTest =
|
||||
new SimVisionSystem("Test", 160.0, 0.0, new Transform2d(), 5.0, 99999, 640, 480, 20.0);
|
||||
final var targetPoseL =
|
||||
new Pose3d(new Translation3d(15.98, 2, 0), new Rotation3d(0, 0, Math.PI));
|
||||
final var targetPoseC =
|
||||
new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, Math.PI));
|
||||
final var targetPoseR =
|
||||
new Pose3d(new Translation3d(15.98, -2, 0), new Rotation3d(0, 0, Math.PI));
|
||||
var sysUnderTest = new SimVisionSystem("Test", 160.0, new Transform3d(), 99999, 640, 480, 20.0);
|
||||
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 0.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseC, 1.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseR, 2.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 3.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseC, 4.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseR, 5.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 6.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseC, 7.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 8.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseR, 9.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPoseL, 10.0, 0.25, 0.1));
|
||||
sysUnderTest.addSimVisionTarget(
|
||||
new SimVisionTarget(
|
||||
targetPoseL.transformBy(
|
||||
new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())),
|
||||
0.25,
|
||||
0.25,
|
||||
1));
|
||||
sysUnderTest.addSimVisionTarget(
|
||||
new SimVisionTarget(
|
||||
targetPoseC.transformBy(
|
||||
new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())),
|
||||
0.25,
|
||||
0.25,
|
||||
2));
|
||||
sysUnderTest.addSimVisionTarget(
|
||||
new SimVisionTarget(
|
||||
targetPoseR.transformBy(
|
||||
new Transform3d(new Translation3d(0, 0, 0.00), new Rotation3d())),
|
||||
0.25,
|
||||
0.25,
|
||||
3));
|
||||
sysUnderTest.addSimVisionTarget(
|
||||
new SimVisionTarget(
|
||||
targetPoseL.transformBy(
|
||||
new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())),
|
||||
0.25,
|
||||
0.25,
|
||||
4));
|
||||
sysUnderTest.addSimVisionTarget(
|
||||
new SimVisionTarget(
|
||||
targetPoseC.transformBy(
|
||||
new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())),
|
||||
0.25,
|
||||
0.25,
|
||||
5));
|
||||
sysUnderTest.addSimVisionTarget(
|
||||
new SimVisionTarget(
|
||||
targetPoseR.transformBy(
|
||||
new Transform3d(new Translation3d(0, 0, 1.00), new Rotation3d())),
|
||||
0.25,
|
||||
0.25,
|
||||
6));
|
||||
sysUnderTest.addSimVisionTarget(
|
||||
new SimVisionTarget(
|
||||
targetPoseL.transformBy(
|
||||
new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())),
|
||||
0.25,
|
||||
0.25,
|
||||
7));
|
||||
sysUnderTest.addSimVisionTarget(
|
||||
new SimVisionTarget(
|
||||
targetPoseC.transformBy(
|
||||
new Transform3d(new Translation3d(0, 0, 0.50), new Rotation3d())),
|
||||
0.25,
|
||||
0.25,
|
||||
8));
|
||||
sysUnderTest.addSimVisionTarget(
|
||||
new SimVisionTarget(
|
||||
targetPoseL.transformBy(
|
||||
new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())),
|
||||
0.25,
|
||||
0.25,
|
||||
9));
|
||||
sysUnderTest.addSimVisionTarget(
|
||||
new SimVisionTarget(
|
||||
targetPoseR.transformBy(
|
||||
new Transform3d(new Translation3d(0, 0, 0.75), new Rotation3d())),
|
||||
0.25,
|
||||
0.25,
|
||||
10));
|
||||
sysUnderTest.addSimVisionTarget(
|
||||
new SimVisionTarget(
|
||||
targetPoseL.transformBy(
|
||||
new Transform3d(new Translation3d(0, 0, 0.25), new Rotation3d())),
|
||||
0.25,
|
||||
0.25,
|
||||
11));
|
||||
|
||||
var robotPose = new Pose2d(new Translation2d(30, 0), Rotation2d.fromDegrees(5));
|
||||
var robotPose = new Pose2d(new Translation2d(6.0, 0), Rotation2d.fromDegrees(0.25));
|
||||
sysUnderTest.processFrame(robotPose);
|
||||
var res = sysUnderTest.cam.getLatestResult();
|
||||
assertTrue(res.hasTargets());
|
||||
|
||||
Reference in New Issue
Block a user