diff --git a/AdvantageScope-simulator.json b/AdvantageScope-simulator.json index dfabfb1f..4e32bbce 100644 --- a/AdvantageScope-simulator.json +++ b/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)", diff --git a/src/main/java/com/team2813/vision/MultiPhotonPoseEstimator.java b/src/main/java/com/team2813/vision/MultiPhotonPoseEstimator.java index ccb8a200..3d762053 100644 --- a/src/main/java/com/team2813/vision/MultiPhotonPoseEstimator.java +++ b/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(); diff --git a/src/main/java/com/team2813/vision/VisionNetworkTables.java b/src/main/java/com/team2813/vision/VisionNetworkTables.java index 895e778c..7ed17e18 100644 --- a/src/main/java/com/team2813/vision/VisionNetworkTables.java +++ b/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. */ diff --git a/src/test/java/com/team2813/vision/LimelightPosePublisherTest.java b/src/test/java/com/team2813/vision/LimelightPosePublisherTest.java index 6f8452ec..df66d4d2 100644 --- a/src/test/java/com/team2813/vision/LimelightPosePublisherTest.java +++ b/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(); diff --git a/src/test/java/com/team2813/vision/PhotonVisionPosePublisherTest.java b/src/test/java/com/team2813/vision/PhotonVisionPosePublisherTest.java index f49bc013..7200dff3 100644 --- a/src/test/java/com/team2813/vision/PhotonVisionPosePublisherTest.java +++ b/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();