diff --git a/core/src/test/java/com/team2813/lib2813/preferences/PersistedConfigurationTest.java b/core/src/test/java/com/team2813/lib2813/preferences/PersistedConfigurationTest.java index 19d1d9fd..f2976298 100644 --- a/core/src/test/java/com/team2813/lib2813/preferences/PersistedConfigurationTest.java +++ b/core/src/test/java/com/team2813/lib2813/preferences/PersistedConfigurationTest.java @@ -39,6 +39,7 @@ import java.util.function.*; import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Disabled; import org.junit.jupiter.api.Nested; import org.junit.jupiter.api.Test; import org.junit.jupiter.api.function.Executable; @@ -46,6 +47,7 @@ import org.junit.jupiter.params.provider.ValueSource; /** Tests for {@link PersistedConfiguration}. */ +@Disabled("WPILib will often crash due to https://github.com/wpilibsuite/allwpilib/issues/8215") @ProvideUniqueNetworkTableInstance(replacePreferencesNetworkTable = true) public final class PersistedConfigurationTest { private static final double EPSILON = 0.001; diff --git a/vision/src/main/java/com/team2813/lib2813/vision/Camera.java b/vision/src/main/java/com/team2813/lib2813/vision/Camera.java new file mode 100644 index 00000000..047d064b --- /dev/null +++ b/vision/src/main/java/com/team2813/lib2813/vision/Camera.java @@ -0,0 +1,86 @@ +/* +Copyright 2026 Prospect Robotics SWENext Club + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + +http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*/ +package com.team2813.lib2813.vision; + +import edu.wpi.first.math.geometry.Transform3d; +import java.util.Objects; +import java.util.Optional; +import java.util.function.Supplier; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; + +/** + * A camera on a robot. + * + *

This class can be extended to add additional metadata about the camera. + * + * @since 2.0.0 + */ +public class Camera { + private final String name; + private final Transform3d robotToCamera; + protected final Optional> simPropertiesSupplier; + + /** + * 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 robotToCamera 3D position of the camera relative to the robot frame. + */ + public Camera(String name, Transform3d robotToCamera) { + this(name, robotToCamera, 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 robotToCamera 3D position of the camera relative to the robot frame. + * @param simPropertiesSupplier Factory for providing simulation properties for the camera. This + * is only called when {@link MultiPhotonPoseEstimator#addCamerasToSimulator(VisionSystemSim)} + * is called. + */ + public Camera( + String name, Transform3d robotToCamera, Supplier simPropertiesSupplier) { + this(name, robotToCamera, Optional.of(simPropertiesSupplier)); + } + + private Camera( + String name, + Transform3d robotToCamera, + Optional> simPropertiesSupplier) { + Objects.requireNonNull(name, "camera name cannot be null"); + Objects.requireNonNull(robotToCamera, "robotToCamera cannot be null"); + if (name.isEmpty()) { + throw new IllegalArgumentException("camera name cannot be empty"); + } + this.name = name; + this.robotToCamera = robotToCamera; + this.simPropertiesSupplier = simPropertiesSupplier; + } + + /** Gets the name of the camera. */ + public final String name() { + return name; + } + + /** Gets the 3D position of the camera relative to the robot frame. */ + public final Transform3d robotToCamera() { + return robotToCamera; + } +} 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 589e26ee..0e728a5a 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -25,7 +25,6 @@ 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; @@ -35,7 +34,9 @@ import java.util.Map; import java.util.Objects; import java.util.Optional; -import java.util.function.Supplier; +import java.util.function.Consumer; +import java.util.function.Predicate; +import java.util.stream.Collectors; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -60,18 +61,21 @@ * href="https://docs.wpilib.org/en/stable/docs/software/basic-programming/coordinate-system.html#always-blue-origin" * target="_top">always specified relative to the blue origin. * + * @param the type for the camera * @since 2.0.0 */ -public class MultiPhotonPoseEstimator implements AutoCloseable { - private final List cameraWrappers; +public class MultiPhotonPoseEstimator implements AutoCloseable { + private final List> cameraWrappers; + private final Predicate isPoseValid; private PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; /** A builder for {@code MultiPhotonPoseEstimator}. */ - public static final class Builder { - private final Map cameraConfigs = new HashMap<>(); + public static final class Builder { + private final Map cameras = new HashMap<>(); private final AprilTagFieldLayout aprilTagFieldLayout; private final NetworkTableInstance ntInstance; private final PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; + private Predicate isPoseValid = pose -> true; Builder( NetworkTableInstance ntInstance, @@ -87,48 +91,32 @@ public static final class Builder { /** * 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. + * @param camera The camera. Must have a unique name. * @return Builder instance. */ - public Builder addCamera(String name, Transform3d transform) { - return addCamera(name, transform, Optional.empty()); + public Builder addCamera(C camera) { + if (cameras.put(camera.name(), camera) != null) { + throw new IllegalArgumentException( + String.format("Already a camera with name '%s'", camera.name())); + } + return this; } /** - * Adds a camera and associated simulator properties to the multi pose estimator. + * Sets the filter to use deciding which estimates to consider. * - * @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. + * @param isPoseValid A predicate that returns {@code true} if the pose should be considered + * valid. * @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)); - } + public Builder withPoseFilter(Predicate isPoseValid) { + this.isPoseValid = isPoseValid; return this; } /** Builds a configured MultiPhotonPoseEstimator. */ - public MultiPhotonPoseEstimator build() { - return new MultiPhotonPoseEstimator(this); + public MultiPhotonPoseEstimator build() { + return new MultiPhotonPoseEstimator<>(this); } } @@ -141,17 +129,40 @@ public MultiPhotonPoseEstimator build() { * locations. * @param poseEstimatorStrategy Posing strategy (for instance, multi tag PnP, closest to camera * tag, etc.) + * @param cameraType The type for the camera. + */ + public static Builder builder( + NetworkTableInstance ntInstance, + AprilTagFieldLayout aprilTagFieldLayout, + PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy, + Class cameraType) { + return new Builder<>(ntInstance, aprilTagFieldLayout, poseEstimatorStrategy); + } + + /** + * Creates a builder for building {@link MultiPhotonPoseEstimator} instances with a custom Camera + * type, + * + * @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 static Builder builder( + public static Builder builder( NetworkTableInstance ntInstance, AprilTagFieldLayout aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy) { - return new Builder(ntInstance, aprilTagFieldLayout, poseEstimatorStrategy); + return builder(ntInstance, aprilTagFieldLayout, poseEstimatorStrategy, Camera.class); } /** * Adds all cameras to a simulated vision system. * + *

Note that the robot code is responsible for calling {@link VisionSystemSim#update(Pose2d)} + * or {@link VisionSystemSim#update(Pose3d)} in {@code simulationPeriodic()}. + * * @param simVisionSystem The simulated visual system. */ public void addCamerasToSimulator(VisionSystemSim simVisionSystem) { @@ -159,48 +170,33 @@ public void addCamerasToSimulator(VisionSystemSim simVisionSystem) { Map cameraNameToSimProperties = cameraWrappers.stream() .collect( - toMap( - wrapper -> wrapper.camera.getName(), PhotonCameraWrapper::createSimProperties)); + toMap(wrapper -> wrapper.camera.name(), 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); + SimCameraProperties cameraProps = cameraNameToSimProperties.get(wrapper.camera.name()); + PhotonCameraSim simCamera = new PhotonCameraSim(wrapper.photonCamera, 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 camera The camera. + * @param photonCamera 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, + private record PhotonCameraWrapper( + C camera, + PhotonCamera photonCamera, PhotonPoseEstimator estimator, - Transform3d robotToCamera, - Optional> simPropertiesSupplier, PhotonVisionPosePublisher robotPosePublisher, StructPublisher cameraPosePublisher) implements AutoCloseable { @@ -211,7 +207,7 @@ private record PhotonCameraWrapper( * @param robotPose 3D field-centric (relative to blue origin) pose of the drive train. */ void publishCameraPose(Pose3d robotPose) { - cameraPosePublisher.set(robotPose.plus(robotToCamera)); + cameraPosePublisher.set(robotPose.plus(camera.robotToCamera())); } /** @@ -222,39 +218,41 @@ void publishCameraPose(Pose3d robotPose) { */ private SimCameraProperties createSimProperties() { SimCameraProperties simProperties = - simPropertiesSupplier + camera + .simPropertiesSupplier .orElseThrow( () -> new IllegalStateException( String.format( "Must pass Supplier to addCamera() to use camera" + " %s in simulation", - camera().getName()))) + camera().name()))) .get(); if (simProperties == null) { throw new NullPointerException( String.format( "Supplier passed to addCamera(\"%s\", ...) cannot provide null" + " values", - camera().getName())); + camera().name())); } return simProperties; } @Override public void close() { - camera.close(); + photonCamera.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) { + private MultiPhotonPoseEstimator(Builder builder) { poseEstimatorStrategy = builder.poseEstimatorStrategy; + isPoseValid = poseIsInField(builder.aprilTagFieldLayout).and(builder.isPoseValid); cameraWrappers = - builder.cameraConfigs.entrySet().stream() - .map(entry -> createCameraWrapperFromConfig(builder, entry.getKey(), entry.getValue())) + builder.cameras.values().stream() + .map(camera -> createCameraWrapper(builder, camera)) .collect(toCollection(ArrayList::new)); } @@ -264,28 +262,23 @@ private MultiPhotonPoseEstimator(Builder builder) { * *

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); + private static PhotonCameraWrapper createCameraWrapper( + Builder builder, C camera) { + PhotonCamera photonCamera = new PhotonCamera(builder.ntInstance, camera.name()); PhotonPoseEstimator estimator = new PhotonPoseEstimator( - builder.aprilTagFieldLayout, builder.poseEstimatorStrategy, cameraConfig.robotToCamera); + builder.aprilTagFieldLayout, builder.poseEstimatorStrategy, camera.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); + NetworkTable parentTable = getTableForCamera(photonCamera); 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); + return new PhotonCameraWrapper<>( + camera, photonCamera, estimator, estimatedPosePublisher, cameraPosePublisher); } /** @@ -331,7 +324,7 @@ public boolean poseStrategyRequiresHeadingData() { */ public void publishCameraPosesRelativeTo(Pose2d pose) { Pose3d pose3d = new Pose3d(pose); - for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { + for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { cameraWrapper.publishCameraPose(pose3d); } } @@ -345,7 +338,7 @@ public void publishCameraPosesRelativeTo(Pose2d pose) { * coordinates. */ public void addHeadingData(double timestampSeconds, Rotation2d heading) { - for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { + for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { cameraWrapper.estimator.addHeadingData(timestampSeconds, heading); } } @@ -359,7 +352,7 @@ public void addHeadingData(double timestampSeconds, Rotation2d heading) { * coordinates. */ public void addHeadingData(double timestampSeconds, Rotation3d heading) { - for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { + for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { cameraWrapper.estimator.addHeadingData(timestampSeconds, heading); } } @@ -373,7 +366,7 @@ public void addHeadingData(double timestampSeconds, Rotation3d heading) { * coordinates. */ public void resetHeadingData(double timestampSeconds, Rotation2d heading) { - for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { + for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { cameraWrapper.estimator.resetHeadingData(timestampSeconds, heading); } } @@ -381,30 +374,51 @@ public void resetHeadingData(double timestampSeconds, Rotation2d 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) { + 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. + * Sends all validated 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. + *

This method should 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. + * @param poseEstimateConsumer Consumer for validated pose estimates. */ - public void processAllUnreadResults(PoseEstimateConsumer poseEstimateConsumer) { - for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { - List poses = - cameraWrapper.camera.getAllUnreadResults().stream() + public void processAllUnreadResults(PoseEstimateConsumer poseEstimateConsumer) { + processAllUnreadResults(poseEstimateConsumer, pose -> {}); + } + + /** + * Sends all unread robot-pose estimations from all cameras to the provided consumers. + * + *

This method should be called from a routine updating drive-train pose with pose estimates + * from the photon vision cameras. + * + * @param poseEstimateConsumer Consumer for validated pose estimates. + * @param rejectedPoseConsumer Consumer for rejected pose estimates. + */ + public void processAllUnreadResults( + PoseEstimateConsumer poseEstimateConsumer, + Consumer rejectedPoseConsumer) { + for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { + Map> poses = + cameraWrapper.photonCamera.getAllUnreadResults().stream() .map(cameraWrapper.estimator::update) // PhotonPipelineResult -> EstimatedRobotPose .flatMap(Optional::stream) // Convert Stream> -> Stream

- .toList(); + .collect(Collectors.partitioningBy(isPoseValid)); + + List validatedPoses = poses.get(Boolean.TRUE); + for (EstimatedRobotPose pose : validatedPoses) { + poseEstimateConsumer.addEstimatedRobotPose(pose, cameraWrapper.camera); + } + cameraWrapper.robotPosePublisher.publish(validatedPoses); - poses.forEach(poseEstimateConsumer::addEstimatedRobotPose); - cameraWrapper.robotPosePublisher.publish(poses); + List rejectedPoses = poses.get(Boolean.FALSE); + rejectedPoses.forEach(rejectedPoseConsumer); } } @@ -413,4 +427,19 @@ public void close() { cameraWrappers.forEach(PhotonCameraWrapper::close); cameraWrappers.clear(); } + + /** Creates a predicate that determines if a pose is inside the field. */ + private static Predicate poseIsInField( + AprilTagFieldLayout aprilTagFieldLayout) { + return pose -> { + Pose3d estimate = pose.estimatedPose; + double x = estimate.getX(); + double y = estimate.getY(); + + return x >= 0.0 + && x <= aprilTagFieldLayout.getFieldLength() + && y >= 0.0 + && y <= aprilTagFieldLayout.getFieldWidth(); + }; + } } diff --git a/vision/src/main/java/com/team2813/lib2813/vision/PoseEstimateConsumer.java b/vision/src/main/java/com/team2813/lib2813/vision/PoseEstimateConsumer.java index f6bff896..d4a5c69b 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/PoseEstimateConsumer.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/PoseEstimateConsumer.java @@ -20,14 +20,16 @@ /** * Represents an operation that accepts estimated robot positions. * + * @param the type for the camera * @since 2.0.0 */ @FunctionalInterface -public interface PoseEstimateConsumer { +public interface PoseEstimateConsumer { /** * Performs an operation on the given estimated robot positions. * * @param estimatedPose The estimated robot positions. + * @param camera The camera. */ - void addEstimatedRobotPose(EstimatedRobotPose estimatedPose); + void addEstimatedRobotPose(EstimatedRobotPose estimatedPose, C camera); } diff --git a/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java b/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java index 73f650e6..447ad23b 100644 --- a/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java +++ b/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java @@ -48,12 +48,14 @@ class MultiPhotonPoseEstimatorTest { 0.1708140348, new Rotation3d(0, -0.1745329252, -0.5235987756)); + private static final Camera FRONT_CAMERA = new Camera("front", FRONT_CAMERA_TRANSFORM); + @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) + MultiPhotonPoseEstimator.builder(ntInstance, createFieldLayout(), poseStrategy) + .addCamera(FRONT_CAMERA) .build()) { assertThat(estimator.getPrimaryStrategy()).isEqualTo(poseStrategy); }