From 1dee1848d5cbf50b2c77ec2e7501ee7dbfdb2cc7 Mon Sep 17 00:00:00 2001 From: Niti Date: Mon, 5 Oct 2026 19:27:28 -0400 Subject: [PATCH 1/4] created basis for stereovision files --- .../subsystems/stereoVision/StereoVision.java | 7 ++++ .../stereoVision/StereoVisionConstants.java | 37 +++++++++++++++++++ .../stereoVision/StereoVisionIO.java | 20 ++++++++++ .../StereoVisionIOPhotonVision.java | 3 ++ 4 files changed, 67 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java create mode 100644 src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java create mode 100644 src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIO.java create mode 100644 src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java new file mode 100644 index 000000000..06ce6a871 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.stereoVision; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class StereoVision extends SubsystemBase { + +} diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java new file mode 100644 index 000000000..b6d1ce552 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java @@ -0,0 +1,37 @@ +package frc.robot.subsystems.stereoVision; + +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.math.util.Units; + +public class StereoVisionConstants { + // TODO: chnange names accordingly + public static String camera0Name = "camera_0"; + public static String camera1Name = "camera_1"; + + //TODO: get correct + public static Transform3d robotToCamera0 = + new Transform3d( // front camera + new Translation3d( + Units.inchesToMeters(8.779), + Units.inchesToMeters(10.445), + Units.inchesToMeters(27.152 + 1.75)) + .rotateBy(RotationCorrection), + new Rotation3d( + Units.degreesToRadians(0.0), + Units.degreesToRadians(-25.2), + Units.degreesToRadians(0.0))); + + public static Transform3d robotToCamera1 = + new Transform3d( // rear left camera + new Translation3d( + Units.inchesToMeters(9.562), // x + Units.inchesToMeters(10.974), // y + Units.inchesToMeters(17.035 + 1.75)) // z + .rotateBy(RotationCorrection), + new Rotation3d( + Units.degreesToRadians(0.0), + Units.degreesToRadians(-11.32), + Units.degreesToRadians(155.3))); +} diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIO.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIO.java new file mode 100644 index 000000000..f7d810181 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIO.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.stereoVision; + +import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.subsystems.vision.VisionIO.TargetObservation; +import frc.robot.subsystems.vision.VisionIO.VisionIOInputs; +import org.littletonrobotics.junction.AutoLog; + +public interface StereoVisionIO { + @AutoLog + public static class StereoVisionIOInputs { + public boolean connected = false; + public TargetObservation latestTargetObservation = + new TargetObservation(new Rotation2d(), new Rotation2d()); + } + + /** Represents the angle to a simple target, not used for pose estimation. */ + public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} + + public default void updateInputs(VisionIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java new file mode 100644 index 000000000..23d1dab7c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java @@ -0,0 +1,3 @@ +package frc.robot.subsystems.stereoVision; + +public class StereoVisionIOPhotonVision {} From d710861e26878696136d53b471895faf504b8266 Mon Sep 17 00:00:00 2001 From: Niti Date: Mon, 5 Oct 2026 19:33:57 -0400 Subject: [PATCH 2/4] add rotation correction back --- .../robot/subsystems/stereoVision/StereoVisionConstants.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java index b6d1ce552..0f31556d8 100644 --- a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java +++ b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java @@ -10,7 +10,10 @@ public class StereoVisionConstants { public static String camera0Name = "camera_0"; public static String camera1Name = "camera_1"; - //TODO: get correct + private static Rotation3d RotationCorrection = + new Rotation3d(0, 0, Math.PI / 2); // 90 degree roation around z-axis + + //TODO: get correct coordinates for cams public static Transform3d robotToCamera0 = new Transform3d( // front camera new Translation3d( From 579d2e40859b84c1ddc89e90d21b4ca6e1136457 Mon Sep 17 00:00:00 2001 From: Niti Date: Mon, 5 Oct 2026 20:56:21 -0400 Subject: [PATCH 3/4] adds input handling for stereo subsystem --- .../subsystems/stereoVision/StereoVision.java | 43 ++++++++++++++++++ .../StereoVisionIOPhotonVision.java | 44 ++++++++++++++++++- 2 files changed, 86 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java index 06ce6a871..0d1cd31a9 100644 --- a/src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java +++ b/src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java @@ -1,7 +1,50 @@ package frc.robot.subsystems.stereoVision; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.vision.VisionIOInputsAutoLogged; +import org.littletonrobotics.junction.Logger; public class StereoVision extends SubsystemBase { + private final StereoVisionIO[] io; + private final VisionIOInputsAutoLogged[] inputs; + private final Alert[] disconnectedAlerts; + public StereoVision(StereoVisionIO... io) { + this.io = io; + + // Initialize inputs + this.inputs = new VisionIOInputsAutoLogged[io.length]; + for (int i = 0; i < inputs.length; i++) { + inputs[i] = new VisionIOInputsAutoLogged(); + } + + // Initialize disconnected alerts + this.disconnectedAlerts = new Alert[io.length]; + for (int i = 0; i < inputs.length; i++) { + disconnectedAlerts[i] = + new Alert( + "Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); + } + } + + public Rotation2d getTargetX(int cameraIndex) { + return inputs[cameraIndex].latestTargetObservation.tx(); + } + + @Override + public void periodic() { + for (int i = 0; i < io.length; i++) { + io[i].updateInputs(inputs[i]); + Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); + } + + // Loop over cameras + for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) { + // Update disconnected alert + disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); + } + } } diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java index 23d1dab7c..305b32800 100644 --- a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java @@ -1,3 +1,45 @@ package frc.robot.subsystems.stereoVision; -public class StereoVisionIOPhotonVision {} +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import frc.robot.subsystems.vision.VisionIO; +import frc.robot.subsystems.vision.VisionIO.TargetObservation; +import frc.robot.subsystems.vision.VisionIO.VisionIOInputs; +import frc.robot.subsystems.vision.VisionIOPhotonVision; +import org.photonvision.PhotonCamera; + +public class StereoVisionIOPhotonVision implements StereoVisionIO { + protected final PhotonCamera camera; + protected final Transform3d robotToCamera; + + /** + * 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 StereoVisionIOPhotonVision(String name, Transform3d robotToCamera) { + camera = new PhotonCamera(name); + this.robotToCamera = robotToCamera; + } + + public void updates(VisionIOInputs inputs) { + inputs.connected = camera.isConnected(); + + for (var result : camera.getAllUnreadResults()) { + // Update latest target observation + if (result.hasTargets()) { + inputs.latestTargetObservation = + new VisionIO.TargetObservation( + Rotation2d.fromDegrees(result.getBestTarget().getYaw()), + Rotation2d.fromDegrees(result.getBestTarget().getPitch())); + } else { + inputs.latestTargetObservation = new VisionIO.TargetObservation(new Rotation2d(), + new Rotation2d()); + } + } + + + } +} From 02615de812467f58e43369a1bc360c9d6b304219 Mon Sep 17 00:00:00 2001 From: LashikaKapoor28 Date: Wed, 7 Oct 2026 19:53:03 -0400 Subject: [PATCH 4/4] Added stero implementations --- src/main/java/frc/robot/RobotContainer.java | 3 + .../subsystems/stereoVision/StereoVision.java | 177 ++++++++++++++++-- .../stereoVision/StereoVisionConstants.java | 29 ++- .../stereoVision/StereoVisionIO.java | 15 +- .../StereoVisionIOPhotonVision.java | 39 ++-- 5 files changed, 210 insertions(+), 53 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 465b90e29..bf0f8c820 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -53,6 +53,7 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterIO; import frc.robot.subsystems.shooter.ShooterIOSparkMax; +import frc.robot.subsystems.stereoVision.StereoVision; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIOPhotonVision; import frc.robot.util.CustomTriggers; @@ -70,6 +71,7 @@ public class RobotContainer { // private final Subsystem subsystem; private Drive drive; private Vision vision; + private StereoVision stereoVision; private Leds leds; private MagicCarpet magicCarpet; private Indexer indexer; @@ -126,6 +128,7 @@ public RobotContainer() { new VisionIOPhotonVision(camera1Name, robotToCamera1), new VisionIOPhotonVision(camera2Name, robotToCamera2), new VisionIOPhotonVision(camera3Name, robotToCamera3)); + stereoVision = new StereoVision(); leds = new Leds(); shooter = new Shooter(new ShooterIOSparkMax()); magicCarpet = new MagicCarpet(new MagicCarpetSparkMax()); diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java index 0d1cd31a9..88872db4e 100644 --- a/src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java +++ b/src/main/java/frc/robot/subsystems/stereoVision/StereoVision.java @@ -1,44 +1,176 @@ package frc.robot.subsystems.stereoVision; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.subsystems.vision.VisionIOInputsAutoLogged; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.HashSet; +import java.util.List; +import java.util.Optional; +import java.util.Set; import org.littletonrobotics.junction.Logger; public class StereoVision extends SubsystemBase { - private final StereoVisionIO[] io; - private final VisionIOInputsAutoLogged[] inputs; - private final Alert[] disconnectedAlerts; + private final StereoVisionIO[] io = new StereoVisionIO[2]; + private final StereoVisionIOInputsAutoLogged[] inputs = + new StereoVisionIOInputsAutoLogged[2]; + private final Alert[] disconnectedAlerts = new Alert[2]; - public StereoVision(StereoVisionIO... io) { - this.io = io; - - // Initialize inputs - this.inputs = new VisionIOInputsAutoLogged[io.length]; - for (int i = 0; i < inputs.length; i++) { - inputs[i] = new VisionIOInputsAutoLogged(); - } + public StereoVision() { + this( + new StereoVisionIOPhotonVision( + StereoVisionConstants.camera0Name, StereoVisionConstants.robotToCamera0), + new StereoVisionIOPhotonVision( + StereoVisionConstants.camera1Name, StereoVisionConstants.robotToCamera1)); + } - // Initialize disconnected alerts - this.disconnectedAlerts = new Alert[io.length]; + public StereoVision(StereoVisionIO camera0, StereoVisionIO camera1) { + io[0] = camera0; + io[1] = camera1; for (int i = 0; i < inputs.length; i++) { + inputs[i] = new StereoVisionIOInputsAutoLogged(); disconnectedAlerts[i] = new Alert( - "Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); + "Stereo camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning); } } public Rotation2d getTargetX(int cameraIndex) { - return inputs[cameraIndex].latestTargetObservation.tx(); + int[] targetClassIds = inputs[cameraIndex].targetClassIds; + double[] targetYawDegrees = inputs[cameraIndex].targetYawDegrees; + for (int i = 0; i < targetClassIds.length; i++) { + if (targetClassIds[i] == StereoVisionConstants.fuelObjectClassId) { + return Rotation2d.fromDegrees(targetYawDegrees[i]); + } + } + return new Rotation2d(); + } + + /** Returns triangulated robot-relative positions for detected fuel objects. */ + public List getFuelPositionsRobotRelative() { + var camera0 = inputs[0]; + var camera1 = inputs[1]; + if (camera0.targetClassIds.length == 0 || camera1.targetClassIds.length == 0) { + return List.of(); + } + + double now = Timer.getFPGATimestamp(); + if (now - camera0.timestampSeconds > StereoVisionConstants.maxObservationAgeSeconds + || now - camera1.timestampSeconds > StereoVisionConstants.maxObservationAgeSeconds + || Math.abs(camera0.timestampSeconds - camera1.timestampSeconds) + > StereoVisionConstants.maxTimestampDifferenceSeconds) { + return List.of(); + } + + List camera0Rays = getFuelRays(0); + List camera1Rays = getFuelRays(1); + List candidates = new ArrayList<>(); + for (RayObservation ray0 : camera0Rays) { + for (RayObservation ray1 : camera1Rays) { + triangulate(ray0, ray1).ifPresent(candidates::add); + } + } + candidates.sort(Comparator.comparingDouble(TriangulationCandidate::raySeparation)); + + Set usedCamera0Targets = new HashSet<>(); + Set usedCamera1Targets = new HashSet<>(); + List positions = new ArrayList<>(); + for (TriangulationCandidate candidate : candidates) { + if (usedCamera0Targets.contains(candidate.camera0TargetIndex()) + || usedCamera1Targets.contains(candidate.camera1TargetIndex())) { + continue; + } + usedCamera0Targets.add(candidate.camera0TargetIndex()); + usedCamera1Targets.add(candidate.camera1TargetIndex()); + positions.add(candidate.position()); + } + return List.copyOf(positions); + } + + /** Returns the first triangulated fuel position for callers that only need one target. */ + public Optional getTargetPositionRobotRelative() { + return getFuelPositionsRobotRelative().stream().findFirst(); } + private List getFuelRays(int cameraIndex) { + StereoVisionIOInputsAutoLogged cameraInputs = inputs[cameraIndex]; + Transform3d robotToCamera = io[cameraIndex].getRobotToCamera(); + List rays = new ArrayList<>(); + for (int i = 0; i < cameraInputs.targetClassIds.length; i++) { + if (cameraInputs.targetClassIds[i] != StereoVisionConstants.fuelObjectClassId) { + continue; + } + Translation3d direction = + new Translation3d(1.0, 0.0, 0.0) + .rotateBy( + robotToCamera + .getRotation() + .rotateBy( + new Rotation3d( + 0.0, + Math.toRadians(cameraInputs.targetPitchDegrees[i]), + Math.toRadians(cameraInputs.targetYawDegrees[i])))); + rays.add(new RayObservation(i, robotToCamera.getTranslation(), direction)); + } + return rays; + } + + private Optional triangulate( + RayObservation ray0, RayObservation ray1) { + Translation3d origin0 = ray0.origin(); + Translation3d origin1 = ray1.origin(); + Translation3d direction0 = ray0.direction(); + Translation3d direction1 = ray1.direction(); + + Translation3d betweenOrigins = origin0.minus(origin1); + double dot = direction0.dot(direction1); + double denominator = 1.0 - dot * dot; + if (denominator < StereoVisionConstants.minimumTriangulationDenominator) { + return Optional.empty(); + } + + double origin0Projection = direction0.dot(betweenOrigins); + double origin1Projection = direction1.dot(betweenOrigins); + double distance0 = (dot * origin1Projection - origin0Projection) / denominator; + double distance1 = (origin1Projection - dot * origin0Projection) / denominator; + if (distance0 <= 0.0 || distance1 <= 0.0) { + return Optional.empty(); + } + + Translation3d point0 = origin0.plus(direction0.times(distance0)); + Translation3d point1 = origin1.plus(direction1.times(distance1)); + double raySeparation = point0.getDistance(point1); + if (raySeparation > StereoVisionConstants.maximumRaySeparationMeters) { + return Optional.empty(); + } + return Optional.of( + new TriangulationCandidate( + ray0.targetIndex(), + ray1.targetIndex(), + point0.plus(point1).div(2.0), + raySeparation)); + } + + private record RayObservation( + int targetIndex, Translation3d origin, Translation3d direction) {} + + private record TriangulationCandidate( + int camera0TargetIndex, + int camera1TargetIndex, + Translation3d position, + double raySeparation) {} + @Override public void periodic() { for (int i = 0; i < io.length; i++) { io[i].updateInputs(inputs[i]); - Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]); + Logger.processInputs("StereoVision/Camera" + Integer.toString(i), inputs[i]); } // Loop over cameras @@ -46,5 +178,16 @@ public void periodic() { // Update disconnected alert disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected); } + + List fuelPositions = getFuelPositionsRobotRelative(); + double[] coordinatesMeters = new double[fuelPositions.size() * 3]; + for (int i = 0; i < fuelPositions.size(); i++) { + Translation3d position = fuelPositions.get(i); + coordinatesMeters[i * 3] = position.getX(); + coordinatesMeters[i * 3 + 1] = position.getY(); + coordinatesMeters[i * 3 + 2] = position.getZ(); + } + Logger.recordOutput("StereoVision/FuelPositionCount", fuelPositions.size()); + Logger.recordOutput("StereoVision/FuelPositionsRobotRelativeMeters", coordinatesMeters); } } diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java index 0f31556d8..f793be8f5 100644 --- a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java +++ b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionConstants.java @@ -6,33 +6,42 @@ import edu.wpi.first.math.util.Units; public class StereoVisionConstants { - // TODO: chnange names accordingly - public static String camera0Name = "camera_0"; - public static String camera1Name = "camera_1"; + public static final String camera0Name = "stereo_camera_0"; + public static final String camera1Name = "stereo_camera_1"; - private static Rotation3d RotationCorrection = - new Rotation3d(0, 0, Math.PI / 2); // 90 degree roation around z-axis + // Configure this PhotonVision pipeline to use the built-in Fuel v11n model. + public static final int yoloPipelineIndex = 0; + // Fuel v11n has one class, so its class ID is 0. + public static final int fuelObjectClassId = 0; - //TODO: get correct coordinates for cams - public static Transform3d robotToCamera0 = + private static final Rotation3d rotationCorrection = + new Rotation3d(0, 0, Math.PI / 2); + + public static final double maxObservationAgeSeconds = 0.25; + public static final double maxTimestampDifferenceSeconds = 0.10; + public static final double minimumTriangulationDenominator = 1e-4; + public static final double maximumRaySeparationMeters = 0.25; + + //replace these sample poses with measured stereo camera mounting poses. + public static final Transform3d robotToCamera0 = new Transform3d( // front camera new Translation3d( Units.inchesToMeters(8.779), Units.inchesToMeters(10.445), Units.inchesToMeters(27.152 + 1.75)) - .rotateBy(RotationCorrection), + .rotateBy(rotationCorrection), new Rotation3d( Units.degreesToRadians(0.0), Units.degreesToRadians(-25.2), Units.degreesToRadians(0.0))); - public static Transform3d robotToCamera1 = + public static final Transform3d robotToCamera1 = new Transform3d( // rear left camera new Translation3d( Units.inchesToMeters(9.562), // x Units.inchesToMeters(10.974), // y Units.inchesToMeters(17.035 + 1.75)) // z - .rotateBy(RotationCorrection), + .rotateBy(rotationCorrection), new Rotation3d( Units.degreesToRadians(0.0), Units.degreesToRadians(-11.32), diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIO.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIO.java index f7d810181..03a5f2219 100644 --- a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIO.java +++ b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIO.java @@ -1,20 +1,19 @@ package frc.robot.subsystems.stereoVision; -import edu.wpi.first.math.geometry.Rotation2d; -import frc.robot.subsystems.vision.VisionIO.TargetObservation; -import frc.robot.subsystems.vision.VisionIO.VisionIOInputs; +import edu.wpi.first.math.geometry.Transform3d; import org.littletonrobotics.junction.AutoLog; public interface StereoVisionIO { @AutoLog public static class StereoVisionIOInputs { public boolean connected = false; - public TargetObservation latestTargetObservation = - new TargetObservation(new Rotation2d(), new Rotation2d()); + public int[] targetClassIds = new int[0]; + public double[] targetYawDegrees = new double[0]; + public double[] targetPitchDegrees = new double[0]; + public double timestampSeconds = 0.0; } - /** Represents the angle to a simple target, not used for pose estimation. */ - public static record TargetObservation(Rotation2d tx, Rotation2d ty) {} + Transform3d getRobotToCamera(); - public default void updateInputs(VisionIOInputs inputs) {} + default void updateInputs(StereoVisionIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java index 305b32800..2aac99caa 100644 --- a/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java +++ b/src/main/java/frc/robot/subsystems/stereoVision/StereoVisionIOPhotonVision.java @@ -1,19 +1,16 @@ package frc.robot.subsystems.stereoVision; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; -import frc.robot.subsystems.vision.VisionIO; -import frc.robot.subsystems.vision.VisionIO.TargetObservation; -import frc.robot.subsystems.vision.VisionIO.VisionIOInputs; -import frc.robot.subsystems.vision.VisionIOPhotonVision; +import java.util.List; import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonTrackedTarget; public class StereoVisionIOPhotonVision implements StereoVisionIO { protected final PhotonCamera camera; protected final Transform3d robotToCamera; /** - * Creates a new VisionIOPhotonVision. + * Creates a PhotonVision camera input for object detection. * * @param name The configured name of the camera. * @param robotToCamera The 3D position of the camera relative to the robot. @@ -22,24 +19,30 @@ public class StereoVisionIOPhotonVision implements StereoVisionIO { public StereoVisionIOPhotonVision(String name, Transform3d robotToCamera) { camera = new PhotonCamera(name); this.robotToCamera = robotToCamera; + camera.setPipelineIndex(StereoVisionConstants.yoloPipelineIndex); } - public void updates(VisionIOInputs inputs) { + @Override + public Transform3d getRobotToCamera() { + return robotToCamera; + } + + @Override + public void updateInputs(StereoVisionIOInputs inputs) { inputs.connected = camera.isConnected(); for (var result : camera.getAllUnreadResults()) { - // Update latest target observation - if (result.hasTargets()) { - inputs.latestTargetObservation = - new VisionIO.TargetObservation( - Rotation2d.fromDegrees(result.getBestTarget().getYaw()), - Rotation2d.fromDegrees(result.getBestTarget().getPitch())); - } else { - inputs.latestTargetObservation = new VisionIO.TargetObservation(new Rotation2d(), - new Rotation2d()); + inputs.timestampSeconds = result.getTimestampSeconds(); + List targets = result.getTargets(); + inputs.targetClassIds = new int[targets.size()]; + inputs.targetYawDegrees = new double[targets.size()]; + inputs.targetPitchDegrees = new double[targets.size()]; + for (int i = 0; i < targets.size(); i++) { + PhotonTrackedTarget target = targets.get(i); + inputs.targetClassIds[i] = target.getDetectedObjectClassID(); + inputs.targetYawDegrees[i] = target.getYaw(); + inputs.targetPitchDegrees[i] = target.getPitch(); } } - - } }