//LimelightHelpers v1.9 (REQUIRES 2024.9.1) package frc.robot; import edu.wpi.first.networktables.DoubleArrayEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.TimestampedDoubleArray; import frc.robot.LimelightHelpers.LimelightResults; import frc.robot.LimelightHelpers.PoseEstimate; 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.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; import java.io.IOException; import java.net.HttpURLConnection; import java.net.MalformedURLException; import java.net.URL; import java.util.Map; import java.util.concurrent.CompletableFuture; import com.fasterxml.jackson.annotation.JsonFormat; import com.fasterxml.jackson.annotation.JsonFormat.Shape; import com.fasterxml.jackson.annotation.JsonProperty; import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.DeserializationFeature; import com.fasterxml.jackson.databind.ObjectMapper; import java.util.concurrent.ConcurrentHashMap; public class LimelightHelpers { private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); public static class LimelightTarget_Retro { @JsonProperty("t6c_ts") private double[] cameraPose_TargetSpace; @JsonProperty("t6r_fs") private double[] robotPose_FieldSpace; @JsonProperty("t6r_ts") private double[] robotPose_TargetSpace; @JsonProperty("t6t_cs") private double[] targetPose_CameraSpace; @JsonProperty("t6t_rs") private double[] targetPose_RobotSpace; public Pose3d getCameraPose_TargetSpace() { return toPose3D(cameraPose_TargetSpace); } public Pose3d getRobotPose_FieldSpace() { return toPose3D(robotPose_FieldSpace); } public Pose3d getRobotPose_TargetSpace() { return toPose3D(robotPose_TargetSpace); } public Pose3d getTargetPose_CameraSpace() { return toPose3D(targetPose_CameraSpace); } public Pose3d getTargetPose_RobotSpace() { return toPose3D(targetPose_RobotSpace); } public Pose2d getCameraPose_TargetSpace2D() { return toPose2D(cameraPose_TargetSpace); } public Pose2d getRobotPose_FieldSpace2D() { return toPose2D(robotPose_FieldSpace); } public Pose2d getRobotPose_TargetSpace2D() { return toPose2D(robotPose_TargetSpace); } public Pose2d getTargetPose_CameraSpace2D() { return toPose2D(targetPose_CameraSpace); } public Pose2d getTargetPose_RobotSpace2D() { return toPose2D(targetPose_RobotSpace); } @JsonProperty("ta") public double ta; @JsonProperty("tx") public double tx; @JsonProperty("txp") public double tx_pixels; @JsonProperty("ty") public double ty; @JsonProperty("typ") public double ty_pixels; @JsonProperty("ts") public double ts; public LimelightTarget_Retro() { cameraPose_TargetSpace = new double[6]; robotPose_FieldSpace = new double[6]; robotPose_TargetSpace = new double[6]; targetPose_CameraSpace = new double[6]; targetPose_RobotSpace = new double[6]; } } public static class LimelightTarget_Fiducial { @JsonProperty("fID") public double fiducialID; @JsonProperty("fam") public String fiducialFamily; @JsonProperty("t6c_ts") private double[] cameraPose_TargetSpace; @JsonProperty("t6r_fs") private double[] robotPose_FieldSpace; @JsonProperty("t6r_ts") private double[] robotPose_TargetSpace; @JsonProperty("t6t_cs") private double[] targetPose_CameraSpace; @JsonProperty("t6t_rs") private double[] targetPose_RobotSpace; public Pose3d getCameraPose_TargetSpace() { return toPose3D(cameraPose_TargetSpace); } public Pose3d getRobotPose_FieldSpace() { return toPose3D(robotPose_FieldSpace); } public Pose3d getRobotPose_TargetSpace() { return toPose3D(robotPose_TargetSpace); } public Pose3d getTargetPose_CameraSpace() { return toPose3D(targetPose_CameraSpace); } public Pose3d getTargetPose_RobotSpace() { return toPose3D(targetPose_RobotSpace); } public Pose2d getCameraPose_TargetSpace2D() { return toPose2D(cameraPose_TargetSpace); } public Pose2d getRobotPose_FieldSpace2D() { return toPose2D(robotPose_FieldSpace); } public Pose2d getRobotPose_TargetSpace2D() { return toPose2D(robotPose_TargetSpace); } public Pose2d getTargetPose_CameraSpace2D() { return toPose2D(targetPose_CameraSpace); } public Pose2d getTargetPose_RobotSpace2D() { return toPose2D(targetPose_RobotSpace); } @JsonProperty("ta") public double ta; @JsonProperty("tx") public double tx; @JsonProperty("txp") public double tx_pixels; @JsonProperty("ty") public double ty; @JsonProperty("typ") public double ty_pixels; @JsonProperty("ts") public double ts; public LimelightTarget_Fiducial() { cameraPose_TargetSpace = new double[6]; robotPose_FieldSpace = new double[6]; robotPose_TargetSpace = new double[6]; targetPose_CameraSpace = new double[6]; targetPose_RobotSpace = new double[6]; } } public static class LimelightTarget_Barcode { } public static class LimelightTarget_Classifier { @JsonProperty("class") public String className; @JsonProperty("classID") public double classID; @JsonProperty("conf") public double confidence; @JsonProperty("zone") public double zone; @JsonProperty("tx") public double tx; @JsonProperty("txp") public double tx_pixels; @JsonProperty("ty") public double ty; @JsonProperty("typ") public double ty_pixels; public LimelightTarget_Classifier() { } } public static class LimelightTarget_Detector { @JsonProperty("class") public String className; @JsonProperty("classID") public double classID; @JsonProperty("conf") public double confidence; @JsonProperty("ta") public double ta; @JsonProperty("tx") public double tx; @JsonProperty("txp") public double tx_pixels; @JsonProperty("ty") public double ty; @JsonProperty("typ") public double ty_pixels; public LimelightTarget_Detector() { } } public static class LimelightResults { public String error; @JsonProperty("pID") public double pipelineID; @JsonProperty("tl") public double latency_pipeline; @JsonProperty("cl") public double latency_capture; public double latency_jsonParse; @JsonProperty("ts") public double timestamp_LIMELIGHT_publish; @JsonProperty("ts_rio") public double timestamp_RIOFPGA_capture; @JsonProperty("v") @JsonFormat(shape = Shape.NUMBER) public boolean valid; @JsonProperty("botpose") public double[] botpose; @JsonProperty("botpose_wpired") public double[] botpose_wpired; @JsonProperty("botpose_wpiblue") public double[] botpose_wpiblue; @JsonProperty("botpose_tagcount") public double botpose_tagcount; @JsonProperty("botpose_span") public double botpose_span; @JsonProperty("botpose_avgdist") public double botpose_avgdist; @JsonProperty("botpose_avgarea") public double botpose_avgarea; @JsonProperty("t6c_rs") public double[] camerapose_robotspace; public Pose3d getBotPose3d() { return toPose3D(botpose); } public Pose3d getBotPose3d_wpiRed() { return toPose3D(botpose_wpired); } public Pose3d getBotPose3d_wpiBlue() { return toPose3D(botpose_wpiblue); } public Pose2d getBotPose2d() { return toPose2D(botpose); } public Pose2d getBotPose2d_wpiRed() { return toPose2D(botpose_wpired); } public Pose2d getBotPose2d_wpiBlue() { return toPose2D(botpose_wpiblue); } @JsonProperty("Retro") public LimelightTarget_Retro[] targets_Retro; @JsonProperty("Fiducial") public LimelightTarget_Fiducial[] targets_Fiducials; @JsonProperty("Classifier") public LimelightTarget_Classifier[] targets_Classifier; @JsonProperty("Detector") public LimelightTarget_Detector[] targets_Detector; @JsonProperty("Barcode") public LimelightTarget_Barcode[] targets_Barcode; public LimelightResults() { botpose = new double[6]; botpose_wpired = new double[6]; botpose_wpiblue = new double[6]; camerapose_robotspace = new double[6]; targets_Retro = new LimelightTarget_Retro[0]; targets_Fiducials = new LimelightTarget_Fiducial[0]; targets_Classifier = new LimelightTarget_Classifier[0]; targets_Detector = new LimelightTarget_Detector[0]; targets_Barcode = new LimelightTarget_Barcode[0]; } } public static class RawFiducial { public int id = 0; public double txnc = 0; public double tync = 0; public double ta = 0; public double distToCamera = 0; public double distToRobot = 0; public double ambiguity = 0; public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { this.id = id; this.txnc = txnc; this.tync = tync; this.ta = ta; this.distToCamera = distToCamera; this.distToRobot = distToRobot; this.ambiguity = ambiguity; } } public static class RawDetection { public int classId = 0; public double txnc = 0; public double tync = 0; public double ta = 0; public double corner0_X = 0; public double corner0_Y = 0; public double corner1_X = 0; public double corner1_Y = 0; public double corner2_X = 0; public double corner2_Y = 0; public double corner3_X = 0; public double corner3_Y = 0; public RawDetection(int classId, double txnc, double tync, double ta, double corner0_X, double corner0_Y, double corner1_X, double corner1_Y, double corner2_X, double corner2_Y, double corner3_X, double corner3_Y ) { this.classId = classId; this.txnc = txnc; this.tync = tync; this.ta = ta; this.corner0_X = corner0_X; this.corner0_Y = corner0_Y; this.corner1_X = corner1_X; this.corner1_Y = corner1_Y; this.corner2_X = corner2_X; this.corner2_Y = corner2_Y; this.corner3_X = corner3_X; this.corner3_Y = corner3_Y; } } public static class PoseEstimate { public Pose2d pose; public double timestampSeconds; public double latency; public int tagCount; public double tagSpan; public double avgTagDist; public double avgTagArea; public RawFiducial[] rawFiducials; /** * Makes a PoseEstimate object with default values */ public PoseEstimate() { this.pose = new Pose2d(); this.timestampSeconds = 0; this.latency = 0; this.tagCount = 0; this.tagSpan = 0; this.avgTagDist = 0; this.avgTagArea = 0; this.rawFiducials = new RawFiducial[]{}; } public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, int tagCount, double tagSpan, double avgTagDist, double avgTagArea, RawFiducial[] rawFiducials) { this.pose = pose; this.timestampSeconds = timestampSeconds; this.latency = latency; this.tagCount = tagCount; this.tagSpan = tagSpan; this.avgTagDist = avgTagDist; this.avgTagArea = avgTagArea; this.rawFiducials = rawFiducials; } } private static ObjectMapper mapper; /** * Print JSON Parse time to the console in milliseconds */ static boolean profileJSON = false; static final String sanitizeName(String name) { if (name == "" || name == null) { return "limelight"; } return name; } public static Pose3d toPose3D(double[] inData){ if(inData.length < 6) { //System.err.println("Bad LL 3D Pose Data!"); return new Pose3d(); } return new Pose3d( new Translation3d(inData[0], inData[1], inData[2]), new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), Units.degreesToRadians(inData[5]))); } public static Pose2d toPose2D(double[] inData){ if(inData.length < 6) { //System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); return new Pose2d(tran2d, r2d); } /** * Converts a Pose3d object to an array of doubles. * * @param pose The Pose3d object to convert. * @return The array of doubles representing the pose. **/ public static double[] pose3dToArray(Pose3d pose) { double[] result = new double[6]; result[0] = pose.getTranslation().getX(); result[1] = pose.getTranslation().getY(); result[2] = pose.getTranslation().getZ(); result[3] = Units.radiansToDegrees(pose.getRotation().getX()); result[4] = Units.radiansToDegrees(pose.getRotation().getY()); result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); return result; } /** * Converts a Pose2d object to an array of doubles. * * @param pose The Pose2d object to convert. * @return The array of doubles representing the pose. **/ public static double[] pose2dToArray(Pose2d pose) { double[] result = new double[6]; result[0] = pose.getTranslation().getX(); result[1] = pose.getTranslation().getY(); result[2] = 0; result[3] = Units.radiansToDegrees(0); result[4] = Units.radiansToDegrees(0); result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); return result; } private static double extractArrayEntry(double[] inData, int position){ if(inData.length < position+1) { return 0; } return inData[position]; } private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); TimestampedDoubleArray tsValue = poseEntry.getAtomic(); double[] poseArray = tsValue.value; long timestamp = tsValue.timestamp; if (poseArray.length == 0) { // Handle the case where no data is available return null; // or some default PoseEstimate } var pose = toPose2D(poseArray); double latency = extractArrayEntry(poseArray, 6); int tagCount = (int)extractArrayEntry(poseArray, 7); double tagSpan = extractArrayEntry(poseArray, 8); double tagDist = extractArrayEntry(poseArray, 9); double tagArea = extractArrayEntry(poseArray, 10); // Convert server timestamp from microseconds to seconds and adjust for latency double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); RawFiducial[] rawFiducials = new RawFiducial[tagCount]; int valsPerFiducial = 7; int expectedTotalVals = 11 + valsPerFiducial * tagCount; if (poseArray.length != expectedTotalVals) { // Don't populate fiducials } else { for(int i = 0; i < tagCount; i++) { int baseIndex = 11 + (i * valsPerFiducial); int id = (int)poseArray[baseIndex]; double txnc = poseArray[baseIndex + 1]; double tync = poseArray[baseIndex + 2]; double ta = poseArray[baseIndex + 3]; double distToCamera = poseArray[baseIndex + 4]; double distToRobot = poseArray[baseIndex + 5]; double ambiguity = poseArray[baseIndex + 6]; rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); } } return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); } private static RawFiducial[] getRawFiducials(String limelightName) { var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); var rawFiducialArray = entry.getDoubleArray(new double[0]); int valsPerEntry = 7; if (rawFiducialArray.length % valsPerEntry != 0) { return new RawFiducial[0]; } int numFiducials = rawFiducialArray.length / valsPerEntry; RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; for (int i = 0; i < numFiducials; i++) { int baseIndex = i * valsPerEntry; int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); } return rawFiducials; } public static RawDetection[] getRawDetections(String limelightName) { var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); var rawDetectionArray = entry.getDoubleArray(new double[0]); int valsPerEntry = 11; if (rawDetectionArray.length % valsPerEntry != 0) { return new RawDetection[0]; } int numDetections = rawDetectionArray.length / valsPerEntry; RawDetection[] rawDetections = new RawDetection[numDetections]; for (int i = 0; i < numDetections; i++) { int baseIndex = i * valsPerEntry; // Starting index for this detection's data int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); } return rawDetections; } public static void printPoseEstimate(PoseEstimate pose) { if (pose == null) { System.out.println("No PoseEstimate available."); return; } System.out.printf("Pose Estimate Information:%n"); System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); System.out.printf("Latency: %.3f ms%n", pose.latency); System.out.printf("Tag Count: %d%n", pose.tagCount); System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); System.out.println(); if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { System.out.println("No RawFiducials data available."); return; } System.out.println("Raw Fiducials Details:"); for (int i = 0; i < pose.rawFiducials.length; i++) { RawFiducial fiducial = pose.rawFiducials[i]; System.out.printf(" Fiducial #%d:%n", i + 1); System.out.printf(" ID: %d%n", fiducial.id); System.out.printf(" TXNC: %.2f%n", fiducial.txnc); System.out.printf(" TYNC: %.2f%n", fiducial.tync); System.out.printf(" TA: %.2f%n", fiducial.ta); System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); System.out.println(); } } public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } public static void Flush() { NetworkTableInstance.getDefault().flush(); } public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { return getLimelightNTTable(tableName).getEntry(entryName); } public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { String key = tableName + "/" + entryName; return doubleArrayEntries.computeIfAbsent(key, k -> { NetworkTable table = getLimelightNTTable(tableName); return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); }); } public static double getLimelightNTDouble(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); } public static void setLimelightNTDouble(String tableName, String entryName, double val) { getLimelightNTTableEntry(tableName, entryName).setDouble(val); } public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); } public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); } public static String getLimelightNTString(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getString(""); } public static String[] getLimelightNTStringArray(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); } public static URL getLimelightURLString(String tableName, String request) { String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; URL url; try { url = new URL(urlString); return url; } catch (MalformedURLException e) { System.err.println("bad LL URL"); } return null; } ///// ///// public static double getTX(String limelightName) { return getLimelightNTDouble(limelightName, "tx"); } public static double getTY(String limelightName) { return getLimelightNTDouble(limelightName, "ty"); } public static double getTA(String limelightName) { return getLimelightNTDouble(limelightName, "ta"); } public static double[] getT2DArray(String limelightName) { return getLimelightNTDoubleArray(limelightName, "t2d"); } public static int getTargetCount(String limelightName) { double[] t2d = getT2DArray(limelightName); if(t2d.length == 17) { return (int)t2d[1]; } return 0; } public static int getClassifierClassIndex (String limelightName) { double[] t2d = getT2DArray(limelightName); if(t2d.length == 17) { return (int)t2d[10]; } return 0; } public static int getDetectorClassIndex (String limelightName) { double[] t2d = getT2DArray(limelightName); if(t2d.length == 17) { return (int)t2d[11]; } return 0; } public static String getClassifierClass (String limelightName) { return getLimelightNTString(limelightName, "tcclass"); } public static String getDetectorClass (String limelightName) { return getLimelightNTString(limelightName, "tdclass"); } public static double getLatency_Pipeline(String limelightName) { return getLimelightNTDouble(limelightName, "tl"); } public static double getLatency_Capture(String limelightName) { return getLimelightNTDouble(limelightName, "cl"); } public static double getCurrentPipelineIndex(String limelightName) { return getLimelightNTDouble(limelightName, "getpipe"); } public static String getCurrentPipelineType(String limelightName) { return getLimelightNTString(limelightName, "getpipetype"); } public static String getJSONDump(String limelightName) { return getLimelightNTString(limelightName, "json"); } /** * Switch to getBotPose * * @param limelightName * @return */ @Deprecated public static double[] getBotpose(String limelightName) { return getLimelightNTDoubleArray(limelightName, "botpose"); } /** * Switch to getBotPose_wpiRed * * @param limelightName * @return */ @Deprecated public static double[] getBotpose_wpiRed(String limelightName) { return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); } /** * Switch to getBotPose_wpiBlue * * @param limelightName * @return */ @Deprecated public static double[] getBotpose_wpiBlue(String limelightName) { return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); } public static double[] getBotPose(String limelightName) { return getLimelightNTDoubleArray(limelightName, "botpose"); } public static double[] getBotPose_wpiRed(String limelightName) { return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); } public static double[] getBotPose_wpiBlue(String limelightName) { return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); } public static double[] getBotPose_TargetSpace(String limelightName) { return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); } public static double[] getCameraPose_TargetSpace(String limelightName) { return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); } public static double[] getTargetPose_CameraSpace(String limelightName) { return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); } public static double[] getTargetPose_RobotSpace(String limelightName) { return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); } public static double[] getTargetColor(String limelightName) { return getLimelightNTDoubleArray(limelightName, "tc"); } public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } public static String getNeuralClassID(String limelightName) { return getLimelightNTString(limelightName, "tclass"); } public static String[] getRawBarcodeData(String limelightName) { return getLimelightNTStringArray(limelightName, "rawbarcodes"); } ///// ///// public static Pose3d getBotPose3d(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); return toPose3D(poseArray); } public static Pose3d getBotPose3d_wpiRed(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); return toPose3D(poseArray); } public static Pose3d getBotPose3d_wpiBlue(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); return toPose3D(poseArray); } public static Pose3d getBotPose3d_TargetSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); return toPose3D(poseArray); } public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); return toPose3D(poseArray); } public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); return toPose3D(poseArray); } public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); return toPose3D(poseArray); } public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); return toPose3D(poseArray); } /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) * * @param limelightName * @return */ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { double[] result = getBotPose_wpiBlue(limelightName); return toPose2D(result); } /** * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE * alliance * * @param limelightName * @return */ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpiblue"); } /** * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE * alliance * * @param limelightName * @return */ public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); } /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) * * @param limelightName * @return */ public static Pose2d getBotPose2d_wpiRed(String limelightName) { double[] result = getBotPose_wpiRed(limelightName); return toPose2D(result); } /** * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED * alliance * @param limelightName * @return */ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_wpired"); } /** * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED * alliance * @param limelightName * @return */ public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); } /** * Gets the Pose2d for easy use with Odometry vision pose estimator * (addVisionMeasurement) * * @param limelightName * @return */ public static Pose2d getBotPose2d(String limelightName) { double[] result = getBotPose(limelightName); return toPose2D(result); } public static boolean getTV(String limelightName) { return 1.0 == getLimelightNTDouble(limelightName, "tv"); } ///// ///// public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } public static void setPriorityTagID(String limelightName, int ID) { setLimelightNTDouble(limelightName, "priorityid", ID); } /** * The LEDs will be controlled by Limelight pipeline settings, and not by robot * code. */ public static void setLEDMode_PipelineControl(String limelightName) { setLimelightNTDouble(limelightName, "ledMode", 0); } public static void setLEDMode_ForceOff(String limelightName) { setLimelightNTDouble(limelightName, "ledMode", 1); } public static void setLEDMode_ForceBlink(String limelightName) { setLimelightNTDouble(limelightName, "ledMode", 2); } public static void setLEDMode_ForceOn(String limelightName) { setLimelightNTDouble(limelightName, "ledMode", 3); } public static void setStreamMode_Standard(String limelightName) { setLimelightNTDouble(limelightName, "stream", 0); } public static void setStreamMode_PiPMain(String limelightName) { setLimelightNTDouble(limelightName, "stream", 1); } public static void setStreamMode_PiPSecondary(String limelightName) { setLimelightNTDouble(limelightName, "stream", 2); } /** * Sets the crop window. The crop window in the UI must be completely open for * dynamic cropping to work. */ public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { double[] entries = new double[4]; entries[0] = cropXMin; entries[1] = cropXMax; entries[2] = cropYMin; entries[3] = cropYMax; setLimelightNTDoubleArray(limelightName, "crop", entries); } /** * Sets 3D offset point for easy 3D targeting. */ public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { double[] entries = new double[3]; entries[0] = offsetX; entries[1] = offsetY; entries[2] = offsetZ; setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); } public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, double pitch, double pitchRate, double roll, double rollRate) { SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); } public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, double pitch, double pitchRate, double roll, double rollRate) { SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); } private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, double pitch, double pitchRate, double roll, double rollRate, boolean flush) { double[] entries = new double[6]; entries[0] = yaw; entries[1] = yawRate; entries[2] = pitch; entries[3] = pitchRate; entries[4] = roll; entries[5] = rollRate; setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); if(flush) { Flush(); } } public static void SetFidcuial3DOffset(String limelightName, double x, double y, double z) { double[] entries = new double[3]; entries[0] = x; entries[1] = y; entries[2] = z; setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); } public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { double[] validIDsDouble = new double[validIDs.length]; for (int i = 0; i < validIDs.length; i++) { validIDsDouble[i] = validIDs[i]; } setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); } public static void SetFiducialDownscalingOverride(String limelightName, float downscale) { int d = 0; // pipeline if (downscale == 1.0) { d = 1; } if (downscale == 1.5) { d = 2; } if (downscale == 2) { d = 3; } if (downscale == 3) { d = 4; } if (downscale == 4) { d = 5; } setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); } public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; entries[1] = side; entries[2] = up; entries[3] = roll; entries[4] = pitch; entries[5] = yaw; setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); } ///// ///// public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); } public static double[] getPythonScriptData(String limelightName) { return getLimelightNTDoubleArray(limelightName, "llpython"); } ///// ///// /** * Asynchronously take snapshot. */ public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { return CompletableFuture.supplyAsync(() -> { return SYNCH_TAKESNAPSHOT(tableName, snapshotName); }); } private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { URL url = getLimelightURLString(tableName, "capturesnapshot"); try { HttpURLConnection connection = (HttpURLConnection) url.openConnection(); connection.setRequestMethod("GET"); if (snapshotName != null && snapshotName != "") { connection.setRequestProperty("snapname", snapshotName); } int responseCode = connection.getResponseCode(); if (responseCode == 200) { return true; } else { System.err.println("Bad LL Request"); } } catch (IOException e) { System.err.println(e.getMessage()); } return false; } /** * Parses Limelight's JSON results dump into a LimelightResults Object */ public static LimelightResults getLatestResults(String limelightName) { long start = System.nanoTime(); LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); if (mapper == null) { mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); } try { results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); } catch (JsonProcessingException e) { results.error = "lljson error: " + e.getMessage(); } long end = System.nanoTime(); double millis = (end - start) * .000001; results.latency_jsonParse = millis; if (profileJSON) { System.out.printf("lljson: %.2f\r\n", millis); } return results; } }