From 5a356c00d16037fb78d059d93acb5cbd9323c082 Mon Sep 17 00:00:00 2001 From: Alvin Zhang <41vin2h4n9@gmail.com> Date: Wed, 31 Jan 2024 22:38:05 -0800 Subject: [PATCH] feat: untested vision sim io with photonvision sim support --- .../org/team1540/robot2024/Constants.java | 13 +++ .../team1540/robot2024/RobotContainer.java | 11 +-- .../vision/AprilTagVisionIOSim.java | 90 +++++++++++++++++++ vendordeps/photonlib-json-1.0.json | 57 ++++++++++++ 4 files changed, 166 insertions(+), 5 deletions(-) create mode 100644 src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java create mode 100644 vendordeps/photonlib-json-1.0.json diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java index 300b1dd3..bfca092f 100644 --- a/src/main/java/org/team1540/robot2024/Constants.java +++ b/src/main/java/org/team1540/robot2024/Constants.java @@ -1,9 +1,13 @@ package org.team1540.robot2024; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import java.io.IOException; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -78,6 +82,15 @@ public static class Vision { public static final double MIN_ACCEPTED_NUM_TAGS = 1; public static final double MAX_ACCEPTED_AVG_TAG_DIST_METERS = 8.0; public static final double MAX_ACCEPTED_ELEVATOR_SPEED_MPS = 0.05; + + public static final AprilTagFieldLayout SIM_APRILTAG_LAYOUT; + static { + try { + SIM_APRILTAG_LAYOUT = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); + } catch (IOException e) { + throw new RuntimeException(e); + } + } } public static class Elevator { diff --git a/src/main/java/org/team1540/robot2024/RobotContainer.java b/src/main/java/org/team1540/robot2024/RobotContainer.java index 6c30dbd2..6f6706d5 100644 --- a/src/main/java/org/team1540/robot2024/RobotContainer.java +++ b/src/main/java/org/team1540/robot2024/RobotContainer.java @@ -17,6 +17,7 @@ import org.team1540.robot2024.subsystems.vision.AprilTagVision; import org.team1540.robot2024.subsystems.vision.AprilTagVisionIO; import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOLimelight; +import org.team1540.robot2024.subsystems.vision.AprilTagVisionIOSim; import org.team1540.robot2024.util.PhoenixTimeSyncSignalRefresher; import org.team1540.robot2024.util.swerve.SwerveFactory; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -84,11 +85,11 @@ public RobotContainer() { shooter = new Shooter(new ShooterPivotIOSim(), new FlywheelsIOSim()); aprilTagVision = new AprilTagVision( - new AprilTagVisionIO() {}, - new AprilTagVisionIO() {}, - (ignored) -> {}, - () -> 0.0, - new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); + new AprilTagVisionIOSim(Constants.Vision.FRONT_CAMERA_NAME, Constants.Vision.FRONT_CAMERA_POSE, drivetrain::getPose), + new AprilTagVisionIOSim(Constants.Vision.REAR_CAMERA_NAME, Constants.Vision.REAR_CAMERA_POSE, drivetrain::getPose), + ignored -> {}, + () -> 0.0, // TODO: ACTUALLY GET ELEVATOR HEIGHT HERE + new VisionPoseAcceptor(drivetrain::getChassisSpeeds, () -> 0.0)); // TODO: ACTUALLY GET ELEVATOR VELOCITY HERE break; default: diff --git a/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java new file mode 100644 index 00000000..0882501c --- /dev/null +++ b/src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java @@ -0,0 +1,90 @@ +package org.team1540.robot2024.subsystems.vision; + +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.Transform3d; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonTrackedTarget; + +import java.util.List; +import java.util.Optional; +import java.util.function.Supplier; + +import static org.team1540.robot2024.Constants.Vision.*; + +public class AprilTagVisionIOSim implements AprilTagVisionIO { + private final VisionSystemSim visionSystemSim; + private final PhotonCameraSim cameraSim; + private final PhotonCamera camera; + private final PhotonPoseEstimator photonEstimator; + + private final Supplier poseSupplier; + + private Transform3d cameraTransform; + private Pose2d lastEstimatedPose; + + public AprilTagVisionIOSim(String name, Pose3d cameraOffsetMeters, Supplier poseSupplier) { + this.visionSystemSim = new VisionSystemSim(name); + this.poseSupplier = poseSupplier; + visionSystemSim.addAprilTags(SIM_APRILTAG_LAYOUT); + + SimCameraProperties cameraProps = new SimCameraProperties(); + cameraProps.setCalibration(1280, 960, Rotation2d.fromDegrees(100)); + cameraProps.setFPS(14.5); + cameraProps.setAvgLatencyMs(67.0); + camera = new PhotonCamera(name); + cameraSim = new PhotonCameraSim(camera, cameraProps); + + cameraTransform = new Transform3d(cameraOffsetMeters.getTranslation(), cameraOffsetMeters.getRotation()); + visionSystemSim.addCamera(cameraSim, cameraTransform); + + photonEstimator = new PhotonPoseEstimator( + SIM_APRILTAG_LAYOUT, + PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + camera, + cameraTransform); + photonEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_LAST_POSE); + lastEstimatedPose = poseSupplier.get(); + } + + @Override + public void updateInputs(AprilTagVisionIOInputs inputs) { + photonEstimator.setReferencePose(lastEstimatedPose); + visionSystemSim.update(poseSupplier.get()); + + Optional estimatedPose = photonEstimator.update(); + List trackedTargets = camera.getLatestResult().getTargets(); + + if (estimatedPose.isPresent()) { + lastEstimatedPose = estimatedPose.get().estimatedPose.toPose2d(); + inputs.estimatedPoseMeters = lastEstimatedPose; + inputs.lastMeasurementTimestampSecs = estimatedPose.get().timestampSeconds; + } + inputs.hasTarget = !trackedTargets.isEmpty(); + inputs.seenTagIDs = new int[trackedTargets.size()]; + inputs.tagPosesMeters = new Pose2d[trackedTargets.size()]; + for (int i = 0; i < trackedTargets.size(); i++) { + PhotonTrackedTarget target = trackedTargets.get(i); + inputs.seenTagIDs[i] = target.getFiducialId(); + inputs.tagPosesMeters[i] = + new Pose3d().plus(target.getBestCameraToTarget().plus(cameraTransform.inverse())).toPose2d(); + } + } + + @Override + public void setPoseOffset(Pose3d newPose) { + cameraTransform = new Transform3d(newPose.getTranslation(), newPose.getRotation()); + visionSystemSim.adjustCamera(cameraSim, cameraTransform); + } + + @Override + public String getName() { + return camera.getName(); + } +} diff --git a/vendordeps/photonlib-json-1.0.json b/vendordeps/photonlib-json-1.0.json new file mode 100644 index 00000000..8b19d38a --- /dev/null +++ b/vendordeps/photonlib-json-1.0.json @@ -0,0 +1,57 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2024.2.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2024", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2024.2.2", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.2.2", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2024.2.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2024.2.2" + } + ] +}