From c432a3ff54c398ff682dd1d66fcc20b58b8b7fce Mon Sep 17 00:00:00 2001 From: Mihir Patankar Date: Fri, 26 Jan 2024 20:22:30 -0500 Subject: [PATCH] Add vision code --- src/main/java/frc/robot/Robot.java | 13 +- src/main/java/frc/robot/Subsystems.java | 12 ++ .../java/frc/robot/drivetrain/Drivetrain.java | 33 +++- src/main/java/frc/robot/vision/Vision.java | 141 +++++++++++++++++- src/main/java/frc/robot/vision/VisionIO.java | 45 +++++- .../frc/robot/vision/VisionIOArducam.java | 3 - .../robot/vision/VisionIOPhotonVision.java | 61 ++++++++ .../java/frc/robot/vision/VisionIOSim.java | 3 - 8 files changed, 285 insertions(+), 26 deletions(-) delete mode 100644 src/main/java/frc/robot/vision/VisionIOArducam.java create mode 100644 src/main/java/frc/robot/vision/VisionIOPhotonVision.java delete mode 100644 src/main/java/frc/robot/vision/VisionIOSim.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 628474e..15574a4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.drivetrain.Drivetrain; +import frc.robot.vision.Vision; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -20,6 +21,7 @@ public class Robot extends LoggedRobot { private final LoggedDashboardChooser m_chooser = new LoggedDashboardChooser<>("Auto Choices"); private final Drivetrain m_drivetrain; + private final Vision m_vision; // private final Intake m_intake = new Intake(); // private final Shooter m_shooter = new Shooter(); private TimeOfFlight mytimeofflight = new TimeOfFlight(12); @@ -52,20 +54,24 @@ public Robot() { switch (Constants.kCurrentMode) { case kReal: m_drivetrain = Subsystems.createTalonFXDrivetrain(); + m_vision = Subsystems.createFourCameraVision(); break; case kSim: m_drivetrain = Subsystems.createTalonFXDrivetrain(); + m_vision = Subsystems.createFourCameraVision(); break; default: m_drivetrain = Subsystems.createBlankDrivetrain(); + m_vision = Subsystems.createBlankFourCameraVision(); break; } + m_vision.setVisionPoseConsumer(m_drivetrain.getVisionPoseConsumer()); m_drivetrain.setDefaultCommand( m_drivetrain.joystickDrive( () -> -m_driverController.getLeftY(), () -> -m_driverController.getLeftX(), // This needs to be getRawAxis(2) when using sim on a Mac - () -> -m_driverController.getRightX())); + () -> -m_driverController.getRawAxis(2))); m_driverController.y().onTrue(m_drivetrain.zeroRotation()); double sensorValue = SmartDashboard.getNumber("Distance", mytimeofflight.getRange()); @@ -97,6 +103,7 @@ public Robot() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + m_vision.periodic(); } @Override @@ -129,7 +136,9 @@ public void testPeriodic() {} public void simulationInit() {} @Override - public void simulationPeriodic() {} + public void simulationPeriodic() { + m_vision.updateSim(m_drivetrain.getPose()); + } private void recordMetadeta() { Logger.recordMetadata("ProjectName", BuildConstants.MAVEN_NAME); diff --git a/src/main/java/frc/robot/Subsystems.java b/src/main/java/frc/robot/Subsystems.java index b7d54dc..a4ea109 100644 --- a/src/main/java/frc/robot/Subsystems.java +++ b/src/main/java/frc/robot/Subsystems.java @@ -1,10 +1,14 @@ package frc.robot; +import edu.wpi.first.math.geometry.Transform3d; import frc.robot.drivetrain.Drivetrain; import frc.robot.drivetrain.ImuIO; import frc.robot.drivetrain.ImuIOPigeon2; import frc.robot.drivetrain.ModuleIO; import frc.robot.drivetrain.ModuleIOTalonFX; +import frc.robot.vision.Vision; +import frc.robot.vision.VisionIO; +import frc.robot.vision.VisionIOPhotonVision; public class Subsystems { @@ -21,4 +25,12 @@ public static Drivetrain createBlankDrivetrain() { return new Drivetrain( new ImuIO() {}, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); } + + public static Vision createFourCameraVision() { + return new Vision(new VisionIOPhotonVision("front-cam", new Transform3d())); + } + + public static Vision createBlankFourCameraVision() { + return new Vision(new VisionIO() {}, new VisionIO() {}, new VisionIO() {}, new VisionIO() {}); + } } diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index 32a10f1..61279a5 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -23,9 +23,11 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.LocalADStarAK; import frc.robot.Constants; +import frc.robot.vision.Vision.VisionUpdate; import java.util.Arrays; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; +import java.util.function.Consumer; import java.util.function.DoubleSupplier; import java.util.function.UnaryOperator; import org.littletonrobotics.junction.AutoLogOutput; @@ -33,13 +35,14 @@ public class Drivetrain extends SubsystemBase { - public static final double kTrackwidthMeters = Units.inchesToMeters(26.0); - public static final double kWheelbaseMeters = Units.inchesToMeters(26.0); - private static final double kDriveBaseRadius = + public final double kTrackwidthMeters = Units.inchesToMeters(26.0); + public final double kWheelbaseMeters = Units.inchesToMeters(26.0); + private final double kDriveBaseRadius = Math.hypot(kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0); - private static final double kMaxLinearSpeedMetersPerSecond = Units.feetToMeters(16.5); - private static final double kMaxAngularSpeedRadPerSec = 2 * Math.PI; - private static final double kDeadband = 0.05; + private final double kMaxLinearSpeedMetersPerSecond = Units.feetToMeters(16.5); + private final double kMaxAngularSpeedRadPerSec = 2 * Math.PI; + private final double kDeadband = 0.05; + private final boolean kUseVisionCorrection = false; public static final Lock odometryLock = new ReentrantLock(); private final ImuIO m_imuIO; @@ -62,6 +65,8 @@ public class Drivetrain extends SubsystemBase { new SwerveModulePosition() }; + private final Consumer m_visionUpdateConsumer; + public Drivetrain( ImuIO imuIO, ModuleIO frontLeftModuleIO, @@ -77,6 +82,16 @@ public Drivetrain( m_poseEstimator = new SwerveDrivePoseEstimator( m_kinematics, new Rotation2d(), getModulePositions(), new Pose2d()); + m_visionUpdateConsumer = + (VisionUpdate visionUpdate) -> { + if (!kUseVisionCorrection) { + return; + } + m_poseEstimator.addVisionMeasurement( + visionUpdate.pose(), + visionUpdate.timestampSeconds(), + visionUpdate.standardDeviations()); + }; configurePathing(); } @@ -216,11 +231,15 @@ private SwerveModulePosition[] getModulePositions() { } @AutoLogOutput(key = "Drivetrain/Estimated Pose") - private Pose2d getPose() { + public Pose2d getPose() { return m_poseEstimator.getEstimatedPosition(); } public void setPose(Pose2d pose) { m_poseEstimator.resetPosition(m_imuInputs.yawPosition, getModulePositions(), pose); } + + public Consumer getVisionPoseConsumer() { + return m_visionUpdateConsumer; + } } diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index 28c13f9..1faef34 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -1,17 +1,152 @@ package frc.robot.vision; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +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.numbers.N1; +import edu.wpi.first.math.numbers.N3; import frc.robot.Constants; +import frc.robot.vision.VisionIO.VisionIOInputs; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import java.util.function.Consumer; +import java.util.stream.Collectors; +import org.littletonrobotics.junction.Logger; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; public class Vision { + + private final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + private final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); + private final VisionIO[] io; + private final VisionIOInputs[] m_inputs; + private final PhotonPoseEstimator[] m_poseEstimators; private VisionSystemSim m_visionSimSystem = null; + private final AprilTagFieldLayout kTagLayout = + AprilTagFields.kDefaultField.loadAprilTagLayoutField(); + private Consumer m_visionUpdateConsumer = null; - public Vision() { + public Vision(VisionIO... io) { + this.io = io; + m_inputs = new VisionIOInputs[io.length]; + m_poseEstimators = new PhotonPoseEstimator[io.length]; + for (int i = 0; i < io.length; i++) { + m_inputs[i] = new VisionIOInputs(); + io[i].updateInputs(m_inputs[i]); // This sets camera names and transforms + m_poseEstimators[i] = + new PhotonPoseEstimator( + kTagLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + new PhotonCamera(m_inputs[i].cameraName), + m_inputs[i].robotToCamera); + } if (Constants.kIsSim) { m_visionSimSystem = new VisionSystemSim("sim_vision_system"); + m_visionSimSystem.addAprilTags(kTagLayout); + for (int index = 0; index < io.length; index++) { + PhotonCameraSim cameraSim = + new PhotonCameraSim( + new PhotonCamera(m_inputs[index].cameraName), getSimCameraProperties()); + cameraSim.enableDrawWireframe(true); + m_visionSimSystem.addCamera(cameraSim, m_inputs[index].robotToCamera); + } + } + } + + public void periodic() { + for (int index = 0; index < io.length; index++) { + io[index].updateInputs(m_inputs[index]); + Logger.processInputs("Vision/Cam" + Integer.toString(index), m_inputs[index]); + } + + final List posesToLog = new ArrayList<>(); + final List tagPosesToLog = new ArrayList<>(); + for (int i = 0; i < io.length; i++) { + for (PhotonPipelineResult result : m_inputs[i].results) { + Optional poseOptional = m_poseEstimators[i].update(result); + poseOptional.ifPresent( + pose -> { + Pose2d estimatedPose = pose.estimatedPose.toPose2d(); + List targets = result.getTargets(); + targets.forEach( + target -> { + // if (target.getPoseAmbiguity() >= 0.2) return; + kTagLayout + .getTagPose(target.getFiducialId()) + .ifPresent(tagPose -> tagPosesToLog.add(tagPose)); + }); + posesToLog.add(estimatedPose); + m_visionUpdateConsumer.accept( + new VisionUpdate( + estimatedPose, + result.getTimestampSeconds(), + getEstimationStdDevs(estimatedPose, targets))); + }); + } + } + Logger.recordOutput("Vision/EstimatedPoses", posesToLog.toArray(new Pose2d[posesToLog.size()])); + Logger.recordOutput("Vision/TagPoses", tagPosesToLog.toArray(new Pose3d[tagPosesToLog.size()])); + Logger.recordOutput( + "Vision/TagPoses2D", + tagPosesToLog.stream() + .map(p -> p.toPose2d()) + .collect(Collectors.toList()) + .toArray(new Pose2d[tagPosesToLog.size()])); + } + + public Matrix getEstimationStdDevs( + Pose2d estimatedPose, List targets) { + var estimatedStdDevs = kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + for (var target : targets) { + var tagPose = kTagLayout.getTagPose(target.getFiducialId()); + if (tagPose.isEmpty()) continue; + numTags++; + avgDist += + tagPose.get().toPose2d().getTranslation().getDistance(estimatedPose.getTranslation()); } + if (numTags == 0) return estimatedStdDevs; + avgDist /= numTags; + if (numTags > 1) estimatedStdDevs = kMultiTagStdDevs; + // After 4 meters we can't trust vision + if (numTags == 1 && avgDist > 4) + estimatedStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else estimatedStdDevs = estimatedStdDevs.times(1 + (avgDist * avgDist / 30)); + return estimatedStdDevs; + } + + private SimCameraProperties getSimCameraProperties() { + final SimCameraProperties cameraProperties = new SimCameraProperties(); + cameraProperties.setCalibration(1600, 1200, Rotation2d.fromDegrees(75)); + cameraProperties.setCalibError(0.35, 0.10); + cameraProperties.setFPS(20); + cameraProperties.setAvgLatencyMs(50); + cameraProperties.setLatencyStdDevMs(15); + return cameraProperties; + } + + public void updateSim(Pose2d currentPose) { + m_visionSimSystem.update(currentPose); + } - // PhotonPoseEstimator m_poseEstimator = new PhotonPoseEstimator(); - // m_poseEstimator.setMultiTagFallbackStrategy(null); + public void setVisionPoseConsumer(Consumer consumer) { + m_visionUpdateConsumer = consumer; } + + public static record VisionUpdate( + Pose2d pose, double timestampSeconds, Matrix standardDeviations) {} } diff --git a/src/main/java/frc/robot/vision/VisionIO.java b/src/main/java/frc/robot/vision/VisionIO.java index c575dec..8be0893 100644 --- a/src/main/java/frc/robot/vision/VisionIO.java +++ b/src/main/java/frc/robot/vision/VisionIO.java @@ -1,12 +1,41 @@ package frc.robot.vision; -import edu.wpi.first.math.geometry.Pose2d; -import org.littletonrobotics.junction.AutoLog; - -public class VisionIO { - @AutoLog - public static class VisionIOInputs { - public Pose2d[] poses; - public int fps; +import edu.wpi.first.math.geometry.Transform3d; +import org.littletonrobotics.junction.LogTable; +import org.littletonrobotics.junction.inputs.LoggableInputs; +import org.photonvision.targeting.PhotonPipelineResult; + +public interface VisionIO { + + public static class VisionIOInputs implements LoggableInputs { + public String cameraName = ""; + public Transform3d robotToCamera = new Transform3d(); + public PhotonPipelineResult[] results = new PhotonPipelineResult[] {}; + public boolean connected = false; + + @Override + public void toLog(LogTable table) { + table.put("CameraName", cameraName); + table.put("RobotToCamera", robotToCamera); + table.put("FrameCount", results.length); + for (int i = 0; i < results.length; i++) { + table.put("Result/" + Integer.toString(i), results[i]); + } + table.put("Connected", connected); + } + + @Override + public void fromLog(LogTable table) { + cameraName = table.get("CameraName", cameraName); + robotToCamera = table.get("RobotToCamera", robotToCamera); + int frameCount = (int) table.get("FrameCount", 0); + results = new PhotonPipelineResult[frameCount]; + for (int i = 0; i < frameCount; i++) { + results[i] = table.get("Frame/" + Integer.toString(i), new PhotonPipelineResult()); + } + connected = table.get("Connected", false); + } } + + public default void updateInputs(VisionIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/vision/VisionIOArducam.java b/src/main/java/frc/robot/vision/VisionIOArducam.java deleted file mode 100644 index 885a103..0000000 --- a/src/main/java/frc/robot/vision/VisionIOArducam.java +++ /dev/null @@ -1,3 +0,0 @@ -package frc.robot.vision; - -public class VisionIOArducam {} diff --git a/src/main/java/frc/robot/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/vision/VisionIOPhotonVision.java new file mode 100644 index 0000000..8292519 --- /dev/null +++ b/src/main/java/frc/robot/vision/VisionIOPhotonVision.java @@ -0,0 +1,61 @@ +package frc.robot.vision; + +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.networktables.RawSubscriber; +import edu.wpi.first.networktables.TimestampedRaw; +import edu.wpi.first.wpilibj.Timer; +import org.photonvision.common.dataflow.structures.Packet; +import org.photonvision.targeting.PhotonPipelineResult; + +public class VisionIOPhotonVision implements VisionIO { + + private static final double kDisconnectedTimeout = 0.5; + private RawSubscriber m_photonDataSubscriber; + private final Timer m_disconnectedTimer = new Timer(); + private final String m_cameraName; + private final Transform3d m_robotToCamera; + + public VisionIOPhotonVision(String cameraName, Transform3d robotToCamera) { + m_cameraName = cameraName; + m_robotToCamera = robotToCamera; + m_photonDataSubscriber = + NetworkTableInstance.getDefault() + .getTable("/photonvision/" + cameraName) + .getRawTopic("rawBytes") + .subscribe( + "rawBytes", + new byte[] {}, + PubSubOption.keepDuplicates(true), + PubSubOption.sendAll(true)); + } + + public void updateInputs(VisionIOInputs inputs) { + inputs.cameraName = m_cameraName; + inputs.robotToCamera = m_robotToCamera; + TimestampedRaw[] dataQueue = m_photonDataSubscriber.readQueue(); + inputs.results = new PhotonPipelineResult[dataQueue.length]; + + for (int index = 0; index < dataQueue.length; index++) { + Packet dataPacket = new Packet(1); + dataPacket.setData(dataQueue[index].value); + if (dataPacket.getSize() < 1) { + throw new NullPointerException("Data packet is empty. This should NEVER happen."); + } + PhotonPipelineResult result = PhotonPipelineResult.serde.unpack(dataPacket); + double timestampSeconds = + (dataQueue[index].timestamp / 1e6) - (result.getLatencyMillis() / 1e3); + result.setTimestampSeconds(timestampSeconds); + inputs.results[index] = result; + } + + if (dataQueue.length > 0) { + m_disconnectedTimer.reset(); + } else if (m_disconnectedTimer.hasElapsed(kDisconnectedTimeout)) { + inputs.connected = false; + } else { + inputs.connected = true; + } + } +} diff --git a/src/main/java/frc/robot/vision/VisionIOSim.java b/src/main/java/frc/robot/vision/VisionIOSim.java deleted file mode 100644 index 91e8220..0000000 --- a/src/main/java/frc/robot/vision/VisionIOSim.java +++ /dev/null @@ -1,3 +0,0 @@ -package frc.robot.vision; - -public class VisionIOSim {}