diff --git a/build.gradle b/build.gradle index 6a43065..0ba81b5 100644 --- a/build.gradle +++ b/build.gradle @@ -1,3 +1,5 @@ +import org.gradle.api.tasks.testing.logging.TestExceptionFormat + plugins { id "java" id "edu.wpi.first.GradleRIO" version "2026.2.1" @@ -75,13 +77,21 @@ dependencies { nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) simulationRelease wpi.sim.enableRelease() + // JUnit 5 dependencies, used for unit testing testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + testImplementation 'org.mockito:mockito-core:5.12.0' + testImplementation 'org.mockito:mockito-junit-jupiter:5.12.0' } +// JUnit 5 configuration +// Run tests via the `./gradlew test` command test { useJUnitPlatform() systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' + testLogging { + exceptionFormat TestExceptionFormat.FULL + } } // Simulation configuration (e.g. environment variables). diff --git a/src/main/java/frc/robot/subsystems/VisionII.java b/src/main/java/frc/robot/subsystems/VisionII.java index 916e50a..3a0c936 100644 --- a/src/main/java/frc/robot/subsystems/VisionII.java +++ b/src/main/java/frc/robot/subsystems/VisionII.java @@ -2,7 +2,6 @@ import java.util.ArrayList; import java.util.List; -//import java.util.logging.Handler; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; @@ -11,7 +10,6 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; @@ -23,49 +21,48 @@ public class VisionII { private PhotonCamera leftCam, rightCam; - private final Translation3d RIGHT_ROBOT_TO_CAM_TRANS; - private final Rotation3d RIGHT_ROBOT_TO_CAM_ROT; - private final Transform3d RIGHT_ROBOT_TO_CAM; - - private final Translation3d LEFT_ROBOT_TO_CAM_TRANS; - private final Rotation3d LEFT_ROBOT_TO_CAM_ROT; - private final Transform3d LEFT_ROBOT_TO_CAM; - - private static PhotonPoseEstimator rightEstimator; - private static PhotonPoseEstimator leftEstimator; - - public VisionII(){ - leftCam = new PhotonCamera("2265-ironfish"); - rightCam = new PhotonCamera("2265-greenfish"); - - RIGHT_ROBOT_TO_CAM_TRANS = new Translation3d( + private final Translation3d RIGHT_ROBOT_TO_CAM_TRANS = new Translation3d( Units.inchesToMeters(11.248), Units.inchesToMeters(-8.818), Units.inchesToMeters(9)); - RIGHT_ROBOT_TO_CAM_ROT = new Rotation3d( + private final Rotation3d RIGHT_ROBOT_TO_CAM_ROT = new Rotation3d( 0, 0, 0); - RIGHT_ROBOT_TO_CAM = new Transform3d( + private final Transform3d RIGHT_ROBOT_TO_CAM = new Transform3d( RIGHT_ROBOT_TO_CAM_TRANS, RIGHT_ROBOT_TO_CAM_ROT ); - LEFT_ROBOT_TO_CAM_TRANS = new Translation3d( + private final Translation3d LEFT_ROBOT_TO_CAM_TRANS = new Translation3d( Units.inchesToMeters(11.248), Units.inchesToMeters(8.818), Units.inchesToMeters(9)); - LEFT_ROBOT_TO_CAM_ROT = new Rotation3d( + private final Rotation3d LEFT_ROBOT_TO_CAM_ROT = new Rotation3d( 0, 0, 0); - LEFT_ROBOT_TO_CAM = new Transform3d( + private final Transform3d LEFT_ROBOT_TO_CAM = new Transform3d( LEFT_ROBOT_TO_CAM_TRANS, LEFT_ROBOT_TO_CAM_ROT ); + private final PhotonPoseEstimator rightEstimator; + private final PhotonPoseEstimator leftEstimator; + + public VisionII() { + leftCam = new PhotonCamera("2265-ironfish"); + rightCam = new PhotonCamera("2265-greenfish"); rightEstimator = new PhotonPoseEstimator(TAG_LAYOUT, RIGHT_ROBOT_TO_CAM); - leftEstimator = new PhotonPoseEstimator(TAG_LAYOUT, LEFT_ROBOT_TO_CAM); + leftEstimator = new PhotonPoseEstimator(TAG_LAYOUT, LEFT_ROBOT_TO_CAM); + } + + // For testing + VisionII(PhotonCamera leftCam, PhotonCamera rightCam, PhotonPoseEstimator leftEstimator, PhotonPoseEstimator rightEstimator) { + this.leftCam = leftCam; + this.rightCam = rightCam; + this.leftEstimator = leftEstimator; + this.rightEstimator = rightEstimator; } public List getVisionUpdates(){ diff --git a/src/test/java/frc/robot/subsystems/VisionIITest.java b/src/test/java/frc/robot/subsystems/VisionIITest.java new file mode 100644 index 0000000..c949b0e --- /dev/null +++ b/src/test/java/frc/robot/subsystems/VisionIITest.java @@ -0,0 +1,147 @@ +package frc.robot.subsystems; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.mockito.Mockito.when; + +import java.util.List; +import java.util.Optional; + +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.extension.ExtendWith; +import org.mockito.Mock; +import org.mockito.junit.jupiter.MockitoExtension; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.targeting.PhotonPipelineResult; + +/** + * This is a test class for the VisionII subsystem. + * It demonstrates how to use JUnit and Mockito to write unit tests for your code. + * Unit tests are crucial for ensuring that individual components (or "units") of your software + * work as expected in isolation. + */ +// @ExtendWith(MockitoExtension.class) tells JUnit to use the Mockito extension. +// This extension automatically initializes any fields annotated with @Mock. +@ExtendWith(MockitoExtension.class) +public class VisionIITest { + + /** + * @Mock creates a mock object for the given class. + * Mocks are "fake" objects that we can control in our tests. + * Here, we are creating mocks for the dependencies of the VisionII class. + * This allows us to test VisionII without needing real cameras or estimators. + */ + @Mock + private PhotonCamera mockLeftCam; + @Mock + private PhotonCamera mockRightCam; + @Mock + private PhotonPoseEstimator mockLeftEstimator; + @Mock + private PhotonPoseEstimator mockRightEstimator; + + // These are mock objects that will be returned by our other mocks. + @Mock + private PhotonPipelineResult mockPipelineResult; + @Mock + private EstimatedRobotPose mockEstimatedPose; + + // This is the object we are testing. + private VisionII vision; + + /** + * The @BeforeEach annotation marks a method that should be run before each test method. + * This is a good place to set up the initial state for your tests, like + * initializing the object under test. + */ + @BeforeEach + void setUp() { + // We create a new VisionII instance before each test. + // We use a special constructor that allows us to "inject" our mock dependencies. + // This is a common pattern called "Dependency Injection" and is key for writing testable code. + vision = new VisionII(mockLeftCam, mockRightCam, mockLeftEstimator, mockRightEstimator); + } + + /** + * This is our first test case. The @Test annotation tells JUnit that this is a test method. + * A good test method name clearly describes what it is testing. + * This test verifies that getVisionUpdates correctly processes results from both cameras + * and returns the estimated poses. + */ + @Test + void getVisionUpdates_WhenCamerasHaveResults_ShouldReturnEstimatedPoses() { + // --- ARRANGE --- + // In the "Arrange" phase, we set up the state of our mocks. + // We are defining what should happen when methods are called on our mock objects. + + // We tell our mock cameras to return a list containing our mock pipeline result + // when getAllUnreadResults() is called. + when(mockLeftCam.getAllUnreadResults()).thenReturn(List.of(mockPipelineResult)); + when(mockRightCam.getAllUnreadResults()).thenReturn(List.of(mockPipelineResult)); + + // We tell our mock estimators to return an Optional containing our mock estimated pose + // when estimateCoprocMultiTagPose() is called with the mock pipeline result. + // The use of Optional.of() simulates the case where a pose is successfully estimated. + when(mockLeftEstimator.estimateCoprocMultiTagPose(mockPipelineResult)).thenReturn(Optional.of(mockEstimatedPose)); + when(mockRightEstimator.estimateCoprocMultiTagPose(mockPipelineResult)).thenReturn(Optional.of(mockEstimatedPose)); + + // --- ACT --- + // In the "Act" phase, we call the method we want to test. + List results = vision.getVisionUpdates(); + + // --- ASSERT --- + // In the "Assert" phase, we check if the result of the action is what we expected. + + // We expect to get two results (one from each camera). + assertEquals(2, results.size(), "Should have received two pose estimates"); + // We also check that the results are the same mock objects we arranged to be returned. + assertEquals(mockEstimatedPose, results.get(0), "The first pose should be the one from the right camera"); + assertEquals(mockEstimatedPose, results.get(1), "The second pose should be the one from the left camera"); + } + + /** + * This test case covers the scenario where the cameras have no new results. + * We expect the getVisionUpdates method to return an empty list. + */ + @Test + void getVisionUpdates_WhenCamerasHaveNoResults_ShouldReturnEmptyList() { + // --- ARRANGE --- + // We configure the mock cameras to return an empty list. + when(mockLeftCam.getAllUnreadResults()).thenReturn(List.of()); + when(mockRightCam.getAllUnreadResults()).thenReturn(List.of()); + + // --- ACT --- + List results = vision.getVisionUpdates(); + + // --- ASSERT --- + // We assert that the returned list is empty. + assertTrue(results.isEmpty(), "The list of poses should be empty"); + } + + /** + * This test case covers the scenario where the cameras have results, but the + * estimators are unable to calculate a pose (e.g., due to seeing ambiguous targets). + * In this case, the estimator returns an empty Optional. + */ + @Test + void getVisionUpdates_WhenEstimatorsReturnEmpty_ShouldReturnEmptyList() { + // --- ARRANGE --- + // The cameras have results... + when(mockLeftCam.getAllUnreadResults()).thenReturn(List.of(mockPipelineResult)); + when(mockRightCam.getAllUnreadResults()).thenReturn(List.of(mockPipelineResult)); + + // ...but the estimators cannot determine a pose. + when(mockLeftEstimator.estimateCoprocMultiTagPose(mockPipelineResult)).thenReturn(Optional.empty()); + when(mockRightEstimator.estimateCoprocMultiTagPose(mockPipelineResult)).thenReturn(Optional.empty()); + + // --- ACT --- + List results = vision.getVisionUpdates(); + + // --- ASSERT --- + // We assert that the returned list is empty because no poses were added. + assertTrue(results.isEmpty(), "The list of poses should be empty"); + } +}