diff --git a/src/main/deploy/choreo/C-Source2.traj b/src/main/deploy/choreo/C-Source2.traj index f487fb82..f07c18ac 100644 --- a/src/main/deploy/choreo/C-Source2.traj +++ b/src/main/deploy/choreo/C-Source2.traj @@ -13,7 +13,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"C.x", "val":3.7114241123199463}, "y":{"exp":"C.y", "val":2.9666056632995605}, "heading":{"exp":"C.heading", "val":2.606637673727935}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"C.x", "val":3.657078504562378}, "y":{"exp":"C.y", "val":2.9981400966644287}, "heading":{"exp":"C.heading", "val":2.606637673727935}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"Source2.x", "val":0.7617058753967285}, "y":{"exp":"Source2.y", "val":1.2630654573440552}, "heading":{"exp":"Source2.heading", "val":2.498091607901954}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, diff --git a/src/main/deploy/choreo/D-Source2.traj b/src/main/deploy/choreo/D-Source2.traj index 92c08ede..ccb55be8 100644 --- a/src/main/deploy/choreo/D-Source2.traj +++ b/src/main/deploy/choreo/D-Source2.traj @@ -13,7 +13,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"D.x", "val":4.044410705566406}, "y":{"exp":"D.y", "val":2.776327610015869}, "heading":{"exp":"D.heading", "val":2.606637673727935}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"D.x", "val":3.9831042289733887}, "y":{"exp":"D.y", "val":2.799689769744873}, "heading":{"exp":"D.heading", "val":2.606637673727935}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"Source2.x", "val":0.7617058753967285}, "y":{"exp":"Source2.y", "val":1.2630654573440552}, "heading":{"exp":"Source2.heading", "val":2.498091607901954}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, diff --git a/src/main/deploy/choreo/H-Source2.traj b/src/main/deploy/choreo/H-Source2.traj index 97fad212..0c18fc91 100644 --- a/src/main/deploy/choreo/H-Source2.traj +++ b/src/main/deploy/choreo/H-Source2.traj @@ -14,7 +14,7 @@ }, "params":{ "waypoints":[ - {"x":{"exp":"H.x", "val":5.785916328430176}, "y":{"exp":"H.y", "val":4.243002891540527}, "heading":{"exp":"H.heading", "val":4.71238898038469}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"H.x", "val":5.8544793128967285}, "y":{"exp":"H.y", "val":4.183961868286133}, "heading":{"exp":"H.heading", "val":4.71238898038469}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"5.847537517547607 m", "val":5.847537517547607}, "y":{"exp":"3.057382345199585 m", "val":3.057382345199585}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":62, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, {"x":{"exp":"Source2.x", "val":0.7617058753967285}, "y":{"exp":"Source2.y", "val":1.2630654573440552}, "heading":{"exp":"Source2.heading", "val":2.498091607901954}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ diff --git a/src/main/deploy/choreo/MID-H.traj b/src/main/deploy/choreo/MID-H.traj index 0ee6eb94..a255e49c 100644 --- a/src/main/deploy/choreo/MID-H.traj +++ b/src/main/deploy/choreo/MID-H.traj @@ -14,7 +14,7 @@ "params":{ "waypoints":[ {"x":{"exp":"7.228333473205566 m", "val":7.228333473205566}, "y":{"exp":"4.211480140686035 m", "val":4.211480140686035}, "heading":{"exp":"270 deg", "val":4.71238898038469}, "intervals":23, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"H.x", "val":5.785916328430176}, "y":{"exp":"H.y", "val":4.243002891540527}, "heading":{"exp":"H.heading", "val":4.71238898038469}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"H.x", "val":5.8544793128967285}, "y":{"exp":"H.y", "val":4.183961868286133}, "heading":{"exp":"H.heading", "val":4.71238898038469}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, diff --git a/src/main/deploy/choreo/New Path (5).traj b/src/main/deploy/choreo/New Path (5).traj new file mode 100644 index 00000000..96dfc5a1 --- /dev/null +++ b/src/main/deploy/choreo/New Path (5).traj @@ -0,0 +1,29 @@ +{ + "name":"New Path (5)", + "version":1, + "snapshot":{ + "waypoints":[], + "constraints":[], + "targetDt":0.05 + }, + "params":{ + "waypoints":[ + {"x":{"exp":"4.507959842681885 m", "val":4.507959842681885}, "y":{"exp":"4.0053911209106445 m", "val":4.0053911209106445}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, + {"x":{"exp":"6.465804576873779 m", "val":6.465804576873779}, "y":{"exp":"4.026000022888184 m", "val":4.026000022888184}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + "constraints":[ + {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false}], + "targetDt":{ + "exp":"0.05 s", + "val":0.05 + } + }, + "trajectory":{ + "sampleType":null, + "waypoints":[], + "samples":[], + "splits":[] + }, + "events":[] +} diff --git a/src/main/deploy/choreo/Source2-C.traj b/src/main/deploy/choreo/Source2-C.traj index 02fcd8ec..3927e09a 100644 --- a/src/main/deploy/choreo/Source2-C.traj +++ b/src/main/deploy/choreo/Source2-C.traj @@ -17,7 +17,7 @@ "waypoints":[ {"x":{"exp":"Source2.x", "val":0.7617058753967285}, "y":{"exp":"Source2.y", "val":1.2630654573440552}, "heading":{"exp":"Source2.heading", "val":2.498091607901954}, "intervals":46, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"2.75620436668396 m", "val":2.75620436668396}, "y":{"exp":"2.418506622314453 m", "val":2.418506622314453}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":38, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"C.x", "val":3.7114241123199463}, "y":{"exp":"C.y", "val":2.9666056632995605}, "heading":{"exp":"C.heading", "val":2.606637673727935}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"C.x", "val":3.657078504562378}, "y":{"exp":"C.y", "val":2.9981400966644287}, "heading":{"exp":"C.heading", "val":2.606637673727935}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, diff --git a/src/main/deploy/choreo/Source2-D.traj b/src/main/deploy/choreo/Source2-D.traj index d87573ab..ed675752 100644 --- a/src/main/deploy/choreo/Source2-D.traj +++ b/src/main/deploy/choreo/Source2-D.traj @@ -14,7 +14,7 @@ "params":{ "waypoints":[ {"x":{"exp":"Source2.x", "val":0.7617058753967285}, "y":{"exp":"Source2.y", "val":1.2630654573440552}, "heading":{"exp":"Source2.heading", "val":2.498091607901954}, "intervals":39, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, - {"x":{"exp":"D.x", "val":4.044410705566406}, "y":{"exp":"D.y", "val":2.776327610015869}, "heading":{"exp":"D.heading", "val":2.606637673727935}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"D.x", "val":3.9831042289733887}, "y":{"exp":"D.y", "val":2.799689769744873}, "heading":{"exp":"D.heading", "val":2.606637673727935}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, diff --git a/src/main/deploy/choreo/Source2-H.traj b/src/main/deploy/choreo/Source2-H.traj index cba9da9e..bdb15caa 100644 --- a/src/main/deploy/choreo/Source2-H.traj +++ b/src/main/deploy/choreo/Source2-H.traj @@ -16,7 +16,7 @@ "waypoints":[ {"x":{"exp":"Source2.x", "val":0.7617058753967285}, "y":{"exp":"Source2.y", "val":1.2630654573440552}, "heading":{"exp":"Source2.heading", "val":2.498091607901954}, "intervals":62, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}, {"x":{"exp":"5.8681464195251465 m", "val":5.8681464195251465}, "y":{"exp":"2.8306844234466553 m", "val":2.8306844234466553}, "heading":{"exp":"0 deg", "val":0.0}, "intervals":37, "split":false, "fixTranslation":true, "fixHeading":false, "overrideIntervals":false}, - {"x":{"exp":"H.x", "val":5.785916328430176}, "y":{"exp":"H.y", "val":4.243002891540527}, "heading":{"exp":"H.heading", "val":4.71238898038469}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], + {"x":{"exp":"H.x", "val":5.8544793128967285}, "y":{"exp":"H.y", "val":4.183961868286133}, "heading":{"exp":"H.heading", "val":4.71238898038469}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}], "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ba9510af..7a3b2f2c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -43,10 +43,7 @@ import frc.robot.subsystems.endeffector.EndEffectorIOTalonFX; import frc.robot.subsystems.swerve.CommandSwerveDrivetrain; import frc.robot.subsystems.swerve.generated.TunerConstants; -import frc.robot.subsystems.vision.Vision; -import frc.robot.subsystems.vision.VisionConstants; -import frc.robot.subsystems.vision.VisionIOPhotonVision; -import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; +import frc.robot.subsystems.vision.*; import frc.robot.utils.MappedXboxController; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.CoralTargets; @@ -88,18 +85,29 @@ public class RobotContainer { private final Vision vision = new Vision( drivetrain::addPhotonEstimate, + drivetrain::shouldUseTrig, Utils.isSimulation() ? new VisionIOPhotonVisionSim( VisionConstants.leftCam, VisionConstants.robotToLeftCam, () -> drivetrain.getState().Pose) - : new VisionIOPhotonVision(VisionConstants.leftCam, VisionConstants.robotToLeftCam), + : new VisionIOPhotonVisionTrig( + VisionConstants.leftCam, + VisionConstants.robotToLeftCam, + () -> drivetrain.getPigeon2().getRotation2d(), + Utils::getCurrentTimeSeconds, + drivetrain::shouldUseTrig), Utils.isSimulation() ? new VisionIOPhotonVisionSim( VisionConstants.rightCam, VisionConstants.robotToRightCam, () -> drivetrain.getState().Pose) - : new VisionIOPhotonVision(VisionConstants.rightCam, VisionConstants.robotToRightCam), + : new VisionIOPhotonVisionTrig( + VisionConstants.rightCam, + VisionConstants.robotToRightCam, + () -> drivetrain.getPigeon2().getRotation2d(), + Utils::getCurrentTimeSeconds, + drivetrain::shouldUseTrig), Utils.isSimulation() ? new VisionIOPhotonVisionSim( VisionConstants.backCam, diff --git a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java index d219dff9..10f6f956 100644 --- a/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/swerve/CommandSwerveDrivetrain.java @@ -39,9 +39,11 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.FieldConstants; import frc.robot.drivers.QuestNav; import frc.robot.subsystems.swerve.generated.TunerConstants; import frc.robot.subsystems.swerve.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.subsystems.vision.VisionConstants; import frc.robot.utils.LoggedTracer; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -177,6 +179,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su SwerveConstants.frontRight, SwerveConstants.backLeft, SwerveConstants.backRight); + /* WPILib Alerts start */ private final Alert a_questNavNotConnected = @@ -295,6 +298,11 @@ public void trajLogger(Trajectory sample, boolean isStart) { Logger.recordOutput(this.getClass().getSimpleName() + "/Choreo/TrajPoses", sample.getPoses()); } + public boolean shouldUseTrig() { + return this.getState().Pose.getTranslation().getDistance(FieldConstants.Reef.center) + < VisionConstants.distanceToUseTrigMeters; + } + /** * Creates a new auto factory for this drivetrain with the given trajectory logger. * diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 54a9a6d8..eb8f98da 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.utils.LoggedTracer; +import java.util.function.BooleanSupplier; import org.littletonrobotics.junction.Logger; public class Vision extends SubsystemBase { @@ -25,10 +26,12 @@ public class Vision extends SubsystemBase { private final VisionIO[] io; private final VisionIOInputsAutoLogged[] inputs; private final Alert[] disconnectedAlerts; + private final BooleanSupplier shouldUseTrig; - public Vision(VisionConsumer consumer, VisionIO... io) { + public Vision(VisionConsumer consumer, BooleanSupplier shouldUseTrig, VisionIO... io) { this.consumer = consumer; this.io = io; + this.shouldUseTrig = shouldUseTrig; // Initialize inputs this.inputs = new VisionIOInputsAutoLogged[io.length]; @@ -65,6 +68,11 @@ public void periodic() { // Loop over pose observations for (var observation : inputs[cameraIndex].poseObservations) { + + if (observation.type().equals(VisionIO.PoseObservationType.MULTITAG) + && shouldUseTrig.getAsBoolean()) { + continue; + } // Check whether to reject pose boolean rejectPose = observation.tagCount() == 0 // Must have at least one tag @@ -87,6 +95,9 @@ public void periodic() { // Calculate standard deviations double stdDevFactor = Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount(); + if (observation.type().equals(VisionIO.PoseObservationType.TRIG)) { + stdDevFactor /= 2; + } double linearStdDev = linearStdDevBaseline * stdDevFactor; double angularStdDev = angularStdDevBaseline * stdDevFactor; if (cameraIndex < cameraStdDevFactors.length) { diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 265931cc..69097ef5 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -24,6 +24,10 @@ public class VisionConstants { public static String frontCam = "front"; public static String backCam = "back"; + public static int[] reefIds = new int[] {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; + + public static final double distanceToUseTrigMeters = 2.5; + // Robot to camera transforms // (Not used by Limelight, configure in web UI instead) public static Transform3d robotToLeftCam = @@ -64,14 +68,9 @@ public class VisionConstants { // (Adjust to trust some cameras more than others) public static double[] cameraStdDevFactors = new double[] { - 1.5, // Camera 0 + 1, // Camera 0 1, // Camera 1 1.5, 1.5 // Camera 3 }; - - // Multipliers to apply for MegaTag 2 observations - public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve - public static double angularStdDevMegatag2Factor = - Double.POSITIVE_INFINITY; // No rotation data available } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIO.java b/src/main/java/frc/robot/subsystems/vision/VisionIO.java index 94e88e85..1d15e799 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIO.java @@ -8,7 +8,6 @@ package frc.robot.subsystems.vision; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import org.littletonrobotics.junction.AutoLog; public interface VisionIO { @@ -19,9 +18,6 @@ public static class VisionIOInputs { public int[] tagIds = new int[0]; } - /** Represents the angle to a simple target, not used for pose estimation. */ - public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} - /** Represents a robot pose sample used for pose estimation. */ public static record PoseObservation( double timestamp, @@ -32,9 +28,8 @@ public static record PoseObservation( PoseObservationType type) {} public static enum PoseObservationType { - MEGATAG_1, - MEGATAG_2, - PHOTONVISION + MULTITAG, + TRIG } public default void updateInputs(VisionIOInputs inputs) {} diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java index 6ef80bc7..85ea4619 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java @@ -69,7 +69,7 @@ public void updateInputs(VisionIOInputs inputs) { multitagResult.estimatedPose.ambiguity, // Ambiguity multitagResult.fiducialIDsUsed.size(), // Tag count totalTagDistance / result.targets.size(), // Average tag distance - PoseObservationType.PHOTONVISION)); // Observation type + PoseObservationType.MULTITAG)); // Observation type } else if (!result.targets.isEmpty()) { // Single tag result var target = result.targets.get(0); @@ -95,7 +95,7 @@ public void updateInputs(VisionIOInputs inputs) { target.poseAmbiguity, // Ambiguity 1, // Tag count cameraToTarget.getTranslation().getNorm(), // Average tag distance - PoseObservationType.PHOTONVISION)); // Observation type + PoseObservationType.MULTITAG)); // Observation type } } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionTrig.java b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionTrig.java new file mode 100644 index 00000000..f12c7a26 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionTrig.java @@ -0,0 +1,157 @@ +// Copyright (c) 2025 FRC 3256 +// https://github.com/Team3256 +// +// Use of this source code is governed by a +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.subsystems.vision; + +import static frc.robot.subsystems.vision.VisionConstants.*; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import java.util.*; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; + +/** IO implementation for real PhotonVision hardware. */ +public class VisionIOPhotonVisionTrig implements VisionIO { + protected final PhotonCamera camera; + protected final Transform3d robotToCamera; + protected final PhotonPoseEstimator photonPoseEstimator; + + protected Supplier headingSupplier; + protected DoubleSupplier headingTimeSupplier; + + protected BooleanSupplier shouldUseTrig; + + /** + * Creates a new VisionIOPhotonVision. + * + * @param name The configured name of the camera. + * @param robotToCamera The 3D position of the camera relative to the robot. + */ + public VisionIOPhotonVisionTrig( + String name, + Transform3d robotToCamera, + Supplier headingSupplier, + DoubleSupplier headingTimeSupplier, + BooleanSupplier shouldUseTrig) { + camera = new PhotonCamera(name); + this.robotToCamera = robotToCamera; + this.headingSupplier = headingSupplier; + this.headingTimeSupplier = headingTimeSupplier; + photonPoseEstimator = + new PhotonPoseEstimator( + aprilTagLayout, + PhotonPoseEstimator.PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, + robotToCamera); + this.shouldUseTrig = shouldUseTrig; + } + + @Override + public void updateInputs(VisionIOInputs inputs) { + photonPoseEstimator.addHeadingData(headingTimeSupplier.getAsDouble(), headingSupplier.get()); + inputs.connected = camera.isConnected(); + + // Read new camera observations + Set tagIds = new HashSet<>(); + List poseObservations = new LinkedList<>(); + for (var result : camera.getAllUnreadResults()) { + // Update latest target observation + + // Add pose observation + + if (shouldUseTrig.getAsBoolean() + && result.getTargets().stream() + .anyMatch(target -> Arrays.stream(reefIds).anyMatch(id -> id == target.fiducialId))) { + Optional robotPose = photonPoseEstimator.update(result); + if (robotPose.isEmpty()) { + continue; + } + poseObservations.add( + new PoseObservation( + robotPose.get().timestampSeconds, + robotPose.get().estimatedPose, + robotPose.get().targetsUsed.get(0).poseAmbiguity, + 1, + robotPose.get().targetsUsed.get(0).bestCameraToTarget.getTranslation().getNorm(), + PoseObservationType.TRIG)); + continue; + } + if (result.multitagResult.isPresent()) { // Multitag result + var multitagResult = result.multitagResult.get(); + + // Calculate robot pose + Transform3d fieldToCamera = multitagResult.estimatedPose.best; + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + // Calculate average tag distance + double totalTagDistance = 0.0; + for (var target : result.targets) { + totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm(); + } + + // Add tag IDs + tagIds.addAll(multitagResult.fiducialIDsUsed); + + // Add observation + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + multitagResult.estimatedPose.ambiguity, // Ambiguity + multitagResult.fiducialIDsUsed.size(), // Tag count + totalTagDistance / result.targets.size(), // Average tag distance + PoseObservationType.MULTITAG)); // Observation type + + } else if (!result.targets.isEmpty()) { // Single tag result + var target = result.targets.get(0); + + // Calculate robot pose + var tagPose = aprilTagLayout.getTagPose(target.fiducialId); + if (tagPose.isPresent()) { + Transform3d fieldToTarget = + new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); + Transform3d cameraToTarget = target.bestCameraToTarget; + Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse()); + Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse()); + Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation()); + + // Add tag ID + tagIds.add((short) target.fiducialId); + + // Add observation + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + target.poseAmbiguity, // Ambiguity + 1, // Tag count + cameraToTarget.getTranslation().getNorm(), // Average tag distance + PoseObservationType.MULTITAG)); // Observation type + } + } + } + + // Save pose observations to inputs object + inputs.poseObservations = new PoseObservation[poseObservations.size()]; + for (int i = 0; i < poseObservations.size(); i++) { + inputs.poseObservations[i] = poseObservations.get(i); + } + + // Save tag IDs to inputs objects + inputs.tagIds = new int[tagIds.size()]; + int i = 0; + for (int id : tagIds) { + inputs.tagIds[i++] = id; + } + } +}