Log latest poses from cameras when pose is present using advantagekit…

… logger
team401 · Nov 9, 2024 · bb82986 · bb82986
1 parent 1260d09
commit bb82986
Show file tree

Hide file tree

Showing 3 changed files with 79 additions and 9 deletions.
diff --git a/vision/build.gradle b/vision/build.gradle
@@ -16,11 +16,22 @@ java {
 // Set this to true to enable desktop support.
 def includeDesktopSupport = true
 
+configurations.all {
+	exclude group: "edu.wpi.first.wpilibj"
+}
+
+task(checkAkitInstall, dependsOn: "classes", type: JavaExec) {
+	mainClass = "org.littletonrobotics.junction.CheckInstall"
+	classpath = sourceSets.main.runtimeClasspath
+}
+compileJava.finalizedBy checkAkitInstall
+
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 5.
 dependencies {
-	implementation 'edu.wpi.first.wpilibj:wpilibj-java:2024.3.2'
-	implementation 'edu.wpi.first.wpilibNewCommands:wpilibNewCommands-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"
+	annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:3.2.1"
 
 	implementation wpi.java.deps.wpilib()
 	implementation wpi.java.vendor.java()
@@ -56,10 +67,18 @@ repositories {
 	mavenLocal()
 	maven {
 		url 'https://frcmaven.wpi.edu/artifactory/release/'
+	}
+		maven {
+		url = uri("https://maven.pkg.github.com/Mechanical-Advantage/AdvantageKit")
+		credentials {
+			username = "Mechanical-Advantage-Bot"
+			password = "\u0067\u0068\u0070\u005f\u006e\u0056\u0051\u006a\u0055\u004f\u004c\u0061\u0079\u0066\u006e\u0078\u006e\u0037\u0051\u0049\u0054\u0042\u0032\u004c\u004a\u006d\u0055\u0070\u0073\u0031\u006d\u0037\u004c\u005a\u0030\u0076\u0062\u0070\u0063\u0051"
+		}
 	}
 	gradlePluginPortal()
 }
 
+
 spotless {
 // optional: limit format enforcement to just the files changed by this feature branch
 ratchetFrom 'origin/main'

diff --git a/vision/src/main/java/VisionLocalizer.java b/vision/src/main/java/VisionLocalizer.java
@@ -2,9 +2,11 @@
 
 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;
 import java.util.Optional;
 import java.util.function.Consumer;
+import org.littletonrobotics.junction.Logger;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
@@ -39,13 +41,20 @@ public VisionLocalizer(CameraParams[] cameraParams, AprilTagFieldLayout fieldLay
     public void periodic() {
         for (CameraWrapper camera : cameras) {
             for (PhotonPipelineResult result : camera.getCamera().getAllUnreadResults()) {
-                Optional<EstimatedRobotPose> pose = camera.getPoseEstimator().update(result);
-                visionMeasurementConsumer.accept(
-                        // TODO: Actual standard deviations (not zero)
-                        new VisionMeasurement(
-                                pose.get().estimatedPose.toPose2d(),
-                                result.getTimestampSeconds(),
-                                VecBuilder.fill(0.0, 0.0, Double.MAX_VALUE)));
+                Optional<EstimatedRobotPose> potentialPose =
+                        camera.getPoseEstimator().update(result);
+
+                if (potentialPose.isPresent()) {
+                    EstimatedRobotPose est = potentialPose.get();
+                    Pose3d pose = est.estimatedPose;
+                    visionMeasurementConsumer.accept( // TODO: Actual standard deviations (not zero)
+                            new VisionMeasurement(
+                                    pose.toPose2d(),
+                                    result.getTimestampSeconds(),
+                                    VecBuilder.fill(0.0, 0.0, 0.0)));
+                    Logger.recordOutput(
+                            "vision/" + camera.getCamera().getName() + "/latestPoseEstimate", pose);
+                }
             }
         }
     }

diff --git a/vision/vendordeps/AdvantageKit.json b/vision/vendordeps/AdvantageKit.json
@@ -0,0 +1,42 @@
+{
+    "fileName": "AdvantageKit.json",
+    "name": "AdvantageKit",
+    "version": "3.2.1",
+    "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
+    "frcYear": "2024",
+    "mavenUrls": [],
+    "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
+    "javaDependencies": [        
+        {
+            "groupId": "org.littletonrobotics.akit.junction",
+            "artifactId": "wpilib-shim",
+            "version": "3.2.1"
+        },        
+        {
+            "groupId": "org.littletonrobotics.akit.junction",
+            "artifactId": "junction-core",
+            "version": "3.2.1"
+        },        
+        {
+            "groupId": "org.littletonrobotics.akit.conduit",
+            "artifactId": "conduit-api",
+            "version": "3.2.1"
+        }
+    ],
+    "jniDependencies": [        
+        {
+            "groupId": "org.littletonrobotics.akit.conduit",
+            "artifactId": "conduit-wpilibio",
+            "version": "3.2.1",
+            "skipInvalidPlatforms": false,
+            "isJar": false,
+            "validPlatforms": [
+                "linuxathena",
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": []
+}