From b3099a303cfc57a7794f322034b09810f0d21bbe Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Wed, 8 Oct 2025 21:33:15 -0700 Subject: [PATCH 1/8] Add MultiPhotonPoseEstimator (from Robot2025) --- .../vision/MultiPhotonPoseEstimator.java | 378 ++++++++++++++++++ .../lib2813/vision/VisionNetworkTables.java | 3 + 2 files changed, 381 insertions(+) create mode 100644 vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java diff --git a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java new file mode 100644 index 00000000..94f4931f --- /dev/null +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -0,0 +1,378 @@ +package com.team2813.lib2813.vision; + +import static com.team2813.lib2813.vision.VisionNetworkTables.CAMERA_POSE_TOPIC; +import static com.team2813.lib2813.vision.VisionNetworkTables.getTableForCamera; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Objects; +import java.util.Optional; +import java.util.function.Consumer; +import java.util.function.Function; +import java.util.stream.Collectors; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; + +/** + * Provides estimated robot positions, in field pose, from multiple PhotonVision cameras. + * + *

This class manages one or more PhotonVision cameras, and provides an API {@link + * #update(Consumer)} to provide an updated estimated robot pose by combining readings from + * AprilTags visible from the cameras. It also supports adding camera configurations to + * PhotonVision's simulated vision system. + * + *

Note that, when we are dealing with 2D and 3D poses, we follow the transformation conventions + * established by WPILib and PhotonVision: + * https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html + * + *

Furthermore note that the global robot pose or any of the camera global poses are also + * referred to as "field-centric pose". In our libraries, field-centric poses are always specified + * relative to the blue origin per + * https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html#always-blue-origin + */ +public class MultiPhotonPoseEstimator implements AutoCloseable { + private static final String LIMELIGHT_CAMERA_NAME = "limelight"; + private final List cameraWrappers; + private PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; + + public static class Builder { + private final Map cameraConfigs = new HashMap<>(); + private final AprilTagFieldLayout aprilTagFieldLayout; + private final NetworkTableInstance ntInstance; + private final PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; + + /** + * MultiPhotonPoseEstimator builder constructor. + * + * @param ntInstance Network table instance used to log the pose of AprilTag detections as well + * as pose estimates. + * @param aprilTagFieldLayout WPILib field description (dimensions) including AprilTag 3D + * locations. + * @param poseEstimatorStrategy Posing strategy (for instance, multi tag PnP, closest to camera + * tag, etc.) + */ + public Builder( + NetworkTableInstance ntInstance, + AprilTagFieldLayout aprilTagFieldLayout, + PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy) { + this.ntInstance = Objects.requireNonNull(ntInstance, "ntInstance cannot be null"); + this.aprilTagFieldLayout = + Objects.requireNonNull(aprilTagFieldLayout, "aprilTagFieldLayout cannot be null"); + this.poseEstimatorStrategy = + Objects.requireNonNull(poseEstimatorStrategy, "poseEstimatorStrategy cannot be null"); + } + + /** + * Adds a camera to the multi pose estimator. + * + * @param name Unique name of the camera. + * @param transform 3D position of the camera relative to the robot frame. + * @return Builder instance. + */ + public Builder addCamera(String name, Transform3d transform) { + return addCamera(name, transform, Optional.empty()); + } + + /** + * Adds a camera to the multi pose estimator. + * + * @param name Unique name of the camera. + * @param transform 3D position of the camera relative to the robot frame. + * @param description Camera description. + * @return Builder instance. + */ + public Builder addCamera(String name, Transform3d transform, String description) { + return addCamera(name, transform, Optional.of(description)); + } + + /** + * Adds a camera to the multi pose estimator. + * + * @param name Unique name of the camera. + * @param transform 3D position of the camera relative to the robot frame. + * @param description Camera description. + * @return Builder instance. + */ + private Builder addCamera(String name, Transform3d transform, Optional description) { + Objects.requireNonNull(name, "camera name cannot be null"); + Objects.requireNonNull(transform, "transform cannot be null"); + if (name.equals(LIMELIGHT_CAMERA_NAME)) { + throw new IllegalArgumentException(String.format("Invalid camera name: '%s'", name)); + } + + if (cameraConfigs.put(name, new CameraConfig(transform, description)) != null) { + throw new IllegalArgumentException(String.format("Already a camera with name '%s'", name)); + } + return this; + } + + /** Builds a configured MultiPhotonPoseEstimator. */ + public MultiPhotonPoseEstimator build() { + return new MultiPhotonPoseEstimator(this); + } + } + + /** + * Adds the current Multi-Photon camera setup to a simulated vision system. + * + * @param simVisionSystem The simulated visual system. + * @param propertyFactory Functor that creates simulated camera properties. + */ + public void addToSim( + VisionSystemSim simVisionSystem, Function propertyFactory) { + cameraWrappers.forEach( + cameraWrapper -> { + SimCameraProperties cameraProp = propertyFactory.apply(cameraWrapper.camera.getName()); + PhotonCameraSim simCamera = new PhotonCameraSim(cameraWrapper.camera(), cameraProp); + simVisionSystem.addCamera(simCamera, cameraWrapper.estimator.getRobotToCameraTransform()); + }); + } + + /** Configuration for a camera that is connected to PhotonVision. */ + private record CameraConfig(Transform3d robotToCamera, Optional description) {} + + /** + * Wrapper containing a PhotonVision camera, pose estimator and publishers. + * + * @param camera A camera connected to PhotonVision. + * @param estimator A pose estimator configured for this camera. + * @param robotToCamera The 3D fixed pose of the camera relative to the robot. Intuitively, this + * field describes where on the robot the camera is mounted. + * @param robotPosePublisher A publisher reporting PhotonVision pose detections to NetworkTables + * during the robot runtime. + * @param cameraPosePublisher A publisher reporting the position of the camera in field-centric + * coordinates. In other words, this is the pose most recently set by {@link @setDrivePose} + * with the camera's own robotToCamera pose appended to it. + */ + private record PhotonCameraWrapper( + PhotonCamera camera, + PhotonPoseEstimator estimator, + Transform3d robotToCamera, + PhotonVisionPosePublisher robotPosePublisher, + StructPublisher cameraPosePublisher) + implements AutoCloseable { + + /** + * Publishes the estimated drive pose calculated from this camera. + * + * @param pose 3D field-centric (relative to blue origin) pose of the drive train. + */ + void publishEstimatedDrivePose(Pose3d pose) { + cameraPosePublisher.set(pose.plus(robotToCamera)); + } + + @Override + public void close() { + // TODO: Close publishers + camera.close(); + } + } + + /** Creates an instance using values from a {@code Builder}. */ + private MultiPhotonPoseEstimator(Builder builder) { + cameraWrappers = + builder.cameraConfigs.entrySet().stream() + .map(entry -> createCameraWrapperFromConfig(builder, entry.getKey(), entry.getValue())) + .collect(Collectors.toCollection(ArrayList::new)); + } + + /** + * Creates a {@link PhotonCameraWrapper} instance for a camera with the given name and + * configuration. + * + *

The returned value is used to get pose estimates from the camera. + */ + private static PhotonCameraWrapper createCameraWrapperFromConfig( + Builder builder, String cameraName, CameraConfig cameraConfig) { + PhotonCamera camera = new PhotonCamera(builder.ntInstance, cameraName); + PhotonPoseEstimator estimator = + new PhotonPoseEstimator( + builder.aprilTagFieldLayout, builder.poseEstimatorStrategy, cameraConfig.robotToCamera); + + // (TODO(vdikov): consider restructuring how the topics under which we publish in network + // tables are more explicitly listed somewhere, e.g., in a constants file of sorts. Right now, + // the actual path under which we publish is constructed across multiple levels of function + // calls, so if a software developer needs to track where a specific value they observe in, + // say, Advantage Scope is reported from, they have to trace through all these function + // calls. In contrast, the easiest paths to track from one system back to the code are the + // hard-coded paths - but that's not really an option here either, since we need dynamic + // information, like the camera name, to be part of the final topic path. Using template paths + // might be a good middle ground.) + // + // Note that some of the current code can be simplified when we remove the Limelight code + // (since VisionNetworkTable.getTableForLimelight() could be removed, allowing us to remove + // some of the levels of function calls). + + // Create NetworkTables publishers for 1) the position of the camera relative to the robot and + // 2) the estimated position provided by the camera. + NetworkTable table = getTableForCamera(camera); + StructPublisher cameraPosePublisher = + table.getStructTopic(CAMERA_POSE_TOPIC, Pose3d.struct).publish(); + var estimatedPosePublisher = new PhotonVisionPosePublisher(camera, builder.aprilTagFieldLayout); + + // If the caller provided a description for this camera, publish it to the camera network table. + cameraConfig.description.ifPresent( + description -> table.getEntry("description").setString(description)); + + return new PhotonCameraWrapper( + camera, estimator, cameraConfig.robotToCamera, estimatedPosePublisher, cameraPosePublisher); + } + + /** + * Gets the Position Estimation Strategy being used by the Position Estimators. + * + * @return the strategy + */ + public PhotonPoseEstimator.PoseStrategy getPrimaryStrategy() { + return poseEstimatorStrategy; + } + + /** + * Sets the Position Estimation Strategy used by the Position Estimators. + * + * @param poseStrategy the strategy to set + */ + public void setPrimaryStrategy(PhotonPoseEstimator.PoseStrategy poseStrategy) { + Objects.requireNonNull(poseStrategy, "poseStrategy cannot be null"); + if (!poseStrategy.equals(poseEstimatorStrategy)) { + cameraWrappers.forEach(wrapper -> wrapper.estimator.setPrimaryStrategy(poseStrategy)); + poseEstimatorStrategy = poseStrategy; + } + } + + /** + * Determines if the pose strategy requires addHeadingData() to be called with every frame. + * + * @return {@code true} if the pose strategy is documented to require addHeadingData(). + */ + public boolean poseStrategyRequiresHeadingData() { + return switch (poseEstimatorStrategy) { + case PNP_DISTANCE_TRIG_SOLVE, CONSTRAINED_SOLVEPNP -> true; + default -> false; + }; + } + + /** + * Sets a 2D pose estimate in a field-centric frame (relative to the blue origin). + * + *

This method takes a field-centric drive train pose (drive train and robot are the same + * here), update the camera field-centric poses and publish them on network tables. + * + *

TODO(vdikov): This method sits very counter-intiutively in this class. The class is all + * about estimating pose and feeding it to the drive train pose estimation. Yet, this method is + * feeding a drive-train `pose` back to it. One could reasonably assume that the drive train pose + * is somehow used for the multi-photon pose estimation and plays some role there. But the method + * code tells a much more prosaic story - the `pose` is only used for reporting camera poses, + * relative to "some" drive-train (at that point we don't even know if that pose is derived from + * the multi-photon pose estimation whatsoever). The MultiPhotonPoseEstimator API would become + * cleaner if we remove this method and find other ways to report Camera poses. kcooney@ has + * drafted several cool ideas how we can address that with a better class/interfaces architecture + * here: https://github.com/Prospect-Robotics/Robot2025/pull/157#discussion_r2282753534 + * + * @param pose 2D field-centric (relative to blue origin) pose of the drive train (i.e., the + * robot). + */ + public void setDrivePose(Pose2d pose) { + Pose3d pose3d = new Pose3d(pose); + for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { + cameraWrapper.publishEstimatedDrivePose(pose3d); + } + } + + /** + * Add robot heading data to buffer. Must be called periodically for the + * PNP_DISTANCE_TRIG_SOLVE strategy. + * + * @param timestampSeconds timestamp of the robot heading data. + * @param heading Field-relative robot heading at given timestamp. Standard WPILIB field + * coordinates. + */ + public void addHeadingData(double timestampSeconds, Rotation2d heading) { + for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { + cameraWrapper.estimator.addHeadingData(timestampSeconds, heading); + } + } + + /** + * Add robot heading data to buffer. Must be called periodically for the + * PNP_DISTANCE_TRIG_SOLVE strategy. + * + * @param timestampSeconds timestamp of the robot heading data. + * @param heading Field-relative robot heading at given timestamp. Standard WPILIB field + * coordinates. + */ + public void addHeadingData(double timestampSeconds, Rotation3d heading) { + for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { + cameraWrapper.estimator.addHeadingData(timestampSeconds, heading); + } + } + + /** + * Clears all heading data in the buffer, and adds a new seed. Useful for preventing estimates + * from utilizing heading data provided prior to a pose or rotation reset. + * + * @param timestampSeconds timestamp of the robot heading data. + * @param heading Field-relative robot heading at given timestamp. Standard WPILIB field + * coordinates. + */ + public void resetHeadingData(double timestampSeconds, Rotation2d heading) { + for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { + cameraWrapper.estimator.resetHeadingData(timestampSeconds, heading); + } + } + + public void resetHeadingData(double timestampSeconds, Rotation3d heading) { + // TODO: Use PhotonPoseEstimator.resetHeadingData(double, Rotation2d) once we use a version of + // PhotonVision that includes it (see https://github.com/PhotonVision/photonvision/pull/2013). + for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { + cameraWrapper.estimator.resetHeadingData(timestampSeconds, heading.toRotation2d()); + cameraWrapper.estimator.addHeadingData(timestampSeconds, heading); + } + } + + /** + * Takes a consumer for estimated poses and applies all unread robot-pose estimations from all + * cameras against `apply`. + * + *

This method is supposed to be called from a routine updating drive-train pose with pose + * estimates from the photon vision cameras. + * + *

TODO(vdikov): Further ideas how to refactor this interface are suggested by kcooney@ in this + * comment https://github.com/Prospect-Robotics/Robot2025/pull/157#discussion_r2282806711 + * + * @param apply Callback to consume unread photonevision robot-pose estimations. + */ + public void update(Consumer apply) { + for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { + List poses = + cameraWrapper.camera.getAllUnreadResults().stream() + .map(cameraWrapper.estimator::update) + .flatMap(Optional::stream) + .toList(); + + poses.forEach(apply); + cameraWrapper.robotPosePublisher.publish(poses); + } + } + + @Override + public void close() { + cameraWrappers.forEach(PhotonCameraWrapper::close); + cameraWrappers.clear(); + } +} diff --git a/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java b/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java index 966b28a4..7f26d1d8 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java @@ -8,6 +8,9 @@ * Contains methods and constants for publishing data from robot vision systems to network tables. */ final class VisionNetworkTables { + /** Topic name to use when publishing the Pose3d position of a camera. */ + static final String CAMERA_POSE_TOPIC = "cameraPose"; + /** Topic name to use when publishing the estimated robot position as a Pose2d value */ static final String POSE_ESTIMATE_TOPIC = "poseEstimate"; From 1872af949337fe33096992164d09613897e37c10 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Thu, 9 Oct 2025 01:59:06 -0700 Subject: [PATCH 2/8] Improve Javadoc --- .../vision/MultiPhotonPoseEstimator.java | 44 +++++++++---------- 1 file changed, 20 insertions(+), 24 deletions(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java index 94f4931f..0532a2ff 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -31,25 +31,26 @@ /** * Provides estimated robot positions, in field pose, from multiple PhotonVision cameras. * - *

This class manages one or more PhotonVision cameras, and provides an API {@link - * #update(Consumer)} to provide an updated estimated robot pose by combining readings from - * AprilTags visible from the cameras. It also supports adding camera configurations to + *

This class manages one or more PhotonVision cameras, and provides an API ({@link + * #processAll(PoseEstimateConsumer)}) to provide an updated estimated robot pose by combining + * readings from AprilTags visible from the cameras. It also supports adding the cameras to * PhotonVision's simulated vision system. * - *

Note that, when we are dealing with 2D and 3D poses, we follow the transformation conventions - * established by WPILib and PhotonVision: - * https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html + *

Note that, when we are dealing with 2D and 3D poses, we follow the transformation conventions established by WPILib and PhotonVision. * *

Furthermore note that the global robot pose or any of the camera global poses are also - * referred to as "field-centric pose". In our libraries, field-centric poses are always specified - * relative to the blue origin per - * https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html#always-blue-origin + * referred to as "field-centric pose". In our libraries, field-centric poses are always specified relative to the blue origin. */ public class MultiPhotonPoseEstimator implements AutoCloseable { private static final String LIMELIGHT_CAMERA_NAME = "limelight"; private final List cameraWrappers; private PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; + /** A builder for {@code MultiPhotonPoseEstimator}. */ public static class Builder { private final Map cameraConfigs = new HashMap<>(); private final AprilTagFieldLayout aprilTagFieldLayout; @@ -57,7 +58,7 @@ public static class Builder { private final PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; /** - * MultiPhotonPoseEstimator builder constructor. + * {@code MultiPhotonPoseEstimator} builder constructor. * * @param ntInstance Network table instance used to log the pose of AprilTag detections as well * as pose estimates. @@ -168,7 +169,7 @@ private record PhotonCameraWrapper( implements AutoCloseable { /** - * Publishes the estimated drive pose calculated from this camera. + * Publishes the position of this camera. * * @param pose 3D field-centric (relative to blue origin) pose of the drive train. */ @@ -273,21 +274,16 @@ public boolean poseStrategyRequiresHeadingData() { *

This method takes a field-centric drive train pose (drive train and robot are the same * here), update the camera field-centric poses and publish them on network tables. * - *

TODO(vdikov): This method sits very counter-intiutively in this class. The class is all - * about estimating pose and feeding it to the drive train pose estimation. Yet, this method is - * feeding a drive-train `pose` back to it. One could reasonably assume that the drive train pose - * is somehow used for the multi-photon pose estimation and plays some role there. But the method - * code tells a much more prosaic story - the `pose` is only used for reporting camera poses, - * relative to "some" drive-train (at that point we don't even know if that pose is derived from - * the multi-photon pose estimation whatsoever). The MultiPhotonPoseEstimator API would become - * cleaner if we remove this method and find other ways to report Camera poses. kcooney@ has - * drafted several cool ideas how we can address that with a better class/interfaces architecture - * here: https://github.com/Prospect-Robotics/Robot2025/pull/157#discussion_r2282753534 - * - * @param pose 2D field-centric (relative to blue origin) pose of the drive train (i.e., the - * robot). + * @param pose 2D field-centric (relative to blue origin) pose. */ public void setDrivePose(Pose2d pose) { + // TODO(vdikov): This method sits very counter-intuitively in this class. The class is all about + // estimating pose and feeding it to the drive train pose estimation. Yet, this method is + // feeding a drive-train `pose` back to it. The MultiPhotonPoseEstimator API would become + // cleaner if we remove this method and find other ways to report Camera poses. kcooney@ has + // provided several some ideas on how we can address that with a better class/interfaces + // architecture here: + // https://github.com/Prospect-Robotics/Robot2025/pull/157#discussion_r2282753534 Pose3d pose3d = new Pose3d(pose); for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { cameraWrapper.publishEstimatedDrivePose(pose3d); From a9037b52417fd2144d3157f5ed95493341da830e Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 12 Oct 2025 15:13:55 -0700 Subject: [PATCH 3/8] Simplify MultiPhotonPoseEstimator --- .../vision/MultiPhotonPoseEstimator.java | 180 ++++++++++-------- .../vision/PhotonVisionPosePublisher.java | 20 +- .../lib2813/vision/PoseEstimateConsumer.java | 14 ++ .../lib2813/vision/VisionNetworkTables.java | 9 +- 4 files changed, 129 insertions(+), 94 deletions(-) create mode 100644 vision/src/main/java/com/team2813/lib2813/vision/PoseEstimateConsumer.java diff --git a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java index 0532a2ff..d99de61a 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -2,6 +2,8 @@ import static com.team2813.lib2813.vision.VisionNetworkTables.CAMERA_POSE_TOPIC; import static com.team2813.lib2813.vision.VisionNetworkTables.getTableForCamera; +import static java.util.stream.Collectors.toCollection; +import static java.util.stream.Collectors.toMap; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Pose2d; @@ -18,9 +20,7 @@ import java.util.Map; import java.util.Objects; import java.util.Optional; -import java.util.function.Consumer; -import java.util.function.Function; -import java.util.stream.Collectors; +import java.util.function.Supplier; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -32,9 +32,9 @@ * Provides estimated robot positions, in field pose, from multiple PhotonVision cameras. * *

This class manages one or more PhotonVision cameras, and provides an API ({@link - * #processAll(PoseEstimateConsumer)}) to provide an updated estimated robot pose by combining - * readings from AprilTags visible from the cameras. It also supports adding the cameras to - * PhotonVision's simulated vision system. + * #processAllUnreadResults(PoseEstimateConsumer)}) to provide an updated estimated robot pose by + * combining readings from AprilTags visible from the cameras. It also supports adding the cameras + * to * PhotonVision's simulated vision system. * *

Note that, when we are dealing with 2D and 3D poses, we follow always specified relative to the blue origin. */ public class MultiPhotonPoseEstimator implements AutoCloseable { - private static final String LIMELIGHT_CAMERA_NAME = "limelight"; private final List cameraWrappers; private PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; @@ -81,7 +80,8 @@ public Builder( /** * Adds a camera to the multi pose estimator. * - * @param name Unique name of the camera. + * @param name Unique name of the camera. It is recommended for this to describe the camera's + * location (ex: "frontLeft"). * @param transform 3D position of the camera relative to the robot frame. * @return Builder instance. */ @@ -90,33 +90,30 @@ public Builder addCamera(String name, Transform3d transform) { } /** - * Adds a camera to the multi pose estimator. + * Adds a camera and associated simulator properties to the multi pose estimator. * - * @param name Unique name of the camera. + * @param name Unique name of the camera. It is recommended for this to describe the camera's + * location (ex: "frontLeft"). * @param transform 3D position of the camera relative to the robot frame. - * @param description Camera description. + * @param simulationPropertiesSupplier Factory for providing simulation properties for the + * camera. This is only called when {@link #addCamerasToSimulator(VisionSystemSim)} is + * called. * @return Builder instance. */ - public Builder addCamera(String name, Transform3d transform, String description) { - return addCamera(name, transform, Optional.of(description)); + public Builder addCamera( + String name, + Transform3d transform, + Supplier simulationPropertiesSupplier) { + return addCamera(name, transform, Optional.of(simulationPropertiesSupplier)); } - /** - * Adds a camera to the multi pose estimator. - * - * @param name Unique name of the camera. - * @param transform 3D position of the camera relative to the robot frame. - * @param description Camera description. - * @return Builder instance. - */ - private Builder addCamera(String name, Transform3d transform, Optional description) { + private Builder addCamera( + String name, + Transform3d transform, + Optional> simPropertiesSupplier) { Objects.requireNonNull(name, "camera name cannot be null"); Objects.requireNonNull(transform, "transform cannot be null"); - if (name.equals(LIMELIGHT_CAMERA_NAME)) { - throw new IllegalArgumentException(String.format("Invalid camera name: '%s'", name)); - } - - if (cameraConfigs.put(name, new CameraConfig(transform, description)) != null) { + if (cameraConfigs.put(name, new CameraConfig(transform, simPropertiesSupplier)) != null) { throw new IllegalArgumentException(String.format("Already a camera with name '%s'", name)); } return this; @@ -129,23 +126,37 @@ public MultiPhotonPoseEstimator build() { } /** - * Adds the current Multi-Photon camera setup to a simulated vision system. + * Adds all cameras to a simulated vision system. * * @param simVisionSystem The simulated visual system. - * @param propertyFactory Functor that creates simulated camera properties. */ - public void addToSim( - VisionSystemSim simVisionSystem, Function propertyFactory) { + public void addCamerasToSimulator(VisionSystemSim simVisionSystem) { + // Validate all inputs and create SimCameraProperties for each camera. + Map cameraNameToSimProperties = + cameraWrappers.stream() + .collect( + toMap( + wrapper -> wrapper.camera.getName(), PhotonCameraWrapper::createSimProperties)); + + // Add cameras to the simulated vision system cameraWrappers.forEach( - cameraWrapper -> { - SimCameraProperties cameraProp = propertyFactory.apply(cameraWrapper.camera.getName()); - PhotonCameraSim simCamera = new PhotonCameraSim(cameraWrapper.camera(), cameraProp); - simVisionSystem.addCamera(simCamera, cameraWrapper.estimator.getRobotToCameraTransform()); + wrapper -> { + SimCameraProperties cameraProps = cameraNameToSimProperties.get(wrapper.camera.getName()); + PhotonCameraSim simCamera = new PhotonCameraSim(wrapper.camera(), cameraProps); + simVisionSystem.addCamera(simCamera, wrapper.estimator.getRobotToCameraTransform()); }); } - /** Configuration for a camera that is connected to PhotonVision. */ - private record CameraConfig(Transform3d robotToCamera, Optional description) {} + /** + * Configuration for a camera that is connected to PhotonVision. + * + * @param robotToCamera The 3D fixed pose of the camera relative to the robot. Intuitively, this * + * field describes where on the robot the camera is mounted. + * @param simulationPropertiesSupplier Factory for providing simulation properties for the camera. + */ + private record CameraConfig( + Transform3d robotToCamera, + Optional> simulationPropertiesSupplier) {} /** * Wrapper containing a PhotonVision camera, pose estimator and publishers. @@ -154,6 +165,7 @@ private record CameraConfig(Transform3d robotToCamera, Optional descript * @param estimator A pose estimator configured for this camera. * @param robotToCamera The 3D fixed pose of the camera relative to the robot. Intuitively, this * field describes where on the robot the camera is mounted. + * @param simPropertiesSupplier Factory for providing simulation properties for the camera. * @param robotPosePublisher A publisher reporting PhotonVision pose detections to NetworkTables * during the robot runtime. * @param cameraPosePublisher A publisher reporting the position of the camera in field-centric @@ -164,6 +176,7 @@ private record PhotonCameraWrapper( PhotonCamera camera, PhotonPoseEstimator estimator, Transform3d robotToCamera, + Optional> simPropertiesSupplier, PhotonVisionPosePublisher robotPosePublisher, StructPublisher cameraPosePublisher) implements AutoCloseable { @@ -171,16 +184,44 @@ private record PhotonCameraWrapper( /** * Publishes the position of this camera. * - * @param pose 3D field-centric (relative to blue origin) pose of the drive train. + * @param robotPose 3D field-centric (relative to blue origin) pose of the drive train. */ - void publishEstimatedDrivePose(Pose3d pose) { - cameraPosePublisher.set(pose.plus(robotToCamera)); + void publishCameraPose(Pose3d robotPose) { + cameraPosePublisher.set(robotPose.plus(robotToCamera)); + } + + /** + * Create calibration and performance values for this camera using the caller-provided supplier. + * + * @throws IllegalStateException if the caller did not provide a supplier. + * @throws NullPointerException if the caller-provided supplier returns {@code null}. + */ + private SimCameraProperties createSimProperties() { + SimCameraProperties simProperties = + simPropertiesSupplier + .orElseThrow( + () -> + new IllegalStateException( + String.format( + "Must pass Supplier to addCamera() to use camera" + + " %s in simulation", + camera().getName()))) + .get(); + if (simProperties == null) { + throw new NullPointerException( + String.format( + "Supplier passed to addCamera(\"%s\", ...) cannot provide null" + + " values", + camera().getName())); + } + return simProperties; } @Override public void close() { - // TODO: Close publishers camera.close(); + cameraPosePublisher.close(); + // TODO: Update PhotonVisionPosePublisher to support close() and call it here } } @@ -189,11 +230,11 @@ private MultiPhotonPoseEstimator(Builder builder) { cameraWrappers = builder.cameraConfigs.entrySet().stream() .map(entry -> createCameraWrapperFromConfig(builder, entry.getKey(), entry.getValue())) - .collect(Collectors.toCollection(ArrayList::new)); + .collect(toCollection(ArrayList::new)); } /** - * Creates a {@link PhotonCameraWrapper} instance for a camera with the given name and + * Creates a {@link PhotonCameraWrapper} instance for a camera with the given name and camera * configuration. * *

The returned value is used to get pose estimates from the camera. @@ -205,33 +246,21 @@ private static PhotonCameraWrapper createCameraWrapperFromConfig( new PhotonPoseEstimator( builder.aprilTagFieldLayout, builder.poseEstimatorStrategy, cameraConfig.robotToCamera); - // (TODO(vdikov): consider restructuring how the topics under which we publish in network - // tables are more explicitly listed somewhere, e.g., in a constants file of sorts. Right now, - // the actual path under which we publish is constructed across multiple levels of function - // calls, so if a software developer needs to track where a specific value they observe in, - // say, Advantage Scope is reported from, they have to trace through all these function - // calls. In contrast, the easiest paths to track from one system back to the code are the - // hard-coded paths - but that's not really an option here either, since we need dynamic - // information, like the camera name, to be part of the final topic path. Using template paths - // might be a good middle ground.) - // - // Note that some of the current code can be simplified when we remove the Limelight code - // (since VisionNetworkTable.getTableForLimelight() could be removed, allowing us to remove - // some of the levels of function calls). - // Create NetworkTables publishers for 1) the position of the camera relative to the robot and // 2) the estimated position provided by the camera. - NetworkTable table = getTableForCamera(camera); + NetworkTable parentTable = getTableForCamera(camera); StructPublisher cameraPosePublisher = - table.getStructTopic(CAMERA_POSE_TOPIC, Pose3d.struct).publish(); - var estimatedPosePublisher = new PhotonVisionPosePublisher(camera, builder.aprilTagFieldLayout); - - // If the caller provided a description for this camera, publish it to the camera network table. - cameraConfig.description.ifPresent( - description -> table.getEntry("description").setString(description)); + parentTable.getStructTopic(CAMERA_POSE_TOPIC, Pose3d.struct).publish(); + var estimatedPosePublisher = + new PhotonVisionPosePublisher(parentTable, builder.aprilTagFieldLayout); return new PhotonCameraWrapper( - camera, estimator, cameraConfig.robotToCamera, estimatedPosePublisher, cameraPosePublisher); + camera, + estimator, + cameraConfig.robotToCamera, + cameraConfig.simulationPropertiesSupplier, + estimatedPosePublisher, + cameraPosePublisher); } /** @@ -269,14 +298,13 @@ public boolean poseStrategyRequiresHeadingData() { } /** - * Sets a 2D pose estimate in a field-centric frame (relative to the blue origin). + * Publishes the position of all the cameras, relative to the given position. * - *

This method takes a field-centric drive train pose (drive train and robot are the same - * here), update the camera field-centric poses and publish them on network tables. + *

Callers will typically pass a field-centric drive train pose. * * @param pose 2D field-centric (relative to blue origin) pose. */ - public void setDrivePose(Pose2d pose) { + public void publishCameraPosesRelativeTo(Pose2d pose) { // TODO(vdikov): This method sits very counter-intuitively in this class. The class is all about // estimating pose and feeding it to the drive train pose estimation. Yet, this method is // feeding a drive-train `pose` back to it. The MultiPhotonPoseEstimator API would become @@ -286,7 +314,7 @@ public void setDrivePose(Pose2d pose) { // https://github.com/Prospect-Robotics/Robot2025/pull/157#discussion_r2282753534 Pose3d pose3d = new Pose3d(pose); for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { - cameraWrapper.publishEstimatedDrivePose(pose3d); + cameraWrapper.publishCameraPose(pose3d); } } @@ -342,18 +370,14 @@ public void resetHeadingData(double timestampSeconds, Rotation3d heading) { } /** - * Takes a consumer for estimated poses and applies all unread robot-pose estimations from all - * cameras against `apply`. + * Sends all unread robot-pose estimations from all cameras to the provided consumer. * *

This method is supposed to be called from a routine updating drive-train pose with pose * estimates from the photon vision cameras. * - *

TODO(vdikov): Further ideas how to refactor this interface are suggested by kcooney@ in this - * comment https://github.com/Prospect-Robotics/Robot2025/pull/157#discussion_r2282806711 - * - * @param apply Callback to consume unread photonevision robot-pose estimations. + * @param poseEstimateConsumer Functional interface for consuming computed pose estimates. */ - public void update(Consumer apply) { + public void processAllUnreadResults(PoseEstimateConsumer poseEstimateConsumer) { for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { List poses = cameraWrapper.camera.getAllUnreadResults().stream() @@ -361,7 +385,7 @@ public void update(Consumer apply) { .flatMap(Optional::stream) .toList(); - poses.forEach(apply); + poses.forEach(poseEstimateConsumer::addEstimatedRobotPose); cameraWrapper.robotPosePublisher.publish(poses); } } diff --git a/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java index 2fab4c58..4c1a77e4 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java @@ -2,7 +2,6 @@ import static com.team2813.lib2813.vision.VisionNetworkTables.APRIL_TAG_POSE_TOPIC; import static com.team2813.lib2813.vision.VisionNetworkTables.POSE_ESTIMATE_TOPIC; -import static com.team2813.lib2813.vision.VisionNetworkTables.getTableForCamera; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Pose3d; @@ -14,7 +13,6 @@ import java.util.Optional; import java.util.function.Supplier; import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonTrackedTarget; /** @@ -38,28 +36,28 @@ public final class PhotonVisionPosePublisher { private final AprilTagFieldLayout aprilTagFieldLayout; /** - * Creates a publisher for the provided camera and field layout. + * Creates a publisher that publishes values under the given table. * - * @param camera Camera to use to get the Network Tables name to publish to. - * @param aprilTagFieldLayout Layout of AprilTags on a field. + * @param parentTable Parent table for all topics published by this publisher instance. + * @param aprilTagFieldLayout Layout of AprilTags on the field. */ - public PhotonVisionPosePublisher(PhotonCamera camera, AprilTagFieldLayout aprilTagFieldLayout) { - this(camera, aprilTagFieldLayout, Timer::getFPGATimestamp); + public PhotonVisionPosePublisher( + NetworkTable parentTable, AprilTagFieldLayout aprilTagFieldLayout) { + this(parentTable, aprilTagFieldLayout, Timer::getFPGATimestamp); } /** Package-scoped constructor (for unit testing). */ PhotonVisionPosePublisher( - PhotonCamera camera, + NetworkTable parentTable, AprilTagFieldLayout aprilTagFieldLayout, Supplier fpgaTimestampSupplier) { this.aprilTagFieldLayout = aprilTagFieldLayout; - NetworkTable table = getTableForCamera(camera); - StructTopic topic = table.getStructTopic(POSE_ESTIMATE_TOPIC, Pose3d.struct); + StructTopic topic = parentTable.getStructTopic(POSE_ESTIMATE_TOPIC, Pose3d.struct); robotPosePublisher = new TimestampedStructPublisher<>(topic, Pose3d.kZero, fpgaTimestampSupplier); robotPosePublisher.setTimeUntilStale( EXPECTED_MILLIS_BETWEEN_POSE_ESTIMATES, Units.Milliseconds); - topic = table.getStructTopic(APRIL_TAG_POSE_TOPIC, Pose3d.struct); + topic = parentTable.getStructTopic(APRIL_TAG_POSE_TOPIC, Pose3d.struct); aprilTagPosePublisher = new TimestampedStructPublisher<>(topic, Pose3d.kZero, fpgaTimestampSupplier); aprilTagPosePublisher.setTimeUntilStale( diff --git a/vision/src/main/java/com/team2813/lib2813/vision/PoseEstimateConsumer.java b/vision/src/main/java/com/team2813/lib2813/vision/PoseEstimateConsumer.java new file mode 100644 index 00000000..f6864b3c --- /dev/null +++ b/vision/src/main/java/com/team2813/lib2813/vision/PoseEstimateConsumer.java @@ -0,0 +1,14 @@ +package com.team2813.lib2813.vision; + +import org.photonvision.EstimatedRobotPose; + +/** Represents an operation that accepts estimated robot positions. */ +@FunctionalInterface +public interface PoseEstimateConsumer { + /** + * Performs an operation on the given estimated robot positions. + * + * @param estimatedPose The estimated robot positions. + */ + void addEstimatedRobotPose(EstimatedRobotPose estimatedPose); +} diff --git a/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java b/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java index 7f26d1d8..ba82165e 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java @@ -1,7 +1,6 @@ package com.team2813.lib2813.vision; import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import org.photonvision.PhotonCamera; /** @@ -17,16 +16,16 @@ final class VisionNetworkTables { /** Topic name to use when publishing the position of the detected AprilTag as a Pose2d value. */ static final String APRIL_TAG_POSE_TOPIC = "aprilTagPose"; - private static final String TABLE_NAME = "Vision"; + /** Name of the subtable under `photonvision/[cameraName]/' where topics are added. */ + private static final String SUBTABLE_NAME = "LatestPose"; /** * Gets the network table for the provided photon vision camera to use for publishing data. * - *

The key of the network table will be `Vision/[cameraName]`. + *

The key of the network table will be `photonvision/[cameraName]/LatestPose`. */ public static NetworkTable getTableForCamera(PhotonCamera camera) { - NetworkTableInstance ntInstance = camera.getCameraTable().getInstance(); - return ntInstance.getTable(TABLE_NAME).getSubTable(camera.getName()); + return camera.getCameraTable().getSubTable(SUBTABLE_NAME); } private VisionNetworkTables() { From f1502fe883c64d618c1ccdec0155ffed8463d0ef Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 19 Oct 2025 16:39:33 -0700 Subject: [PATCH 4/8] Add comments --- .../com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java index d99de61a..b23de131 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -381,8 +381,8 @@ public void processAllUnreadResults(PoseEstimateConsumer poseEstimateConsumer) { for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { List poses = cameraWrapper.camera.getAllUnreadResults().stream() - .map(cameraWrapper.estimator::update) - .flatMap(Optional::stream) + .map(cameraWrapper.estimator::update) // PhotonPipelineResult -> EstimatedRobotPose + .flatMap(Optional::stream) // Convert Stream> -> Stream

.toList(); poses.forEach(poseEstimateConsumer::addEstimatedRobotPose); From dbc02566f8f24ee0339c7209faaa34fd44a55171 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Fri, 24 Oct 2025 10:31:00 -0700 Subject: [PATCH 5/8] Fix javadoc --- .../com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java index b23de131..5ba64c19 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -34,7 +34,7 @@ *

This class manages one or more PhotonVision cameras, and provides an API ({@link * #processAllUnreadResults(PoseEstimateConsumer)}) to provide an updated estimated robot pose by * combining readings from AprilTags visible from the cameras. It also supports adding the cameras - * to * PhotonVision's simulated vision system. + * to PhotonVision's simulated vision system. * *

Note that, when we are dealing with 2D and 3D poses, we follow Date: Fri, 24 Oct 2025 10:46:46 -0700 Subject: [PATCH 6/8] Remove comment (now tracked by #90) --- .../team2813/lib2813/vision/MultiPhotonPoseEstimator.java | 7 ------- 1 file changed, 7 deletions(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java index 5ba64c19..7c3fccb6 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -305,13 +305,6 @@ public boolean poseStrategyRequiresHeadingData() { * @param pose 2D field-centric (relative to blue origin) pose. */ public void publishCameraPosesRelativeTo(Pose2d pose) { - // TODO(vdikov): This method sits very counter-intuitively in this class. The class is all about - // estimating pose and feeding it to the drive train pose estimation. Yet, this method is - // feeding a drive-train `pose` back to it. The MultiPhotonPoseEstimator API would become - // cleaner if we remove this method and find other ways to report Camera poses. kcooney@ has - // provided several some ideas on how we can address that with a better class/interfaces - // architecture here: - // https://github.com/Prospect-Robotics/Robot2025/pull/157#discussion_r2282753534 Pose3d pose3d = new Pose3d(pose); for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { cameraWrapper.publishCameraPose(pose3d); From 9a8aa46cab75242cad3df72e8c4f31d47117a558 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Mon, 17 Nov 2025 20:15:03 -0800 Subject: [PATCH 7/8] Fix MultiPhotonPoseEstimator constructor to initialize poseEstimatorStrategy --- .../vision/MultiPhotonPoseEstimator.java | 1 + .../vision/MultiPhotonPoseEstimatorTest.java | 53 +++++++++++++++++++ 2 files changed, 54 insertions(+) create mode 100644 vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java diff --git a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java index 7c3fccb6..7b0a2283 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -227,6 +227,7 @@ public void close() { /** Creates an instance using values from a {@code Builder}. */ private MultiPhotonPoseEstimator(Builder builder) { + poseEstimatorStrategy = builder.poseEstimatorStrategy; cameraWrappers = builder.cameraConfigs.entrySet().stream() .map(entry -> createCameraWrapperFromConfig(builder, entry.getKey(), entry.getValue())) diff --git a/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java b/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java new file mode 100644 index 00000000..c4346cb7 --- /dev/null +++ b/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java @@ -0,0 +1,53 @@ +package com.team2813.lib2813.vision; + +import static com.google.common.truth.Truth.assertThat; + +import com.team2813.lib2813.testing.junit.jupiter.IsolatedNetworkTablesExtension; +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.networktables.NetworkTableInstance; +import java.util.List; +import org.junit.jupiter.api.extension.ExtendWith; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.EnumSource; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; + +/** Tests for {@link MultiPhotonPoseEstimator}. */ +@ExtendWith(IsolatedNetworkTablesExtension.class) +class MultiPhotonPoseEstimatorTest { + private static final double FIELD_LENGTH = 17.548; + private static final double FIELD_WIDTH = 8.052; + private static final int REEFSCAPE_APRIL_TAG_ID = 7; + private static final Pose3d REEFSCAPE_APRIL_TAG_POSE = + new Pose3d( + new Translation3d(13.890498, 4.0259, 0.308102), + new Rotation3d(new Quaternion(1.0, 0.0, 0.0, 0.0))); + private static final Transform3d FRONT_CAMERA_TRANSFORM = + new Transform3d( + 0.1688157406, + 0.2939800826, + 0.1708140348, + new Rotation3d(0, -0.1745329252, -0.5235987756)); + + @ParameterizedTest + @EnumSource(value = PoseStrategy.class) + void getPrimaryStrategy(PoseStrategy poseStrategy, NetworkTableInstance ntInstance) { + try (var estimator = + new MultiPhotonPoseEstimator.Builder(ntInstance, createFieldLayout(), poseStrategy) + .addCamera("front", FRONT_CAMERA_TRANSFORM) + .build()) { + assertThat(estimator.getPrimaryStrategy()).isEqualTo(poseStrategy); + } + } + + private static AprilTagFieldLayout createFieldLayout() { + List aprilTags = + List.of(new AprilTag(REEFSCAPE_APRIL_TAG_ID, REEFSCAPE_APRIL_TAG_POSE)); + return new AprilTagFieldLayout(aprilTags, FIELD_LENGTH, FIELD_WIDTH); + } +} From 7aaccfe5b5eccd7acd1c5cd7aab1436c5c9323c7 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Mon, 29 Dec 2025 19:56:29 -0800 Subject: [PATCH 8/8] Fix javadoc Co-authored-by: cuttestkittensrule --- .../com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java index 7b0a2283..250d9a2a 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -150,7 +150,7 @@ public void addCamerasToSimulator(VisionSystemSim simVisionSystem) { /** * Configuration for a camera that is connected to PhotonVision. * - * @param robotToCamera The 3D fixed pose of the camera relative to the robot. Intuitively, this * + * @param robotToCamera The 3D fixed pose of the camera relative to the robot. Intuitively, this * field describes where on the robot the camera is mounted. * @param simulationPropertiesSupplier Factory for providing simulation properties for the camera. */