-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat: untested vision sim io with photonvision sim support
- Loading branch information
1 parent
32b504b
commit 5a356c0
Showing
4 changed files
with
166 additions
and
5 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
90 changes: 90 additions & 0 deletions
90
src/main/java/org/team1540/robot2024/subsystems/vision/AprilTagVisionIOSim.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<Pose2d> poseSupplier; | ||
|
|
||
| private Transform3d cameraTransform; | ||
| private Pose2d lastEstimatedPose; | ||
|
|
||
| public AprilTagVisionIOSim(String name, Pose3d cameraOffsetMeters, Supplier<Pose2d> 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<EstimatedRobotPose> estimatedPose = photonEstimator.update(); | ||
| List<PhotonTrackedTarget> 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(); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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" | ||
| } | ||
| ] | ||
| } |