Untested - add basic vision sim implementation

team401 · Nov 17, 2024 · df9bd58 · df9bd58
1 parent e90ad3a
commit df9bd58
Show file tree

Hide file tree

Showing 5 changed files with 34 additions and 123 deletions.
diff --git 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
diff --git a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java
diff --git a/vision/src/main/java/coppercore/vision/VisionLocalizer.java b/vision/src/main/java/coppercore/vision/VisionLocalizer.java
@@ -1,38 +1,46 @@
 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<VisionMeasurement> visionMeasurementConsumer;
     VisionSystemSim visionSim;
+    Supplier<Pose3d> simRobotPoseSupplier;
+
+    public final boolean isSim;
 
     /**
      * Create a vision localizer
      *
      * @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<Pose3d> newSimRobotPoseSupplier) {
+        simRobotPoseSupplier = newSimRobotPoseSupplier;
+    }
+
     @Override
     public void periodic() {
+        if (isSim) {
+            visionSim.update(simRobotPoseSupplier.get());
+        }
         for (CameraWrapper camera : cameras) {
             for (PhotonPipelineResult result : camera.getCamera().getAllUnreadResults()) {
                 Optional<EstimatedRobotPose> potentialPose =

diff --git a/vision/vendordeps/photonlib-json-1.0.json b/vision/vendordeps/photonlib-json-1.0.json