From 7db6ae87f82e5114b6791723072a245debaeba56 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Tue, 14 Jan 2025 20:25:44 -0800 Subject: [PATCH] 2024 Vision Subsystem added --- src/main/java/competition/Robot.java | 1 + .../CompetitionContract.java | 52 ++++ .../ElectricalContract.java | 2 +- .../components/BaseRobotComponent.java | 2 + .../injection/modules/CompetitionModule.java | 4 + .../injection/modules/PracticeModule.java | 4 + .../injection/modules/RoboxModule.java | 4 + .../modules/SimulatedRobotModule.java | 4 + .../modules/UnitTestRobotModule.java | 4 + .../subsystems/vision/VisionSubsystem.java | 241 ++++++++++++++++++ 10 files changed, 317 insertions(+), 1 deletion(-) create mode 100644 src/main/java/competition/subsystems/vision/VisionSubsystem.java diff --git a/src/main/java/competition/Robot.java b/src/main/java/competition/Robot.java index 1e0e2bb..1c02789 100644 --- a/src/main/java/competition/Robot.java +++ b/src/main/java/competition/Robot.java @@ -32,6 +32,7 @@ protected void initializeSystems() { dataFrameRefreshables.add((DriveSubsystem)getInjectorComponent().driveSubsystem()); dataFrameRefreshables.add(getInjectorComponent().poseSubsystem()); + dataFrameRefreshables.add(getInjectorComponent().visionSubsystem()); } protected BaseRobotComponent createDaggerComponent() { diff --git a/src/main/java/competition/electrical_contract/CompetitionContract.java b/src/main/java/competition/electrical_contract/CompetitionContract.java index 6b0a441..fcf7ce9 100644 --- a/src/main/java/competition/electrical_contract/CompetitionContract.java +++ b/src/main/java/competition/electrical_contract/CompetitionContract.java @@ -5,14 +5,21 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import competition.subsystems.pose.PoseSubsystem; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import xbot.common.injection.electrical_contract.CANBusId; import xbot.common.injection.electrical_contract.CANMotorControllerInfo; import xbot.common.injection.electrical_contract.CANMotorControllerOutputConfig; import xbot.common.injection.electrical_contract.CANTalonInfo; +import xbot.common.injection.electrical_contract.CameraInfo; import xbot.common.injection.electrical_contract.DeviceInfo; import xbot.common.injection.electrical_contract.MotorControllerType; import xbot.common.injection.swerve.SwerveInstance; import xbot.common.math.XYPair; +import xbot.common.subsystems.vision.CameraCapabilities; + +import java.util.EnumSet; public class CompetitionContract extends ElectricalContract { @@ -143,4 +150,49 @@ public XYPair getSwerveModuleOffsetsInInches(SwerveInstance swerveInstance) { default -> new XYPair(0, 0); }; } + + + private static double aprilCameraXDisplacement = 13.153 / PoseSubsystem.INCHES_IN_A_METER; + private static double aprilCameraYDisplacement = 12.972 / PoseSubsystem.INCHES_IN_A_METER; + private static double aprilCameraZDisplacement = 9.014 / PoseSubsystem.INCHES_IN_A_METER; + private static double aprilCameraPitch = Math.toRadians(-55.5); + private static double aprilCameraYaw = Math.toRadians(10); + + @Override + public CameraInfo[] getCameraInfo() { + return new CameraInfo[] { + new CameraInfo("Apriltag_FrontLeft_Camera", + "AprilTagFrontLeft", + new Transform3d(new Translation3d( + aprilCameraXDisplacement, + aprilCameraYDisplacement, + aprilCameraZDisplacement), + new Rotation3d(0, aprilCameraPitch, aprilCameraYaw)), + EnumSet.of(CameraCapabilities.APRIL_TAG)), + new CameraInfo("Apriltag_FrontRight_Camera", + "AprilTagFrontRight", + new Transform3d(new Translation3d( + aprilCameraXDisplacement, + -aprilCameraYDisplacement, + aprilCameraZDisplacement), + new Rotation3d(0, aprilCameraPitch, -aprilCameraYaw)), + EnumSet.of(CameraCapabilities.APRIL_TAG)), + new CameraInfo("Apriltag_RearLeft_Camera", + "AprilTagRearLeft", + new Transform3d(new Translation3d( + -aprilCameraXDisplacement, + aprilCameraYDisplacement, + aprilCameraZDisplacement), + new Rotation3d(0, aprilCameraPitch, Math.toRadians(180) - aprilCameraYaw)), + EnumSet.of(CameraCapabilities.APRIL_TAG)), + new CameraInfo("Apriltag_RearRight_Camera", + "AprilTagRearRight", + new Transform3d(new Translation3d( + -aprilCameraXDisplacement, + -aprilCameraYDisplacement, + aprilCameraZDisplacement), + new Rotation3d(0, aprilCameraPitch, Math.toRadians(180) + aprilCameraYaw)), + EnumSet.of(CameraCapabilities.APRIL_TAG)) + }; + } } diff --git a/src/main/java/competition/electrical_contract/ElectricalContract.java b/src/main/java/competition/electrical_contract/ElectricalContract.java index bdd2e54..a00a486 100644 --- a/src/main/java/competition/electrical_contract/ElectricalContract.java +++ b/src/main/java/competition/electrical_contract/ElectricalContract.java @@ -8,7 +8,7 @@ import xbot.common.injection.swerve.SwerveInstance; import xbot.common.math.XYPair; -public abstract class ElectricalContract implements XSwerveDriveElectricalContract { +public abstract class ElectricalContract implements XCameraElectricalContract, XSwerveDriveElectricalContract { public abstract boolean isDriveReady(); public abstract boolean areCanCodersReady(); diff --git a/src/main/java/competition/injection/components/BaseRobotComponent.java b/src/main/java/competition/injection/components/BaseRobotComponent.java index 795b9fc..2b64fa1 100644 --- a/src/main/java/competition/injection/components/BaseRobotComponent.java +++ b/src/main/java/competition/injection/components/BaseRobotComponent.java @@ -3,6 +3,7 @@ import competition.operator_interface.OperatorCommandMap; import competition.simulation.Simulator; import competition.subsystems.SubsystemDefaultCommandMap; +import competition.subsystems.vision.VisionSubsystem; import xbot.common.injection.components.BaseComponent; import xbot.common.injection.swerve.SwerveComponentHolder; import xbot.common.subsystems.drive.swerve.SwerveDefaultCommandMap; @@ -17,4 +18,5 @@ public abstract class BaseRobotComponent extends BaseComponent { public abstract SwerveComponentHolder swerveComponentHolder(); public abstract Simulator simulator(); + public abstract VisionSubsystem visionSubsystem(); } diff --git a/src/main/java/competition/injection/modules/CompetitionModule.java b/src/main/java/competition/injection/modules/CompetitionModule.java index 81ab0ad..36d7b82 100644 --- a/src/main/java/competition/injection/modules/CompetitionModule.java +++ b/src/main/java/competition/injection/modules/CompetitionModule.java @@ -35,4 +35,8 @@ public abstract class CompetitionModule { @Binds @Singleton public abstract BaseDriveSubsystem getDriveSubsystem(BaseSwerveDriveSubsystem impl); + + @Binds + @Singleton + public abstract XCameraElectricalContract getCameraContract(ElectricalContract impl); } diff --git a/src/main/java/competition/injection/modules/PracticeModule.java b/src/main/java/competition/injection/modules/PracticeModule.java index 466f2cb..951747e 100644 --- a/src/main/java/competition/injection/modules/PracticeModule.java +++ b/src/main/java/competition/injection/modules/PracticeModule.java @@ -35,4 +35,8 @@ public abstract class PracticeModule { @Binds @Singleton public abstract BaseDriveSubsystem getDriveSubsystem(BaseSwerveDriveSubsystem impl); + + @Binds + @Singleton + public abstract XCameraElectricalContract getCameraContract(ElectricalContract impl); } diff --git a/src/main/java/competition/injection/modules/RoboxModule.java b/src/main/java/competition/injection/modules/RoboxModule.java index 1a81ffe..e0e59f6 100644 --- a/src/main/java/competition/injection/modules/RoboxModule.java +++ b/src/main/java/competition/injection/modules/RoboxModule.java @@ -35,4 +35,8 @@ public abstract class RoboxModule { @Binds @Singleton public abstract BaseDriveSubsystem getDriveSubsystem(BaseSwerveDriveSubsystem impl); + + @Binds + @Singleton + public abstract XCameraElectricalContract getCameraContract(ElectricalContract impl); } diff --git a/src/main/java/competition/injection/modules/SimulatedRobotModule.java b/src/main/java/competition/injection/modules/SimulatedRobotModule.java index 08192be..37d4a0a 100644 --- a/src/main/java/competition/injection/modules/SimulatedRobotModule.java +++ b/src/main/java/competition/injection/modules/SimulatedRobotModule.java @@ -36,4 +36,8 @@ public abstract class SimulatedRobotModule { @Binds @Singleton public abstract BaseDriveSubsystem getDriveSubsystem(BaseSwerveDriveSubsystem impl); + + @Binds + @Singleton + public abstract XCameraElectricalContract getCameraContract(ElectricalContract impl); } diff --git a/src/main/java/competition/injection/modules/UnitTestRobotModule.java b/src/main/java/competition/injection/modules/UnitTestRobotModule.java index 82592bc..06a0861 100644 --- a/src/main/java/competition/injection/modules/UnitTestRobotModule.java +++ b/src/main/java/competition/injection/modules/UnitTestRobotModule.java @@ -36,4 +36,8 @@ public abstract class UnitTestRobotModule { @Binds @Singleton public abstract BaseDriveSubsystem getDriveSubsystem(BaseSwerveDriveSubsystem impl); + + @Binds + @Singleton + public abstract XCameraElectricalContract getCameraContract(ElectricalContract impl); } diff --git a/src/main/java/competition/subsystems/vision/VisionSubsystem.java b/src/main/java/competition/subsystems/vision/VisionSubsystem.java new file mode 100644 index 0000000..4d24193 --- /dev/null +++ b/src/main/java/competition/subsystems/vision/VisionSubsystem.java @@ -0,0 +1,241 @@ +package competition.subsystems.vision; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.networktables.NetworkTableInstance; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCameraExtended; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.targeting.PhotonPipelineResult; +import xbot.common.advantage.DataFrameRefreshable; +import xbot.common.command.BaseSubsystem; +import xbot.common.injection.electrical_contract.CameraInfo; +import xbot.common.injection.electrical_contract.XCameraElectricalContract; +import xbot.common.logging.RobotAssertionManager; +import xbot.common.logic.TimeStableValidator; +import xbot.common.properties.BooleanProperty; +import xbot.common.properties.DoubleProperty; +import xbot.common.properties.Property; +import xbot.common.properties.PropertyFactory; +import xbot.common.subsystems.vision.AprilTagCamera; +import xbot.common.subsystems.vision.CameraCapabilities; +import xbot.common.subsystems.vision.SimpleCamera; + +import javax.inject.Inject; +import javax.inject.Singleton; +import java.io.IOException; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; +import java.util.Optional; + +@Singleton +public class VisionSubsystem extends BaseSubsystem implements DataFrameRefreshable { + + final RobotAssertionManager assertionManager; + final BooleanProperty isInverted; + final DoubleProperty yawOffset; + final DoubleProperty waitForStablePoseTime; + final DoubleProperty robotDisplacementThresholdToRejectVisionUpdate; + final DoubleProperty singleTagStableDistance; + final DoubleProperty multiTagStableDistance; + AprilTagFieldLayout aprilTagFieldLayout; + final ArrayList aprilTagCameras; + final ArrayList allCameras; + boolean aprilTagsLoaded = false; + long logCounter = 0; + + + @Inject + public VisionSubsystem(PropertyFactory pf, XCameraElectricalContract electricalContract, RobotAssertionManager assertionManager) { + this.assertionManager = assertionManager; + + pf.setPrefix(this); + isInverted = pf.createPersistentProperty("Yaw inverted", true); + yawOffset = pf.createPersistentProperty("Yaw offset", 0); + singleTagStableDistance = pf.createPersistentProperty("Single tag stable distance", 2.0); + multiTagStableDistance = pf.createPersistentProperty("Multi tag stable distance", 4.0); + + var trackingNt = NetworkTableInstance.getDefault().getTable("SmartDashboard"); + + waitForStablePoseTime = pf.createPersistentProperty("Pose stable time", 0.0, Property.PropertyLevel.Debug); + robotDisplacementThresholdToRejectVisionUpdate = pf.createPersistentProperty("Displacement Threshold to reject Vision (m)",3); + + // TODO: Add resiliency to this subsystem, so that if the camera is not connected, it doesn't cause a pile + // of errors. Some sort of VisionReady in the ElectricalContract may also make sense. Similarly, + // we need to handle cases like not having the AprilTag data loaded. + + try { + aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); + aprilTagsLoaded = true; + log.info("Successfully loaded AprilTagFieldLayout"); + } catch (IOException e) { + log.error("Could not load AprilTagFieldLayout!", e); + } + + PhotonCameraExtended.setVersionCheckEnabled(false); + aprilTagCameras = new ArrayList(); + if (aprilTagsLoaded) { + var aprilTagCapableCameras = Arrays + .stream(electricalContract.getCameraInfo()) + .filter(info -> info.capabilities().contains(CameraCapabilities.APRIL_TAG)) + .toArray(CameraInfo[]::new); + for (var camera : aprilTagCapableCameras) { + aprilTagCameras.add(new AprilTagCamera(camera, waitForStablePoseTime::get, aprilTagFieldLayout, this.getPrefix())); + } + } + + allCameras = new ArrayList<>(); + allCameras.addAll(aprilTagCameras); + } + + public List> getPhotonVisionEstimatedPoses(Pose2d previousEstimatedRobotPose) { + var estimatedPoses = new ArrayList>(); + for (AprilTagCamera cameraState : this.aprilTagCameras) { + if (cameraState.isCameraWorking()) { + var estimatedPose = getPhotonVisionEstimatedPose(cameraState.getName(), cameraState.getPoseEstimator(), + previousEstimatedRobotPose, cameraState.getIsStableValidator(), cameraState.getCamera()); + estimatedPoses.add(estimatedPose); + } + } + return estimatedPoses; + } + + public Optional getPhotonVisionEstimatedPose( + String name, PhotonPoseEstimator estimator, Pose2d previousEstimatedRobotPose, TimeStableValidator poseTimeValidator, + PhotonCameraExtended photonCameraExtended) { + if (aprilTagsLoaded) { + estimator.setReferencePose(previousEstimatedRobotPose); + + PhotonPipelineResult cameraResult = photonCameraExtended.getAllUnreadResults().get(photonCameraExtended.getAllUnreadResults().size() - 1); + + + var estimatedPose = estimator.update(cameraResult, photonCameraExtended.getCameraMatrix(), photonCameraExtended.getDistCoeffs()); + // Log the estimated pose, and log an insane value if we don't have one (so we don't clutter up the visualization) + if (estimatedPose.isPresent()) + { + aKitLog.record(name+"Estimate", estimatedPose.get().estimatedPose.toPose2d()); + } + + var isReliable = estimatedPose.isPresent() && isEstimatedPoseReliable(estimatedPose.get(), previousEstimatedRobotPose); + aKitLog.record(name+"Reliable", isReliable); + var isStable = waitForStablePoseTime.get() == 0.0 || poseTimeValidator.checkStable(isReliable); + if (isReliable && isStable) { + return estimatedPose; + } + return Optional.empty(); + } else { + return Optional.empty(); + } + } + + /** + * Performs several sanity checks on the estimated pose: + * - Are we seeing an invalid tag? + * - Are we pretty close to our previous location? + * - If we see 2+ targets, we're good + * - If we see 1 target, we need to be close and have a low ambiguity + * @param estimatedPose The pose to check + * @param previousEstimatedPose The previous location of the robot in the last loop + * @return True if the pose is reliable and should be consumed by the robot + */ + public boolean isEstimatedPoseReliable(EstimatedRobotPose estimatedPose, Pose2d previousEstimatedPose) { + // No targets, so there's no way we should use this data. + if (estimatedPose.targetsUsed.size() == 0) { + return false; + } + + // Pose isn't reliable if we see a tag id that shouldn't be on the field + var allTagIds = getTagListFromPose(estimatedPose); + if (allTagIds.stream().anyMatch(id -> id < 1 || id > 16)) { + log.warn("Ignoring vision pose with invalid tag id. Visible tags: " + + getStringFromList(allTagIds)); + return false; + } + + // If this is way too far from our current location, then it's probably a bad estimate. + double distance = previousEstimatedPose.getTranslation().getDistance(estimatedPose.estimatedPose.toPose2d().getTranslation()); + var visionDistanceTooLarge = distance > robotDisplacementThresholdToRejectVisionUpdate.get(); + // Unless we're near zero which means we've never set a pose before, use vision even if we're far away + + var isNearZero = previousEstimatedPose.getTranslation().getNorm() < 0.1; + if(!isNearZero && visionDistanceTooLarge) { + if (logCounter++ % 20 == 0) { + log.warn(String.format("Ignoring vision pose because distance is %f from our previous pose. Current pose: %s, vision pose: %s.", + distance, + previousEstimatedPose.getTranslation().toString(), + estimatedPose.estimatedPose.getTranslation().toString())); + } + return false; + } + + // How far away is the camera from the target? + double cameraDistanceToTarget = + estimatedPose.targetsUsed.get(0).getBestCameraToTarget().getTranslation().getNorm(); + + // Two or more targets tends to be very reliable, but there's still a limit for distance + if (estimatedPose.targetsUsed.size() > 1 + && cameraDistanceToTarget < multiTagStableDistance.get()) { + return true; + } + + // For a single target we need to be above reliability threshold and very close. + return estimatedPose.targetsUsed.get(0).getPoseAmbiguity() < 0.20 + && cameraDistanceToTarget < singleTagStableDistance.get(); + } + + private List getTagListFromPose(EstimatedRobotPose estimatedPose) { + return Arrays.asList(estimatedPose.targetsUsed.stream() + .map(target -> target.getFiducialId()).toArray(Integer[]::new)); + } + + private String getStringFromList(List list) { + return String.join(", ", list.stream().mapToInt(id -> id).mapToObj(id -> Integer.toString(id)).toArray(String[]::new)); + } + + int loopCounter = 0; + + @Override + public void periodic() { + loopCounter++; + + var anyCameraBroken = allCameras.stream().anyMatch(state -> !state.isCameraWorking()); + + // If one of the cameras is not working, see if they have self healed every 5 seconds + if (loopCounter % (50 * 5) == 0 && (anyCameraBroken)) { + log.info("Checking if cameras have self healed"); + for (SimpleCamera camera : aprilTagCameras) { + if (!camera.isCameraWorking()) { + log.info("Camera " + camera.getName() + " is still not working"); + } + } + } + + for (SimpleCamera camera : allCameras) { + aKitLog.record(camera.getName() + "CameraWorking", camera.isCameraWorking()); + } + } + + @Override + public void refreshDataFrame() { + if (aprilTagsLoaded) { + for (SimpleCamera camera : allCameras) { + camera.getCamera().refreshDataFrame(); + } + } + } + + public int cameraWorkingState() { + if (allCameras.stream().allMatch(state -> state.isCameraWorking())) { + // If all are working, return 0 + return 0; + } + else if (allCameras.stream().allMatch(state -> !state.isCameraWorking())) { + // If no cameras are working, return 1 + return 1; + } + // If some of the cameras are working, return 2 + return 2; + } +} \ No newline at end of file