diff --git a/src/main/java/frc/lib/util/GeomUtil.java b/src/main/java/frc/lib/util/GeomUtil.java index 633d9ae3..aee93800 100644 --- a/src/main/java/frc/lib/util/GeomUtil.java +++ b/src/main/java/frc/lib/util/GeomUtil.java @@ -7,14 +7,18 @@ package frc.lib.util; +import static edu.wpi.first.units.Units.Meters; +import edu.wpi.first.math.MathUtil; 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.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.Distance; /** Geometry utilities for working with translations, rotations, transforms, and poses. */ public class GeomUtil { @@ -174,4 +178,86 @@ public static Pose2d withRotation(Pose2d pose, Rotation2d rotation) { return new Pose2d(pose.getTranslation(), rotation); } + + /** + * Checks if two Pose3d objects are approximately equal within given distance and rotation + * tolerances. + * + *

+ * The comparison checks both the translation (x, y, z) and the rotation (roll, pitch, yaw) + * components individually against the specified tolerances. + * + * @param first The first pose to compare + * @param second The second pose to compare + * @param distanceEpsilon The maximum allowable difference in translation (in meters) + * @param rotationEpsilon The maximum allowable difference in rotation (in radians) + * @return True if the poses are near each other in both translation and rotation + */ + public static boolean isNear(Pose3d first, Pose3d second, Distance distanceEpsilon, + Rotation3d rotationEpsilon) + { + boolean positionNear = + MathUtil.isNear(first.getX(), second.getX(), distanceEpsilon.in(Meters)) + && MathUtil.isNear(first.getY(), second.getY(), distanceEpsilon.in(Meters)) + && MathUtil.isNear(first.getZ(), second.getZ(), distanceEpsilon.in(Meters)); + + boolean rotationNear = MathUtil.isNear(first.getRotation().getX(), + second.getRotation().getX(), rotationEpsilon.getX()) + && MathUtil.isNear(first.getRotation().getY(), second.getRotation().getY(), + rotationEpsilon.getY()) + && MathUtil.isNear(first.getRotation().getZ(), second.getRotation().getZ(), + rotationEpsilon.getZ()); + + return positionNear && rotationNear; + } + + /** + * Checks if two Transform3d objects are approximately equal within a given distance and + * rotation tolerance. + * + * @param first The first transform to compare + * @param second The second transform to compare + * @param distanceEpsilon The maximum allowable distance difference in meters + * @param rotationEpsilon The maximum allowable rotation difference + * @return True if the transforms are near each other in both translation and rotation + */ + public static boolean isNear(Transform3d first, Transform3d second, Distance distanceEpsilon, + Rotation3d rotationEpsilon) + { + return isNear(toPose3d(first), toPose3d(second), distanceEpsilon, rotationEpsilon); + } + + /** + * Checks if two Pose2d objects are approximately equal within a given distance and rotation + * tolerance. + * + * @param first The first pose to compare + * @param second The second pose to compare + * @param distanceEpsilon The maximum allowable distance difference in meters + * @param rotationEpsilon The maximum allowable rotation difference + * @return True if the poses are near each other in both translation and rotation + */ + public static boolean isNear(Pose2d first, Pose2d second, Distance distanceEpsilon, + Rotation2d rotationEpsilon) + { + return isNear(new Pose3d(first), new Pose3d(second), distanceEpsilon, + new Rotation3d(rotationEpsilon)); + } + + /** + * Checks if two Transform2d objects are approximately equal within a given distance and + * rotation tolerance. + * + * @param first The first transform to compare + * @param second The second transform to compare + * @param distanceEpsilon The maximum allowable distance difference in meters + * @param rotationEpsilon The maximum allowable rotation difference + * @return True if the transforms are near each other in both translation and rotation + */ + public static boolean isNear(Transform2d first, Transform2d second, Distance distanceEpsilon, + Rotation2d rotationEpsilon) + { + return isNear(toPose2d(first), toPose2d(second), distanceEpsilon, rotationEpsilon); + } + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f7722afc..91fd4417 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -150,7 +150,7 @@ public RobotContainer() servo1 = Servo1Constants.get(); objectDetector = ObjectDetectorConstants.get(); turret = TurretSuperstructureConstants.get(); - VisionConstants.create(); + VisionConstants.get(); conditionalChooser = new LoggedDashboardChooser<>("Conditional Choice"); conditionalChooser.addOption("True", true); diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 20a1c3a3..4dd89918 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -19,6 +19,7 @@ import static edu.wpi.first.units.Units.Seconds; import java.util.HashMap; import java.util.Optional; +import java.util.function.Consumer; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; @@ -65,6 +66,10 @@ public static record TrigPoseRecord(Pose2d pose, double distance, double timesta @Setter private ChassisSpeeds velocity = new ChassisSpeeds(); + @Setter + private Consumer visionObservationConsumer = + poseEstimator::addVisionObservation; + @AutoLogOutput(key = "Odometry/OdometryPose") public Pose2d getOdometryPose() { @@ -90,7 +95,7 @@ public void addOdometryObservation(OdometryObservation observation) public void addVisionObservation(VisionPoseObservation observation) { - poseEstimator.addVisionObservation(observation); + visionObservationConsumer.accept(observation); } public void addTrigPose(int tagId, TrigPoseRecord trigPose) diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index b5929496..37eff22c 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -166,24 +166,24 @@ private static VisionIOPhotonVisionSim getFrontRightIOSim() FieldConstants.APRILTAG_LAYOUT); } - public static void create() + public static VisionSubsystem get() { - switch (Constants.currentMode) { + return switch (Constants.currentMode) { case REAL -> { var camera1 = new AprilTagCamera(FRONT_LEFT, getFrontLeftIOReal()); var camera2 = new AprilTagCamera(FRONT_RIGHT, getFrontRightIOReal()); - new VisionSubsystem(camera1, camera2); + yield new VisionSubsystem(camera1, camera2); } case SIM -> { var camera1 = new AprilTagCamera(FRONT_LEFT, getFrontLeftIOSim()); var camera2 = new AprilTagCamera(FRONT_RIGHT, getFrontRightIOSim()); - new VisionSubsystem(camera1, camera2); + yield new VisionSubsystem(camera1, camera2); } case REPLAY -> { var camera1 = new AprilTagCamera(FRONT_LEFT, new VisionIO() {}); var camera2 = new AprilTagCamera(FRONT_RIGHT, new VisionIO() {}); - new VisionSubsystem(camera1, camera2); + yield new VisionSubsystem(camera1, camera2); } - } + }; } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 84c16fa1..d44b7cef 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -16,15 +16,23 @@ package frc.robot.subsystems.vision; import static edu.wpi.first.units.Units.Meters; + import java.util.ArrayList; +import java.util.HashMap; +import java.util.Map; import java.util.Optional; + import org.littletonrobotics.junction.Logger; import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; + import frc.lib.devices.AprilTagCamera; import frc.lib.posestimator.PoseEstimator.VisionPoseObservation; import frc.lib.posestimator.visionprocessors.LowestAmbiguity; @@ -50,78 +58,22 @@ */ public class VisionSubsystem extends SubsystemBase { + public static Transform3d test = new Transform3d(); + public static final String LOG_PREFIX = "VisionProcessor/"; - /** Baseline linear standard deviation used for vision observations. */ public static final double LINEAR_STDDEV_BASELINE = 0.4; - - /** Baseline angular standard deviation used for vision observations. */ public static final double ANGULAR_STDDEV_BASELINE = 0.4; - - /** Maximum allowable height (Z-axis) of a detected pose to be considered valid. */ public static final double MAX_Z_METERS = 0.75; - - /** Maximum allowable distance from a target to be considered valid. */ public static final double MAX_DISTANCE_METERS = 10; - - /** Maximum ambiguity ratio allowed in a result */ public static final double MAX_AMBIGUITY = 0.2; - /** Width of the field in meters. */ public static final double FIELD_WIDTH = FieldConstants.FIELD_WIDTH.in(Meters); - - /** Length of the field in meters. */ public static final double FIELD_LENGTH = FieldConstants.FIELD_LENGTH.in(Meters); - /** - * Quickly checks whether a {@link PhotonPipelineResult} is likely to be useful before full - * processing. - *

- * Rejects results with no targets, ambiguous poses above 0.2, or targets farther than 10 - * meters. - *

- * - * @param result the pipeline result to pre-filter - * @return {@code true} if the result passes preliminary checks, {@code false} otherwise - */ - public static boolean preFilter(PhotonPipelineResult result) - { - if (!result.hasTargets()) { - return false; - } - - if (result.getMultiTagResult().isPresent()) { - return true; - } - - PhotonTrackedTarget bestTarget = result.getBestTarget(); - - if (bestTarget.getBestCameraToTarget().getTranslation().getNorm() > MAX_DISTANCE_METERS) { - return false; - } - - return bestTarget.getPoseAmbiguity() <= MAX_AMBIGUITY; - } - - /** - * Checks whether a given {@link Pose3d} is valid on the field. - *

- * A pose is considered valid if it is within field boundaries and below {@link #MAX_Z_METERS}. - *

- * - * @param pose the pose to validate - * @return {@code true} if the pose is valid, {@code false} otherwise - */ - public static boolean postFilter(Pose3d pose) - { - double x = pose.getX(); - double y = pose.getY(); - double z = pose.getZ(); - return !(z > MAX_Z_METERS || x < 0.0 || x > FIELD_LENGTH || y < 0.0 || y > FIELD_WIDTH); - } - private final RobotState robotState = RobotState.getInstance(); private final AprilTagCamera[] cameras; + private final LowestAmbiguity fallbackVisionProcessor = new LowestAmbiguity(FieldConstants.APRILTAG_LAYOUT); private final MultiTagOnCoproc visionProcessor = @@ -129,131 +81,345 @@ public static boolean postFilter(Pose3d pose) Optional.of(fallbackVisionProcessor), FieldConstants.APRILTAG_LAYOUT); - /** - * Constructs a new {@code VisionSubsystem} with the specified cameras. - * - * @param cameras the cameras to use for vision processing - */ public VisionSubsystem(AprilTagCamera... cameras) { this.cameras = cameras; + setDefaultCommand(visionProcessingCommand()); } - @Override - public void periodic() + /** + * Returns a command that continuously processes vision data from all cameras and adds valid + * pose observations to {@link RobotState}. + */ + public Command visionProcessingCommand() { - for (var camera : cameras) { - String cameraLogPrefix = LOG_PREFIX + camera.getProperties().name() + "/"; - - PhotonPipelineResult[] results = camera.getUnreadResults().orElse(null); - if (results == null) { - continue; - } + return run(() -> { - ArrayList acceptedResults = new ArrayList<>(); - ArrayList rejectedResults = new ArrayList<>(); - ArrayList acceptedPoses = new ArrayList<>(); - ArrayList rejectedPoses = new ArrayList<>(); + for (var camera : cameras) { + String cameraLogPrefix = LOG_PREFIX + camera.getProperties().name() + "/"; - for (var result : results) { - if (!preFilter(result)) { - rejectedResults.add(result); + PhotonPipelineResult[] results = camera.getUnreadResults().orElse(null); + if (results == null) { continue; } - Rotation2d heading = robotState.getPoseAtTime(result.getTimestampSeconds()) - .map(Pose2d::getRotation).orElse(null); - if (heading == null) { - rejectedResults.add(result); - continue; - } + ArrayList acceptedResults = new ArrayList<>(); + ArrayList rejectedResults = new ArrayList<>(); + ArrayList acceptedPoses = new ArrayList<>(); + ArrayList rejectedPoses = new ArrayList<>(); + + for (var result : results) { + // if (!preFilter(result)) { + // rejectedResults.add(result); + // continue; + // } + + Rotation2d heading = robotState.getPoseAtTime(result.getTimestampSeconds()) + .map(Pose2d::getRotation).orElse(null); + if (heading == null) { + rejectedResults.add(result); + continue; + } - result.targets.forEach(target -> { - Pose2d pose = TrigSolve.solveTrigPosition(FieldConstants.APRILTAG_LAYOUT, - camera.getProperties(), target, heading).orElse(null); - if (pose == null || !postFilter(new Pose3d(pose))) { - return; + result.targets.forEach(target -> { + Pose2d pose = TrigSolve.solveTrigPosition(FieldConstants.APRILTAG_LAYOUT, + camera.getProperties(), target, heading).orElse(null); + if (pose == null /* || !postFilter(new Pose3d(pose)) */) { + return; + } + + robotState.addTrigPose( + target.getFiducialId(), + new TrigPoseRecord( + pose, + target.getBestCameraToTarget().getTranslation().getNorm(), + result.getTimestampSeconds())); + }); + + VisionPoseRecord poseRecord = visionProcessor.processVisionObservation( + result, + camera.getProperties(), + heading) + .orElse(null); + + if (poseRecord == null) { + rejectedResults.add(result); + continue; } - robotState.addTrigPose( - target.getFiducialId(), - new TrigPoseRecord( - pose, - target.getBestCameraToTarget().getTranslation().getNorm(), - result.getTimestampSeconds())); - }); - - VisionPoseRecord poseRecord = visionProcessor.processVisionObservation( - result, - camera.getProperties(), - heading) - .orElse(null); - - if (poseRecord == null) { - rejectedResults.add(result); - continue; + // if (!postFilter(poseRecord.pose())) { + // rejectedResults.add(result); + // rejectedPoses.add( + // poseRecord.pose().toPose2d()); + // continue; + // } + + double stdDevFactor = + (Math.pow(poseRecord.averageDistanceMeters(), 2.0) + / result.getTargets().size()) + * camera.getProperties().stdDevFactor(); + double linearStdDev = LINEAR_STDDEV_BASELINE * stdDevFactor; + double angularStdDev = ANGULAR_STDDEV_BASELINE * stdDevFactor; + + robotState.addVisionObservation( + new VisionPoseObservation( + result.getTimestampSeconds(), + poseRecord.pose().toPose2d(), + linearStdDev, + angularStdDev)); + + acceptedResults.add(result); + acceptedPoses.add( + poseRecord.pose().toPose2d()); } - if (!postFilter(poseRecord.pose())) { - rejectedResults.add(result); - rejectedPoses.add( - poseRecord.pose().toPose2d()); - continue; + Logger.recordOutput( + cameraLogPrefix + "/Results/AcceptedLength", + acceptedResults.size()); + for (int i = 0; i < acceptedResults.size(); i++) { + Logger.recordOutput( + cameraLogPrefix + "/Results/Accepted/" + i, + acceptedResults.get(i)); } - double stdDevFactor = - (Math.pow(poseRecord.averageDistanceMeters(), 2.0) / result.getTargets().size()) - * camera.getProperties().stdDevFactor(); - double linearStdDev = LINEAR_STDDEV_BASELINE * stdDevFactor; - double angularStdDev = ANGULAR_STDDEV_BASELINE * stdDevFactor; - - robotState.addVisionObservation( - new VisionPoseObservation( - result.getTimestampSeconds(), - poseRecord.pose().toPose2d(), - linearStdDev, - angularStdDev)); - - acceptedResults.add(result); - acceptedPoses.add( - poseRecord.pose().toPose2d()); - } + Logger.recordOutput( + cameraLogPrefix + "/Results/RejectedLength", + rejectedResults.size()); + for (int i = 0; i < rejectedResults.size(); i++) { + Logger.recordOutput( + cameraLogPrefix + "/Results/Rejected/" + i, + rejectedResults.get(i)); + } - Logger.recordOutput( - cameraLogPrefix + "/Results/AcceptedLength", - acceptedResults.size()); - for (int i = 0; i < acceptedResults.size(); i++) { Logger.recordOutput( - cameraLogPrefix + "/Results/Accepted/" + i, - acceptedResults.get(i)); - } + cameraLogPrefix + "/Poses/AcceptedLength", + acceptedPoses.size()); + for (int i = 0; i < acceptedPoses.size(); i++) { + Logger.recordOutput( + cameraLogPrefix + "/Poses/Accepted/" + i, + acceptedPoses.get(i)); + } - Logger.recordOutput( - cameraLogPrefix + "/Results/RejectedLength", - rejectedResults.size()); - for (int i = 0; i < rejectedResults.size(); i++) { Logger.recordOutput( - cameraLogPrefix + "/Results/Rejected/" + i, - rejectedResults.get(i)); + cameraLogPrefix + "/Poses/RejectedLength", + rejectedPoses.size()); + for (int i = 0; i < rejectedPoses.size(); i++) { + Logger.recordOutput( + cameraLogPrefix + "/Results/Rejected/" + i, + rejectedPoses.get(i)); + } } + }); + } - Logger.recordOutput( - cameraLogPrefix + "/Poses/AcceptedLength", - acceptedPoses.size()); - for (int i = 0; i < acceptedPoses.size(); i++) { - Logger.recordOutput( - cameraLogPrefix + "/Poses/Accepted/" + i, - acceptedPoses.get(i)); + /** + * Creates a command that performs camera extrinsic calibration. + * + *

+ * The returned command repeatedly collects samples of the camera-to-target {@link Transform3d} + * reported by each AprilTag camera while the robot is held at a known calibration point on the + * field. For each camera, the command accumulates {@code SAMPLE_COUNT} camera-to-target + * transform samples, uses the known transform from the target to the robot center to infer the + * camera's pose relative to the robot, and then averages those inferred poses to produce an + * extrinsic calibration for each camera. The resulting calibration data is written to + * NetworkTables so it can be inspected or saved by external tooling. + * + *

+ * The command finishes automatically after it has collected {@code SAMPLE_COUNT} valid samples + * from all cameras in {@link #cameras}. + * + * @param primaryCameraIndex index into {@code cameras[]} designating the primary camera whose + * target observations are used as the reference during calibration + * @param robotCenterTransform known transform from the calibration target to the robot center + * when the robot is positioned at the calibration point + */ + public Command cameraCalibrationCommand( + int primaryCameraIndex, + Transform3d robotCenterTransform) + { + return new Command() { + private static final int SAMPLE_COUNT = 1000; + + private final AprilTagCamera primary = cameras[primaryCameraIndex]; + + private final Map> samples = + new HashMap<>(); + + // Non-static initalizer + { + addRequirements(VisionSubsystem.this); } - Logger.recordOutput( - cameraLogPrefix + "/Poses/RejectedLength", - rejectedPoses.size()); - for (int i = 0; i < rejectedPoses.size(); i++) { + @Override + public void initialize() + { + for (var cam : cameras) { + samples.put(cam, new ArrayList<>()); + } + } + + @Override + public void execute() + { + for (var camera : cameras) { + if (samples.get(camera).size() >= SAMPLE_COUNT) { + continue; + } + + PhotonPipelineResult[] results = + camera.getUnreadResults().orElse(null); + if (results == null) { + continue; + } + + for (var result : results) { + if (!result.hasTargets()) { + continue; + } + + result.getTargets().forEach(target -> { + if (samples.get(camera).size() >= SAMPLE_COUNT) { + return; + } + samples.get(camera) + .add(target.getBestCameraToTarget()); + }); + } + } + } + + @Override + public boolean isFinished() + { + return samples.values().stream() + .allMatch(list -> list.size() >= SAMPLE_COUNT); + } + + @Override + public void end(boolean interrupted) + { + if (interrupted) { + return; + } + + // Estimate primary camera -> target + Transform3d primaryCamToTarget = + average(samples.get(primary)); + + Transform3d primaryCamToRobot = primaryCamToTarget.plus(robotCenterTransform); + + test = primaryCamToRobot; + Logger.recordOutput( - cameraLogPrefix + "/Results/Rejected/" + i, - rejectedPoses.get(i)); + "VisionCalibration/PrimaryCameraToRobot", + primaryCamToRobot); + + // Other cameras: camera -> primary -> robot + for (var camera : cameras) { + if (camera == primary) { + continue; + } + + Transform3d camToTarget = + average(samples.get(camera)); + + Transform3d camToPrimary = + camToTarget.plus(primaryCamToTarget.inverse()); + + Transform3d camToRobot = + camToPrimary.plus(primaryCamToRobot); + + Logger.recordOutput( + "VisionCalibration/" + camera.getProperties().name() + + "/CameraToRobot", + camToRobot); + } + } + + /** + * Computes an average of a list of transforms. + * + *

+ * Translation components (x, y, z) are averaged using a simple arithmetic mean. + * Rotation is averaged by summing the quaternions (with hemisphere correction to keep + * them on a consistent side of the unit sphere) and then normalizing the result to + * produce the final {@link Rotation3d}. + */ + private Transform3d average(ArrayList list) + { + double x = 0; + double y = 0; + double z = 0; + + double qw = 0; + double qx = 0; + double qy = 0; + double qz = 0; + + for (var t : list) { + x += t.getX(); + y += t.getY(); + z += t.getZ(); + + // Quaternion averaging (Unfortunately no WPILib built-in) + var q = t.getRotation().getQuaternion(); + + // Hemisphere correction + if (q.getW() < 0) { + qw -= q.getW(); + qx -= q.getX(); + qy -= q.getY(); + qz -= q.getZ(); + } else { + qw += q.getW(); + qx += q.getX(); + qy += q.getY(); + qz += q.getZ(); + } + } + + int n = list.size(); + + var avgQuat = new Quaternion( + qw / n, + qx / n, + qy / n, + qz / n).normalize(); + + return new Transform3d( + x / n, + y / n, + z / n, + new Rotation3d(avgQuat)); } + }; + } + + public static boolean preFilter(PhotonPipelineResult result) + { + if (!result.hasTargets()) { + return false; + } + + if (result.getMultiTagResult().isPresent()) { + return true; } + + return result.getBestTarget() + .getPoseAmbiguity() <= MAX_AMBIGUITY + && result.getBestTarget() + .getBestCameraToTarget() + .getTranslation() + .getNorm() <= MAX_DISTANCE_METERS; + } + + public static boolean postFilter(Pose3d pose) + { + double x = pose.getX(); + double y = pose.getY(); + double z = pose.getZ(); + return !(z > MAX_Z_METERS + || x < 0.0 || x > FIELD_LENGTH + || y < 0.0 || y > FIELD_WIDTH); } } diff --git a/src/test/java/frc/robot/TestUtil.java b/src/test/java/frc/robot/TestUtil.java index 06ec2c08..e3d04a25 100644 --- a/src/test/java/frc/robot/TestUtil.java +++ b/src/test/java/frc/robot/TestUtil.java @@ -13,41 +13,56 @@ * not, see . */ + package frc.robot; +import edu.wpi.first.hal.NotifierJNI; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.CommandScheduler; public class TestUtil { - private static Timer timer = new Timer(); - private static Timer loopTimer = new Timer(); + private static final boolean USE_TIMING = false; + + private static final int NOTIFIER = NotifierJNI.initializeNotifier(); + private static final CommandScheduler SCHEDULER = CommandScheduler.getInstance(); + private static final Timer TIMER = new Timer(); - public static void runTest(Command command, double duration, Subsystem subsystem) + public static void runTest(Command command, double duration) { - command.initialize(); - - timer.restart(); - loopTimer.restart(); - boolean finished = false; - while (timer.get() < duration) { - if (!finished) { - command.execute(); - if (command.isFinished()) { - finished = true; + SCHEDULER.cancelAll(); + + command.schedule(); + + TIMER.start(); + + double nextCycleSeconds = 0.0; + while (true) { + if (USE_TIMING) { + double currentTime = TIMER.get(); + if (nextCycleSeconds < currentTime) { + // Loop overrun, start next cycle immediately + nextCycleSeconds = currentTime; + } else { + // Wait before next cycle + NotifierJNI.updateNotifierAlarm(NOTIFIER, (long) nextCycleSeconds * 1000000); + if (NotifierJNI.waitForNotifierAlarm(NOTIFIER) == 0L) { + // Break the loop if the notifier was stopped + break; + } } + nextCycleSeconds += 0.02; } - subsystem.periodic(); - while (!loopTimer.advanceIfElapsed(0.02)) { - // wait until 20ms has passed - } + SCHEDULER.run(); + if (TIMER.hasElapsed(duration)) + break; } - command.end(!finished); - subsystem.periodic(); - timer.stop(); - loopTimer.stop(); + TIMER.stop(); + TIMER.reset(); + + SCHEDULER.cancelAll(); } } diff --git a/src/test/java/frc/robot/subsystems/drive/DriveTest.java b/src/test/java/frc/robot/subsystems/drive/DriveTest.java index 0206397e..d89e9576 100644 --- a/src/test/java/frc/robot/subsystems/drive/DriveTest.java +++ b/src/test/java/frc/robot/subsystems/drive/DriveTest.java @@ -66,7 +66,7 @@ void robotIsEnabled() @Test // marks this method as a test void testStop() { - TestUtil.runTest(Commands.runOnce(() -> drive.stop()), 0.1, drive); + TestUtil.runTest(Commands.runOnce(() -> drive.stop()), 0.1); try { assertEquals(0.0, drive.getFFCharacterizationVelocity(), DELTA); // make sure that the // speed of the motor @@ -79,8 +79,8 @@ void testStop() @Test void testDriveVelocity() { - TestUtil.runTest(Commands.run(() -> drive.runVelocity(new ChassisSpeeds(1.5, 1.5, 0.0))), 1, - drive); + TestUtil.runTest(Commands.run(() -> drive.runVelocity(new ChassisSpeeds(1.5, 1.5, 0.0))), + 1); try { assertEquals(1.5, drive.getChassisSpeeds().vxMetersPerSecond, DELTA); assertEquals(1.5, drive.getChassisSpeeds().vyMetersPerSecond, DELTA); @@ -94,8 +94,8 @@ void testDriveVelocity() @Test void testSteerVelocity() { - TestUtil.runTest(Commands.run(() -> drive.runVelocity(new ChassisSpeeds(0.0, 0.0, 1.5))), 1, - drive); + TestUtil.runTest(Commands.run(() -> drive.runVelocity(new ChassisSpeeds(0.0, 0.0, 1.5))), + 1); try { assertEquals(1.5, drive.getChassisSpeeds().omegaRadiansPerSecond, DELTA); } catch (Exception e) { @@ -106,27 +106,37 @@ void testSteerVelocity() @Test void testX() { - TestUtil.runTest(drive.runOnce(() -> drive.stopWithX()), 0.1, drive); + drive.removeDefaultCommand(); + TestUtil.runTest(drive.runOnce(() -> drive.stopWithX()), 0.5); try { SwerveModulePosition[] swerveModulePositions = drive.getModulePositions(); Rotation2d[] targetAngles = { - new Rotation2d(Math.atan( - DriveConstants.FrontLeft.LocationY / DriveConstants.FrontLeft.LocationX)), // Front - // Left - new Rotation2d(Math.atan( - DriveConstants.FrontRight.LocationY / DriveConstants.FrontRight.LocationX)), // Front - // Right - new Rotation2d(Math.atan( - DriveConstants.BackLeft.LocationY / DriveConstants.BackLeft.LocationX)), // Back - // Left - new Rotation2d(Math.atan( - DriveConstants.BackRight.LocationY / DriveConstants.BackRight.LocationX)) // Back - // Right + new Rotation2d(Math.atan2( + DriveConstants.FrontLeft.LocationY, + DriveConstants.FrontLeft.LocationX)), + new Rotation2d(Math.atan2( + DriveConstants.FrontRight.LocationY, + DriveConstants.FrontRight.LocationX)), + new Rotation2d(Math.atan2( + DriveConstants.BackLeft.LocationY, + DriveConstants.BackLeft.LocationX)), + new Rotation2d(Math.atan2( + DriveConstants.BackRight.LocationY, + DriveConstants.BackRight.LocationX)) }; + // Test position of modules for (int i = 0; i < swerveModulePositions.length; i++) { - assertEquals(targetAngles[i].getRadians(), - swerveModulePositions[i].angle.getRadians(), DELTA); + double error = Rotation2d.fromRadians( + swerveModulePositions[i].angle.getRadians() + - targetAngles[i].getRadians()) + .getRadians(); + + error = Math.abs(Math.IEEEremainder(error, 2 * Math.PI)); + + assertTrue( + error < DELTA || Math.abs(error - Math.PI) < DELTA, + "Module " + i + " angle incorrect"); } assertEquals(0.0, drive.getChassisSpeeds().vxMetersPerSecond, DELTA); // make sure that // the drivetrain diff --git a/src/test/java/frc/robot/subsystems/superstructure/SuperstructureTest.java b/src/test/java/frc/robot/subsystems/superstructure/SuperstructureTest.java index c401511d..d16086a8 100644 --- a/src/test/java/frc/robot/subsystems/superstructure/SuperstructureTest.java +++ b/src/test/java/frc/robot/subsystems/superstructure/SuperstructureTest.java @@ -54,8 +54,7 @@ void goToGoal() { TestUtil.runTest( superstructure.setGoalWithWait(Superstructure.Setpoint.STOW), - 3, - superstructure); + 3); try { // Check position to check if the subsystem is actually in tolerance of STOW setpoint. assertTrue(superstructure.nearSetpoint(Superstructure.Setpoint.STOW)); diff --git a/src/test/java/frc/robot/subsystems/vision/VisionSubsystemTest.java b/src/test/java/frc/robot/subsystems/vision/VisionSubsystemTest.java new file mode 100644 index 00000000..68c5b92c --- /dev/null +++ b/src/test/java/frc/robot/subsystems/vision/VisionSubsystemTest.java @@ -0,0 +1,153 @@ +/* + * Copyright (C) 2025 Windham Windup + * + * This program is free software: you can redistribute it and/or modify it under the terms of the + * GNU General Public License as published by the Free Software Foundation, either version 3 of the + * License, or any later version. + * + * This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without + * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with this program. If + * not, see . + */ + +package frc.robot.subsystems.vision; + +import static edu.wpi.first.units.Units.Centimeter; +import static edu.wpi.first.units.Units.Degrees; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static org.junit.jupiter.api.Assertions.fail; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import edu.wpi.first.hal.HAL; +import edu.wpi.first.math.geometry.Rotation2d; +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.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.lib.util.GeomUtil; +import frc.robot.FieldConstants; +import frc.robot.RobotState; +import frc.robot.TestUtil; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.drive.DriveConstants; + +class VisionSubsystemTest { + + private Drive drive; + private VisionSubsystem visionSubsystem; + private RobotState robotState; + + private int visionPoseCount = 0; + + @BeforeEach + void setup() + { + assertTrue(HAL.initialize(500, 0)); + + robotState = RobotState.getInstance(); + + var tags = FieldConstants.APRILTAG_LAYOUT.getTags(); + assertTrue(!tags.isEmpty()); + + var robotLocation = + tags.get(0).pose + .toPose2d() + .plus(new Transform2d(2, 0, Rotation2d.k180deg)); + + robotState.resetPose(robotLocation); + + visionSubsystem = VisionConstants.get(); + drive = DriveConstants.get(); + + DriverStationSim.setEnabled(true); + DriverStationSim.notifyNewData(); + + Timer.delay(0.100); + } + + @Test + void visionProcessingAddsVisionObservations() + { + visionPoseCount = 0; + + Transform3d tagToRobot = + new Transform3d(1.0, 0.0, 0.0, new Rotation3d(Rotation2d.k180deg)) + // Move robot such that camera is perfectly facing the tag + .plus(VisionConstants.FRONT_RIGHT_TRANSFORM.inverse()); + + // Ensure the field has at least one tag + var tags = FieldConstants.APRILTAG_LAYOUT.getTags(); + assertTrue(!tags.isEmpty(), "Expected at least one AprilTag in the field layout"); + + robotState.resetPose(tags.get(0).pose.plus(tagToRobot).toPose2d()); + + try { + robotState.setVisionObservationConsumer(v -> { + visionPoseCount++; + }); + + TestUtil.runTest( + Commands.parallel( + Commands.idle(drive), + visionSubsystem.visionProcessingCommand()), + 2.0); + + assertTrue( + visionPoseCount > 0, + "Expected at least one vision pose observation"); + } catch (Exception e) { + fail("Vision processing command failed: " + e.getMessage()); + } + } + + @Test + void cameraCalibrationCommandProducesCalibrationData() + { + int primaryIndex = 0; + + // Known transform from calibration target (tag) to robot center + Transform3d knownRobotCenterTransform = new Transform3d(1.0, 0.0, 0.0, new Rotation3d()); + + // Ensure the field has at least one tag + var tags = FieldConstants.APRILTAG_LAYOUT.getTags(); + assertTrue(!tags.isEmpty(), "Expected at least one AprilTag in the field layout"); + + robotState.resetPose(tags.get(0).pose.plus(knownRobotCenterTransform).toPose2d()); + TestUtil.runTest(Commands.run(() -> drive.runVelocity(new ChassisSpeeds(1.5, 1.5, 0.0))), + 1); + + // Run the calibration command for a short, deterministic number of loops + // The calibration command uses SAMPLE_COUNT internally; in sim, ensure at least 1 sample + // is collected + TestUtil.runTest( + visionSubsystem.cameraCalibrationCommand(primaryIndex, knownRobotCenterTransform), + 1.0); // 1 second is sufficient for sim + + // Grab the calibration result + Transform3d measuredCamToRobot = VisionSubsystem.test; + + // Compute expected camera → robot transform + // camera → robot = camera → target + target → robot + // In simulation, assume camera → target is identity (or matches what the calibration + // command will sample) + Transform3d expectedCamToRobot = knownRobotCenterTransform; + + // Compare the transforms with tight tolerances + boolean isClose = GeomUtil.isNear( + measuredCamToRobot, + expectedCamToRobot, + Centimeter.one(), + new Rotation3d(Degrees.one(), Degrees.one(), Degrees.one())); + + System.out.println("Actual camera → robot: " + measuredCamToRobot); + System.out.println("Expected camera → robot: " + expectedCamToRobot); + + assertTrue(isClose, "Recorded camera-to-robot transform does not match expected value"); + } +}