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..250d9a2a --- /dev/null +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -0,0 +1,392 @@ +package com.team2813.lib2813.vision; + +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; +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.Supplier; +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 + * #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 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. + */ +public class MultiPhotonPoseEstimator implements AutoCloseable { + 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; + private final NetworkTableInstance ntInstance; + private final PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; + + /** + * {@code 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. 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. + */ + public Builder addCamera(String name, Transform3d transform) { + return addCamera(name, transform, Optional.empty()); + } + + /** + * Adds a camera and associated simulator properties to the multi pose estimator. + * + * @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 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, + Supplier simulationPropertiesSupplier) { + return addCamera(name, transform, Optional.of(simulationPropertiesSupplier)); + } + + 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 (cameraConfigs.put(name, new CameraConfig(transform, simPropertiesSupplier)) != 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 all cameras to a simulated vision system. + * + * @param simVisionSystem The simulated visual system. + */ + 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( + 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. + * + * @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. + * + * @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 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 + * 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, + Optional> simPropertiesSupplier, + PhotonVisionPosePublisher robotPosePublisher, + StructPublisher cameraPosePublisher) + implements AutoCloseable { + + /** + * Publishes the position of this camera. + * + * @param robotPose 3D field-centric (relative to blue origin) pose of the drive train. + */ + 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() { + camera.close(); + cameraPosePublisher.close(); + // TODO: Update PhotonVisionPosePublisher to support close() and call it here + } + } + + /** 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())) + .collect(toCollection(ArrayList::new)); + } + + /** + * 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. + */ + 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); + + // Create NetworkTables publishers for 1) the position of the camera relative to the robot and + // 2) the estimated position provided by the camera. + NetworkTable parentTable = getTableForCamera(camera); + StructPublisher cameraPosePublisher = + parentTable.getStructTopic(CAMERA_POSE_TOPIC, Pose3d.struct).publish(); + var estimatedPosePublisher = + new PhotonVisionPosePublisher(parentTable, builder.aprilTagFieldLayout); + + return new PhotonCameraWrapper( + camera, + estimator, + cameraConfig.robotToCamera, + cameraConfig.simulationPropertiesSupplier, + 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; + }; + } + + /** + * Publishes the position of all the cameras, relative to the given position. + * + *

Callers will typically pass a field-centric drive train pose. + * + * @param pose 2D field-centric (relative to blue origin) pose. + */ + public void publishCameraPosesRelativeTo(Pose2d pose) { + Pose3d pose3d = new Pose3d(pose); + for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { + cameraWrapper.publishCameraPose(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); + } + } + + /** + * 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. + * + * @param poseEstimateConsumer Functional interface for consuming computed pose estimates. + */ + public void processAllUnreadResults(PoseEstimateConsumer poseEstimateConsumer) { + for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { + List poses = + cameraWrapper.camera.getAllUnreadResults().stream() + .map(cameraWrapper.estimator::update) // PhotonPipelineResult -> EstimatedRobotPose + .flatMap(Optional::stream) // Convert Stream> -> Stream

+ .toList(); + + poses.forEach(poseEstimateConsumer::addEstimatedRobotPose); + 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/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 966b28a4..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,29 +1,31 @@ package com.team2813.lib2813.vision; import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; import org.photonvision.PhotonCamera; /** * 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"; /** 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() { 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); + } +}