Publish the PoseStrategy used to construct MultiPhotonPoseEstimator

AdvantageScope-simulator.json

@@ -10,11 +10,11 @@
           "width": 300,
           "expanded": [
             "/Drive",
-            "/photonvision/capt-barnacles",
-            "/photonvision/professor-inkling",
-            "/Vision/capt-barnacles",
-            "/Vision/professor-inkling",
-            "/Vision/limelight"
+            "/photonVision/left_color",
+            "/photonVision/right_color",
+            "/Vision/camera",
+            "/Vision/camera/left_color",
+            "/Vision/camera/right_color"
           ]
         },
         "tabs": {
@@ -73,9 +73,16 @@
                     "visible": true,
                     "options": {}
                   },
+                  {
+                    "type": "rotationOverride",
+                    "logKey": "NT:/Drive/pigeon angle",
+                    "logType": "Rotation3d",
+                    "visible": true,
+                    "options": {}
+                  },
                   {
                     "type": "ghost",
-                    "logKey": "NT:/Vision/professor-inkling/poseEstimate",
+                    "logKey": "NT:/Vision/camera/right_color/poseEstimate",
                     "logType": "Pose3d",
                     "visible": true,
                     "options": {
@@ -84,7 +91,7 @@
                   },
                   {
                     "type": "vision",
-                    "logKey": "NT:/Vision/professor-inkling/aprilTagPose",
+                    "logKey": "NT:/Vision/camera/right_color/aprilTagPose",
                     "logType": "Pose3d",
                     "visible": true,
                     "options": {
@@ -94,7 +101,7 @@
                   },
                   {
                     "type": "ghost",
-                    "logKey": "NT:/Vision/capt-barnacles/poseEstimate",
+                    "logKey": "NT:/Vision/camera/left_color/poseEstimate",
                     "logType": "Pose3d",
                     "visible": true,
                     "options": {
@@ -103,32 +110,13 @@
                   },
                   {
                     "type": "vision",
-                    "logKey": "NT:/Vision/capt-barnacles/aprilTagPose",
+                    "logKey": "NT:/Vision/camera/left_color/aprilTagPose",
                     "logType": "Pose3d",
                     "visible": true,
                     "options": {
                       "color": "#00ffff",
                       "size": "normal"
                     }
-                  },
-                  {
-                    "type": "ghost",
-                    "logKey": "NT:/Vision/limelight/poseEstimate",
-                    "logType": "Pose2d",
-                    "visible": true,
-                    "options": {
-                      "color": "#00ff00"
-                    }
-                  },
-                  {
-                    "type": "vision",
-                    "logKey": "NT:/Vision/limelight/visibleAprilTagPoses",
-                    "logType": "Pose3d[]",
-                    "visible": true,
-                    "options": {
-                      "color": "#00ff00",
-                      "size": "normal"
-                    }
                   }
                 ],
                 "game": "2025 Field (Welded)",

src/main/java/com/team2813/vision/MultiPhotonPoseEstimator.java

@@ -3,6 +3,7 @@
 import static com.team2813.vision.CameraConstants.LIMELIGHT_CAMERA_NAME;
 import static com.team2813.vision.VisionNetworkTables.CAMERA_POSE_TOPIC;
 import static com.team2813.vision.VisionNetworkTables.getTableForCamera;
+import static com.team2813.vision.VisionNetworkTables.getVisionEstimatorTable;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -160,6 +161,15 @@ private record CameraData(
 
   /** Creates an instance using values from a {@code Builder}. */
   private MultiPhotonPoseEstimator(Builder builder) {
+    // Publish the pose strategy that was passed into the builder.
+    // The pose strategy is derived from data in Preferences, but the build() method is called when
+    // com.team2813.Robot is constructed (i.e. soon over the JVM is started), so if the preference
+    // value is updated in Elastic, the pose strategy wouldn't be recalculated right away.
+    getVisionEstimatorTable(builder.ntInstance)
+        .getStringTopic("poseStrategy")
+        .publish()
+        .set(builder.poseStrategy.name());
+
     for (var entry : builder.cameraConfigs.entrySet()) {
       String cameraName = entry.getKey();
       CameraConfig cameraConfig = entry.getValue();

src/main/java/com/team2813/vision/VisionNetworkTables.java

@@ -26,6 +26,17 @@ public final class VisionNetworkTables {
   static final String APRIL_TAG_POSE_TOPIC = "aprilTagPose";
 
   private static final String TABLE_NAME = "Vision";
+  private static final String CAMERA_SUBTABLE_NAME = "camera";
+  private static final String ESTIMATOR_SUBTABLE_NAME = "estimator";
+
+  /**
+   * Gets the network table to use to store data related to pose estimation.
+   *
+   * @param ntInstance network table instance to publish to.
+   */
+  public static NetworkTable getVisionEstimatorTable(NetworkTableInstance ntInstance) {
+    return ntInstance.getTable(TABLE_NAME).getSubTable(ESTIMATOR_SUBTABLE_NAME);
+  }
 
   /**
    * Gets the network table for the camera with the given name
@@ -34,7 +45,10 @@ public final class VisionNetworkTables {
    * @param cameraName name of the camera.
    */
   public static NetworkTable getTableForCamera(NetworkTableInstance ntInstance, String cameraName) {
-    return ntInstance.getTable(TABLE_NAME).getSubTable(cameraName);
+    return ntInstance
+        .getTable(TABLE_NAME)
+        .getSubTable(CAMERA_SUBTABLE_NAME)
+        .getSubTable(cameraName);
   }
 
   /** Gets the network table for the provided photon vision camera. */

src/test/java/com/team2813/vision/LimelightPosePublisherTest.java

@@ -17,7 +17,7 @@
 public class LimelightPosePublisherTest {
   private static final double FAKE_TIMESTAMP_OFFSET = 0.25;
   private static final Pose2d DEFAULT_POSE = new Pose2d(28, 13, Rotation2d.fromDegrees(45));
-  private static final String EXPECTED_TABLE_NAME = "Vision/limelight";
+  private static final String EXPECTED_TABLE_NAME = "Vision/camera/limelight";
 
   @Rule public final NetworkTableResource networkTable = new NetworkTableResource();
 

src/test/java/com/team2813/vision/PhotonVisionPosePublisherTest.java

@@ -23,7 +23,7 @@ public class PhotonVisionPosePublisherTest {
       new Pose3d(
           28, 13, 25, new Rotation3d(Math.toRadians(28), Math.toRadians(13), Math.toRadians(25)));
   private static final String CAMERA_NAME = "tweak";
-  private static final String EXPECTED_TABLE_NAME = "Vision/tweak";
+  private static final String EXPECTED_TABLE_NAME = "Vision/camera/tweak";
 
   @Rule public final NetworkTableResource networkTable = new NetworkTableResource();