From 56efc5c30e4725cb7610777418d59360c0ab433a Mon Sep 17 00:00:00 2001 From: Richie_Xue Date: Tue, 9 Jan 2024 23:07:49 -0500 Subject: [PATCH] Custom camera subsystem --- .../stuypulse/robot/constants/Cameras.java | 42 ++++ .../com/stuypulse/robot/constants/Field.java | 218 ++++++++++++++++++ .../stuypulse/robot/constants/Settings.java | 5 + .../subsystems/odometry/AbstractOdometry.java | 38 +++ .../robot/subsystems/odometry/Odometry.java | 74 ++++++ .../subsystems/vision/AbstractVision.java | 24 ++ .../robot/subsystems/vision/Vision.java | 67 ++++++ .../stuypulse/robot/util/CustomCamera.java | 142 ++++++++++++ .../com/stuypulse/robot/util/Fiducial.java | 21 ++ .../com/stuypulse/robot/util/VisionData.java | 36 +++ 10 files changed, 667 insertions(+) create mode 100644 src/main/java/com/stuypulse/robot/constants/Cameras.java create mode 100644 src/main/java/com/stuypulse/robot/constants/Field.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/odometry/AbstractOdometry.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/vision/AbstractVision.java create mode 100644 src/main/java/com/stuypulse/robot/subsystems/vision/Vision.java create mode 100644 src/main/java/com/stuypulse/robot/util/CustomCamera.java create mode 100644 src/main/java/com/stuypulse/robot/util/Fiducial.java create mode 100644 src/main/java/com/stuypulse/robot/util/VisionData.java diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/src/main/java/com/stuypulse/robot/constants/Cameras.java new file mode 100644 index 0000000..a202643 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/constants/Cameras.java @@ -0,0 +1,42 @@ +package com.stuypulse.robot.constants; + + +import com.stuypulse.robot.util.CustomCamera; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; + +/*- + * File containing all of the configurations that different motors require. + * + * Such configurations include: + * - If it is Inverted + * - The Idle Mode of the Motor + * - The Current Limit + * - The Open Loop Ramp Rate + */ +public interface Cameras { + + public static final CameraConfig DEFAULT_CAMERA = new CameraConfig("default", + new Pose3d(Units.inchesToMeters(12.5), + Units.inchesToMeters(11.5), + Units.inchesToMeters(8.5), + new Rotation3d(0, 0, 0))); + + public static final CameraConfig[] ROBOT_CAMERAS = new CameraConfig[]{DEFAULT_CAMERA}; + + public static class CameraConfig { + public final String NAME; + public final Pose3d POSITION; + + public CameraConfig(String name, Pose3d position) { + NAME = name; + POSITION = position; + } + + public CustomCamera getCamera() { + return new CustomCamera(NAME, POSITION); + } + } +} diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java new file mode 100644 index 0000000..7d6ee69 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/constants/Field.java @@ -0,0 +1,218 @@ +package com.stuypulse.robot.constants; + +import com.stuypulse.stuylib.network.SmartNumber; + +import com.stuypulse.robot.RobotContainer; +// import com.stuypulse.robot.subsystems.Manager.ScoreSide; +// import com.stuypulse.robot.util.AllianceUtil; +import com.stuypulse.robot.util.Fiducial; + +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.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +public interface Field { + double WIDTH = 16.54; + double HEIGHT = 8.02; + + public static final double FIDUCIAL_SIZE = 0.15716; + + Fiducial TAGS[] = { + new Fiducial(1,new Pose3d(new Translation3d(0, 0, Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(90),Units.degreesToRadians(0),Units.degreesToRadians(0)))), + // new Fiducial(1,new Pose3d(new Translation3d(WIDTH, (1 + Units.inchesToMeters(28.125)), Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(90),Units.degreesToRadians(0),Units.degreesToRadians(180)))) + // new Fiducial(0,new Pose3d(new Translation3d(WIDTH, 0, Units.inchesToMeters(61.5)), new Rotation3d(Units.degreesToRadians(90),Units.degreesToRadians(0),Units.degreesToRadians(180)))), // home + // new Fiducial(1,new Pose3d(new Translation3d(WIDTH, Units.inchesToMeters(33), Units.inchesToMeters(61.5)), new Rotation3d(Units.degreesToRadians(90),Units.degreesToRadians(0),Units.degreesToRadians(180)))), + // new Fiducial(0,new Pose3d(new Translation3d(8.23, 1, Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(180)))), // flipped test + // new Fiducial(1,new Pose3d(new Translation3d(8.23, 1 + Units.inchesToMeters(28.125), Units.inchesToMeters(30)), new Rotation3d(Units.degreesToRadians(0),Units.degreesToRadians(0),Units.degreesToRadians(180)))) + }; + + public static boolean isValidTag(int id) { + for (Fiducial tag : TAGS) + if (tag.getID() == id) return true; + return false; + } + + public static Fiducial getTag(int id) { + for (Fiducial tag : TAGS) + if (tag.getID() == id) return tag; + return null; + } + + public static double[] getTagLayout(Fiducial[] fiducials) { + double[] layout = new double[fiducials.length * 7]; + + for (int i = 0; i < fiducials.length; i++) { + Fiducial tag = fiducials[i]; + layout[i * 7 + 0] = tag.getID(); + layout[i * 7 + 1] = tag.getPose().getTranslation().getX(); + layout[i * 7 + 2] = tag.getPose().getTranslation().getY(); + layout[i * 7 + 3] = tag.getPose().getTranslation().getZ(); + layout[i * 7 + 4] = tag.getPose().getRotation().getX(); + layout[i * 7 + 5] = tag.getPose().getRotation().getY(); + layout[i * 7 + 6] = tag.getPose().getRotation().getZ(); + } + + return layout; + } + + //XXX: Commented out from Jim's code + // SmartNumber CHARGING_STATION_CENTER = new SmartNumber("Field/Charging Station Center", 172.631 - 14); + // double PEG_TO_CHARGING_STATION_EDGE = Units.inchesToMeters(60.69); + + // double GRID_DEPTH = Units.inchesToMeters(54.25); + + // // intake offset from center to the right + // double INTAKE_OFFSET_RIGHT = Units.inchesToMeters(1.625); + + // Pose2d BLUE_APRIL_TAGS[] = { + // // 1-4 + // new Pose2d(WIDTH - Units.inchesToMeters(40.45), Units.inchesToMeters(42.19), Rotation2d.fromDegrees(180)), + // new Pose2d(WIDTH - Units.inchesToMeters(40.45), Units.inchesToMeters(108.19), Rotation2d.fromDegrees(180)), + // new Pose2d(WIDTH - Units.inchesToMeters(40.45), Units.inchesToMeters(174.19), Rotation2d.fromDegrees(180)), + // new Pose2d(WIDTH - Units.inchesToMeters(14.25), Units.inchesToMeters(265.74), Rotation2d.fromDegrees(180)), + + // // 5-8 + // new Pose2d(Units.inchesToMeters(14.25), Units.inchesToMeters(265.74), new Rotation2d(0)), + // new Pose2d(Units.inchesToMeters(40.45), Units.inchesToMeters(174.19), new Rotation2d(0)), + // new Pose2d(Units.inchesToMeters(40.45), Units.inchesToMeters(108.19), new Rotation2d(0)), + // new Pose2d(Units.inchesToMeters(40.45), Units.inchesToMeters(42.19), new Rotation2d(0)), + // }; + + public static boolean isValidAprilTagId(int id) { + return id >= 1 && id <= 8; + } + + // public static Pose2d getAprilTagFromId(int id) { + // if (RobotContainer.getCachedAlliance() == Alliance.Blue) { + // return BLUE_APRIL_TAGS[id - 1]; + // } else { + // return AllianceUtil.getMirroredPose(BLUE_APRIL_TAGS[8 - id]); + // } + // } + + public interface Pegs { + // LAB VALUES + double RED_Y[] = { + 7.494778 - 0.075, + 6.935978 - 0.075, + 6.377178 - 0.075, + 5.843778 - 0.075, + 5.259578 - 0.075, + 4.700778 - 0.075, + 4.141978 - 0.075, + 3.583178 - 0.075, + 3.024378 - 0.075 + }; + + double BLUE_Y[] = { + 4.9784 - 0.06, + 4.4196 - 0.06, + 3.8608 - 0.06, + 3.3020 - 0.06, + 2.7432 - 0.06, + 2.1844 - 0.06, + 1.6256 - 0.06, + 1.0668 - 0.06, + 0.5080 - 0.06 + }; + } + + //XXX: Commented out from Jim's code + // static class RedBlueNumber extends Number { private double red, blue; public RedBlueNumber(double red, double blue) { this.red = red; this.blue = blue; } public double doubleValue() { if (RobotContainer.getCachedAlliance() == Alliance.Red) return red; return blue; } public int intValue() { return (int) doubleValue(); } public float floatValue() { return (float) doubleValue(); } public long longValue() { return (long) doubleValue(); } } + + // public interface ScoreXPoses { + // public interface High { + // Number CUBE_BACK = 1.846; + // Number CUBE_FRONT = 1.825; + // Number CONE_TIP_IN = 1.75; + // Number CONE_TIP_OUT = 1.772 - Units.inchesToMeters(2); + // } + + // public interface Mid { + // Number CUBE_BACK = 1.881; + // Number CUBE_FRONT = 2.106; + // Number CONE_TIP_IN = 2.21; + // Number CONE_TIP_OUT = 2.18; + // } + + // public interface Low { + // Number BACK = 1.9; + // Number FRONT = 1.825; + // } + // } + + // // red left to right + // public interface ScoreYPoses { + // public static double[] getYPoseArray(Alliance alliance, ScoreSide side) { + // if (side == ScoreSide.FRONT) + // return alliance == Alliance.Red ? Front.RED_Y_POSES : Front.BLUE_Y_POSES; + // else + // return alliance == Alliance.Red ? Back.RED_Y_POSES : Back.BLUE_Y_POSES; + // } + + // public static double middleToBack(double midYPose) { + // return midYPose + Field.INTAKE_OFFSET_RIGHT; + // } + + // public interface Back { + // double RED_Y_POSES[] = { + // middleToBack(Pegs.RED_Y[0]), + // middleToBack(Pegs.RED_Y[1]), + // middleToBack(Pegs.RED_Y[2]), + // middleToBack(Pegs.RED_Y[3]), + // middleToBack(Pegs.RED_Y[4]), + // middleToBack(Pegs.RED_Y[5]), + // middleToBack(Pegs.RED_Y[6]), + // middleToBack(Pegs.RED_Y[7]), + // middleToBack(Pegs.RED_Y[8]) + // }; + + // double BLUE_Y_POSES[] = { + // middleToBack(Pegs.BLUE_Y[0]), + // middleToBack(Pegs.BLUE_Y[1]), + // middleToBack(Pegs.BLUE_Y[2]), + // middleToBack(Pegs.BLUE_Y[3]), + // middleToBack(Pegs.BLUE_Y[4]), + // middleToBack(Pegs.BLUE_Y[5]), + // middleToBack(Pegs.BLUE_Y[6]), + // middleToBack(Pegs.BLUE_Y[7]), + // middleToBack(Pegs.BLUE_Y[8]) + // }; + // } + + // public interface Front { + // public static double middleToFront(double midYPose) { + // return midYPose - Field.INTAKE_OFFSET_RIGHT; + // } + + // double RED_Y_POSES[] = { + // middleToFront(Pegs.RED_Y[0]), + // middleToFront(Pegs.RED_Y[1]), + // middleToFront(Pegs.RED_Y[2]), + // middleToFront(Pegs.RED_Y[3]), + // middleToFront(Pegs.RED_Y[4]), + // middleToFront(Pegs.RED_Y[5]), + // middleToFront(Pegs.RED_Y[6]), + // middleToFront(Pegs.RED_Y[7]), + // middleToFront(Pegs.RED_Y[8]) + // }; + + // double BLUE_Y_POSES[] = { + // middleToFront(Pegs.BLUE_Y[0]), + // middleToFront(Pegs.BLUE_Y[1]), + // middleToFront(Pegs.BLUE_Y[2]), + // middleToFront(Pegs.BLUE_Y[3]), + // middleToFront(Pegs.BLUE_Y[4]), + // middleToFront(Pegs.BLUE_Y[5]), + // middleToFront(Pegs.BLUE_Y[6]), + // middleToFront(Pegs.BLUE_Y[7]), + // middleToFront(Pegs.BLUE_Y[8]) + // }; + // } + // } + +} diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java index 59d620a..e308c3b 100644 --- a/src/main/java/com/stuypulse/robot/constants/Settings.java +++ b/src/main/java/com/stuypulse/robot/constants/Settings.java @@ -8,6 +8,8 @@ import com.stuypulse.stuylib.network.SmartBoolean; import com.stuypulse.stuylib.network.SmartNumber; +import edu.wpi.first.math.util.Units; + /*- * File containing tunable settings for every subsystem on the robot. * @@ -17,6 +19,9 @@ public interface Settings { public interface Drivetrain { int CURRENT_LIMIT = 60; + + //TODO: ask rain for true track width currently using cad with +-0.1 + double TRACK_WIDTH = Units.inchesToMeters(23.83); } public interface Launcher { diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/AbstractOdometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/AbstractOdometry.java new file mode 100644 index 0000000..9697532 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/AbstractOdometry.java @@ -0,0 +1,38 @@ +package com.stuypulse.robot.subsystems.odometry; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public abstract class AbstractOdometry extends SubsystemBase { + + private static final AbstractOdometry instance; + + static { + instance = new Odometry(); + } + + public static AbstractOdometry getInstance() { + return instance; + } + + protected AbstractOdometry() {} + + public abstract Field2d getField(); + + public abstract Pose2d getPose(); + + public abstract void resetOdometery(Pose2d pose2d); + + public abstract void updateOdometry(); + + public final Translation2d getTranslation() { + return getPose().getTranslation(); + } + + public final Rotation2d getRotation() { + return getPose().getRotation(); + } +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java new file mode 100644 index 0000000..14448cc --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/odometry/Odometry.java @@ -0,0 +1,74 @@ +package com.stuypulse.robot.subsystems.odometry; + +import com.stuypulse.robot.subsystems.drivetrain.Drivetrain; +import com.stuypulse.robot.subsystems.vision.AbstractVision; +import com.stuypulse.robot.util.Fiducial; +import com.stuypulse.robot.util.VisionData; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class Odometry extends AbstractOdometry { + + private final DifferentialDriveOdometry odometry; + private final Drivetrain drivetrain; + + private final Field2d field; + private final FieldObject2d odometryPose2D; + + public Odometry() { + this.drivetrain = Drivetrain.getInstance(); + this.odometry = new DifferentialDriveOdometry(getRotation(), drivetrain.getLeftDistance(), drivetrain.getRightDistance()); + resetOdometery(new Pose2d()); + + this.field = new Field2d(); + this.odometryPose2D = field.getObject("Odometry Pose"); + SmartDashboard.putData("Field", field); + } + + @Override + public Field2d getField() { + return field; + } + + @Override + public Pose2d getPose() { + updateOdometry(); + return odometry.getPoseMeters(); + } + + @Override + public void updateOdometry() { + odometry.update(getRotation(), drivetrain.getLeftDistance(), drivetrain.getRightDistance()); + } + + @Override + public void resetOdometery(Pose2d pose2d) { + odometry.resetPosition(getRotation(), drivetrain.getLeftDistance(), drivetrain.getRightDistance(), pose2d); + } + + @Override + public void periodic() { + updateOdometry(); + field.setRobotPose(getPose()); + + SmartDashboard.putData("Odometry/Field", field); + + for (VisionData result : AbstractVision.getInstance().getOutput()) { + + Fiducial primaryTag = result.getPrimaryTag(); + double distance = result.calculateDistanceToTag(primaryTag); + + SmartDashboard.putNumber("Odometry/Primary Tag/Distance (m)", distance); + } + + odometryPose2D.setPose(odometry.getPoseMeters()); + SmartDashboard.putNumber("Odometry/Odometry/X (m)", odometry.getPoseMeters().getTranslation().getX()); + SmartDashboard.putNumber("Odometry/Odometry/Y (m)", odometry.getPoseMeters().getTranslation().getY()); + SmartDashboard.putNumber("Odometry/Odometry/Rotation (deg)", odometry.getPoseMeters().getRotation().getDegrees()); + } + +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/AbstractVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/AbstractVision.java new file mode 100644 index 0000000..af43ef6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/AbstractVision.java @@ -0,0 +1,24 @@ +package com.stuypulse.robot.subsystems.vision; + +import com.stuypulse.robot.util.VisionData; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import java.util.List; + +public abstract class AbstractVision extends SubsystemBase { + + private static final AbstractVision instance; + + static { + instance = new Vision(); + } + + public static AbstractVision getInstance() { + return instance; + } + + protected AbstractVision() {} + + public abstract List getOutput(); +} diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/Vision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/Vision.java new file mode 100644 index 0000000..ea04dd6 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/subsystems/vision/Vision.java @@ -0,0 +1,67 @@ +package com.stuypulse.robot.subsystems.vision; + +import com.stuypulse.robot.constants.Cameras; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; +import com.stuypulse.robot.util.CustomCamera; +import com.stuypulse.robot.util.Fiducial; +import com.stuypulse.robot.util.VisionData; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import java.util.ArrayList; +import java.util.List; + +public class Vision extends AbstractVision { + + private final CustomCamera[] cameras; + private List outputs; + + protected Vision() { + + cameras = new CustomCamera[Cameras.ROBOT_CAMERAS.length]; + + + for (int i = 0; i < Cameras.ROBOT_CAMERAS.length; i++) { + cameras[i] = Cameras.ROBOT_CAMERAS[i].getCamera(); + } + + outputs = new ArrayList<>(); + } + + @Override + public List getOutput() { + return outputs; + } + + @Override + public void periodic() { + + outputs = new ArrayList<>(); + + for (CustomCamera camera: cameras) { + camera.getVisionData().ifPresent(data -> { + outputs.add(data); + + Fiducial tag = data.getPrimaryTag(); + + AbstractOdometry.getInstance().getField().getObject("Fiducial").setPose(tag.getPose().toPose2d()); + + String prefix = "Vision/" + camera.getCameraName(); + + SmartDashboard.putNumber(prefix + "/Pose X", data.robotPose.getX()); + SmartDashboard.putNumber(prefix + "/Pose Y", data.robotPose.getY()); + SmartDashboard.putNumber(prefix + "/Pose Z", data.robotPose.getZ()); + + SmartDashboard.putNumber(prefix + "/Distance to Tag", data.calculateDistanceToTag(data.getPrimaryTag())); + + SmartDashboard.putNumber( + prefix + "/Pose Rotation", + Units.radiansToDegrees(data.robotPose.getRotation().getAngle())); + SmartDashboard.putNumber(prefix + "/Timestamp", data.timestamp); + }); + } + + SmartDashboard.putBoolean("Vision/Has Any Data", !outputs.isEmpty()); + } +} diff --git a/src/main/java/com/stuypulse/robot/util/CustomCamera.java b/src/main/java/com/stuypulse/robot/util/CustomCamera.java new file mode 100644 index 0000000..81c13b8 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/CustomCamera.java @@ -0,0 +1,142 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.robot.constants.Field; +import com.stuypulse.robot.subsystems.odometry.AbstractOdometry; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.networktables.DoubleArraySubscriber; +import edu.wpi.first.networktables.DoubleSubscriber; +import edu.wpi.first.networktables.IntegerArraySubscriber; +import edu.wpi.first.networktables.IntegerSubscriber; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; + +import java.util.Optional; + +public class CustomCamera { + + private final Field2d field; + + private final String cameraName; + private final Pose3d cameraPose; + + // Default Values + private final int camera_id = 0; + private final int camera_resolution_width = 1600; + private final int camera_resolution_height = 1200; + private final int camera_auto_exposure = 1; + private final int camera_exposure = 10; + private final double camera_gain = 0.0; + private final double camera_brightness = 0.0; + + private final DoubleArraySubscriber robotPoseSub; + private final DoubleSubscriber latencySub; + private final IntegerSubscriber fpsSub; + private final DoubleArraySubscriber tvecsSub; + private final IntegerArraySubscriber idSub; + + + private double[] rawPoseData; + private double rawLatency; + private double[] rawTvecsData; + private long[] rawIdData; + + public CustomCamera(String cameraName, Pose3d cameraPose) { + + this.field = AbstractOdometry.getInstance().getField(); + + this.cameraName = cameraName; + this.cameraPose = cameraPose; + + NetworkTable table = NetworkTableInstance.getDefault().getTable(cameraName); + + NetworkTable configTable = table.getSubTable("config"); + configTable.getIntegerTopic("camera_id").publish().set(camera_id); + configTable.getIntegerTopic("camera_resolution_width").publish().set(camera_resolution_width); + configTable.getIntegerTopic("camera_resolution_height").publish().set(camera_resolution_height); + configTable.getIntegerTopic("camera_auto_exposure").publish().set(camera_auto_exposure); + configTable.getIntegerTopic("camera_exposure").publish().set(camera_exposure); + configTable.getDoubleTopic("camera_gain").publish().set(camera_gain); + configTable.getDoubleTopic("camera_brightness").publish().set(camera_brightness); + configTable.getDoubleTopic("fiducial_size").publish().set(Field.FIDUCIAL_SIZE); + configTable.getDoubleArrayTopic("fiducial_layout").publish().set(Field.getTagLayout(Field.TAGS)); + configTable.getDoubleArrayTopic("camera_offset").publish() + .set(new double[] { + cameraPose.getX(), + cameraPose.getY(), + cameraPose.getZ(), + Units.radiansToDegrees(cameraPose.getRotation().getX()), + Units.radiansToDegrees(cameraPose.getRotation().getY()), + Units.radiansToDegrees(cameraPose.getRotation().getZ()), + }); + + NetworkTable outputTable = table.getSubTable("output"); + robotPoseSub = outputTable.getDoubleArrayTopic("robot_pose") + .subscribe( + new double[] {}, + PubSubOption.keepDuplicates(true), + PubSubOption.sendAll(true)); + latencySub = outputTable.getDoubleTopic("latency").subscribe(0); + fpsSub = outputTable.getIntegerTopic("fps").subscribe(0); + tvecsSub = outputTable.getDoubleArrayTopic("tvecs") + .subscribe( + new double[] {}, + PubSubOption.keepDuplicates(true), + PubSubOption.sendAll(true)); + idSub = outputTable.getIntegerArrayTopic("ids").subscribe(new long[] {}); + } + + public String getCameraName() { + return cameraName; + } + + private void updateData() { + rawPoseData = robotPoseSub.get(); + rawLatency = latencySub.get(); + rawTvecsData = tvecsSub.get(); + rawIdData = idSub.get(); + } + + private boolean hasData() { + return rawPoseData.length > 0 && + rawTvecsData.length > 0 && + rawIdData.length > 0; + } + + public Optional getVisionData() { + updateData(); + + if (!hasData()) return Optional.empty(); + field.getObject(cameraName).setPose(getRobotPose().toPose2d()); + + double fpgaTime = latencySub.getLastChange() / 1_000_000.0; // replace with Timer.getFPGATimestamp() if breaks + double timestamp = fpgaTime - Units.millisecondsToSeconds(rawLatency); + + return Optional.of(new VisionData(rawIdData, getTvecs(), cameraPose, getRobotPose(), timestamp)); + } + + private Pose3d getRobotPose() { + return new Pose3d( + new Translation3d(rawPoseData[0], rawPoseData[1], rawPoseData[2]), + new Rotation3d( + Units.degreesToRadians(rawPoseData[3]), + Units.degreesToRadians(0), + Units.degreesToRadians(rawPoseData[5]))); + } + + private Translation3d[] getTvecs() { + Translation3d[] tvecs = new Translation3d[rawTvecsData.length / 3]; + for (int i = 0; i < rawTvecsData.length; i += 3) + tvecs[i / 3] = new Translation3d(rawTvecsData[i], rawTvecsData[i + 1], rawTvecsData[i + 2]); + return tvecs; + } + + public long getFPS() { + return fpsSub.get(); + } +} diff --git a/src/main/java/com/stuypulse/robot/util/Fiducial.java b/src/main/java/com/stuypulse/robot/util/Fiducial.java new file mode 100644 index 0000000..723bd8f --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/Fiducial.java @@ -0,0 +1,21 @@ +package com.stuypulse.robot.util; + +import edu.wpi.first.math.geometry.Pose3d; + +public class Fiducial { + private final int id; + private final Pose3d pose; + + public Fiducial(int id, Pose3d pose) { + this.id = id; + this.pose = pose; + } + + public int getID() { + return id; + } + + public Pose3d getPose() { + return pose; + } +} diff --git a/src/main/java/com/stuypulse/robot/util/VisionData.java b/src/main/java/com/stuypulse/robot/util/VisionData.java new file mode 100644 index 0000000..556bfd3 --- /dev/null +++ b/src/main/java/com/stuypulse/robot/util/VisionData.java @@ -0,0 +1,36 @@ +package com.stuypulse.robot.util; + +import com.stuypulse.robot.constants.Field; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation3d; + +public class VisionData { + + public final long[] ids; + public final Translation3d[] tvecs; + public final Pose3d cameraLocation; + public Pose3d robotPose; + public final double timestamp; + + public double calculateDistanceToTag(Fiducial tag) { + return robotPose.getTranslation().getDistance(tag.getPose().getTranslation()); + } + + private int getPrimaryID() { + if (ids.length == 0) return -1; + return (int) ids[0]; + } + + public Fiducial getPrimaryTag() { + return Field.getTag(getPrimaryID()); + } + + public VisionData(long[] ids, Translation3d[] tvecs, Pose3d cameraLocation, Pose3d robotPose, double timestamp) { + this.ids = ids; + this.tvecs = tvecs; + this.cameraLocation = cameraLocation; + this.robotPose = robotPose; + this.timestamp = timestamp; + } +}