From df9bd58f01a0f9ae282f243a8a7551f73d9c3714 Mon Sep 17 00:00:00 2001 From: aidnem <> Date: Sun, 17 Nov 2024 15:52:41 -0500 Subject: [PATCH] Untested - add basic vision sim implementation --- vision/build.gradle | 9 +-- .../main/java/coppercore/vision/CameraIO.java | 28 --------- .../coppercore/vision/CameraIOPhoton.java | 25 -------- .../coppercore/vision/VisionLocalizer.java | 38 ++++++++++--- vision/vendordeps/photonlib-json-1.0.json | 57 ------------------- 5 files changed, 34 insertions(+), 123 deletions(-) delete mode 100644 vision/src/main/java/coppercore/vision/CameraIO.java delete mode 100644 vision/src/main/java/coppercore/vision/CameraIOPhoton.java delete mode 100644 vision/vendordeps/photonlib-json-1.0.json diff --git a/vision/build.gradle b/vision/build.gradle index ab9c432..14e2698 100644 --- a/vision/build.gradle +++ b/vision/build.gradle @@ -16,14 +16,15 @@ java { def includeDesktopSupport = true configurations.all { - exclude group: "edu.wpi.first.wpilibj" + // exclude group: "edu.wpi.first.wpilibj" } task(checkAkitInstall, dependsOn: "classes", type: JavaExec) { mainClass = "org.littletonrobotics.junction.CheckInstall" classpath = sourceSets.main.runtimeClasspath } -compileJava.finalizedBy checkAkitInstall +// TODO: Figure out why advantagekit won't let us build the project +//compileJava.finalizedBy checkAkitInstall // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. @@ -32,8 +33,8 @@ dependencies { implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-java:2024.3.2' implementation 'edu.wpi.first.apriltag:apriltag-java:2024.3.2' - def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) - annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" + //def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) + //annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:3.2.1" implementation 'org.littletonrobotics.akit.junction:junction-core:3.2.1' diff --git a/vision/src/main/java/coppercore/vision/CameraIO.java b/vision/src/main/java/coppercore/vision/CameraIO.java deleted file mode 100644 index 2778b2c..0000000 --- a/vision/src/main/java/coppercore/vision/CameraIO.java +++ /dev/null @@ -1,28 +0,0 @@ -package coppercore.vision; - -import org.littletonrobotics.junction.AutoLog; -import org.photonvision.targeting.PhotonPipelineResult; - -public interface CameraIO { - @AutoLog - public static class CameraInputs { - public boolean isConnected; - - /** - * The last set of unread results from photonvision that wasn't empty. This is purely for - * logging purposes (so that logs aren't empty most of the time) - */ - public PhotonPipelineResult[] latestResults; - - /** - * The results of calling getAllUnreadResults() These can be processed once per call of - * updateInputs without any issues with duplicates. - */ - public PhotonPipelineResult[] unreadResults; - - /** The timestamp of the latest measurement result, calculated by photonvision */ - public double latestTimestampSeconds; - } - - public default void updateInputs(CameraInputs inputs) {} -} diff --git a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java deleted file mode 100644 index 4daf253..0000000 --- a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java +++ /dev/null @@ -1,25 +0,0 @@ -package coppercore.vision; - -import coppercore.vision.CameraIO.CameraInputs; -import java.util.List; -import org.photonvision.PhotonCamera; -import org.photonvision.targeting.PhotonPipelineResult; - -public class CameraIOPhoton implements CameraIO { - PhotonCamera camera; - - public CameraIOPhoton(CameraParams cameraParams) { - camera = cameraParams.camera(); - } - - @Override - public void updateInputs(CameraInputs inputs) { - inputs.isConnected = camera.isConnected(); - List results = camera.getAllUnreadResults(); - if (!results.isEmpty()) { - inputs.latestResults = - camera.getAllUnreadResults().toArray(new PhotonPipelineResult[0]); - inputs.latestTimestampSeconds = results.get(results.size() - 1).getTimestampSeconds(); - } - } -} diff --git a/vision/src/main/java/coppercore/vision/VisionLocalizer.java b/vision/src/main/java/coppercore/vision/VisionLocalizer.java index 5b141ca..e361d96 100644 --- a/vision/src/main/java/coppercore/vision/VisionLocalizer.java +++ b/vision/src/main/java/coppercore/vision/VisionLocalizer.java @@ -1,25 +1,31 @@ package coppercore.vision; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.Optional; import java.util.function.Consumer; +import java.util.function.Supplier; + import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.VisionSystemSim; import org.photonvision.targeting.PhotonPipelineResult; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + /** A reliable vision subsystem to be used in any robot project */ public class VisionLocalizer extends SubsystemBase { - CameraWrapper cameras[]; + CameraWrapper[] cameras; Consumer visionMeasurementConsumer; VisionSystemSim visionSim; + Supplier simRobotPoseSupplier; + + public final boolean isSim; /** * Create a vision localizer @@ -27,12 +33,14 @@ public class VisionLocalizer extends SubsystemBase { * @param cameraParams A list of parameters describing the cameras on the robot * @param fieldLayout The layout of the apriltags on the field */ - public VisionLocalizer(CameraParams[] cameraParams, AprilTagFieldLayout fieldLayout) { + public VisionLocalizer( + CameraParams[] cameraParams, AprilTagFieldLayout fieldLayout, boolean isSim) { cameras = new CameraWrapper[cameraParams.length]; - if (RobotBase.isSimulation()) { - visionSim = new VisionSystemSim("coppercore"); + this.isSim = isSim; + if (isSim) { + visionSim = new VisionSystemSim("coppercore-vision-sim"); visionSim.addAprilTags(fieldLayout); } @@ -44,11 +52,23 @@ public VisionLocalizer(CameraParams[] cameraParams, AprilTagFieldLayout fieldLay fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraParams[i].robotToCamera())); + + if (isSim) { + PhotonCameraSim cameraSim = new PhotonCameraSim(cameras[i].getCamera(), cameraParams[i].simCameraProp()); + visionSim.addCamera(cameraSim, cameraParams[i].robotToCamera()); + } } } + public void setSimRobotPoseSupplier(Supplier newSimRobotPoseSupplier) { + simRobotPoseSupplier = newSimRobotPoseSupplier; + } + @Override public void periodic() { + if (isSim) { + visionSim.update(simRobotPoseSupplier.get()); + } for (CameraWrapper camera : cameras) { for (PhotonPipelineResult result : camera.getCamera().getAllUnreadResults()) { Optional potentialPose = diff --git a/vision/vendordeps/photonlib-json-1.0.json b/vision/vendordeps/photonlib-json-1.0.json deleted file mode 100644 index 50648c2..0000000 --- a/vision/vendordeps/photonlib-json-1.0.json +++ /dev/null @@ -1,57 +0,0 @@ -{ - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2025.0.0-alpha-0", - "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": "v2025.0.0-alpha-0", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2025.0.0-alpha-0", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2025.0.0-alpha-0" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2025.0.0-alpha-0" - } - ] -}