From 4a3cf2fb2bea81abffdf400d1c74309ad0e4eff9 Mon Sep 17 00:00:00 2001 From: Strflightmight09 <148347057+Strflightmight09@users.noreply.github.com> Date: Sat, 14 Sep 2024 22:29:54 +0300 Subject: [PATCH] Updated limelight helpers --- .../hardware/misc/FiducialLimelight.java | 244 -------- .../trigon/utilities/LimelightHelpers.java | 546 ++++++++++++++++-- 2 files changed, 508 insertions(+), 282 deletions(-) delete mode 100644 src/main/java/org/trigon/hardware/misc/FiducialLimelight.java diff --git a/src/main/java/org/trigon/hardware/misc/FiducialLimelight.java b/src/main/java/org/trigon/hardware/misc/FiducialLimelight.java deleted file mode 100644 index 69465d32..00000000 --- a/src/main/java/org/trigon/hardware/misc/FiducialLimelight.java +++ /dev/null @@ -1,244 +0,0 @@ -package org.trigon.hardware.misc; - -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation3d; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; -import org.littletonrobotics.junction.networktables.LoggedDashboardString; -import org.trigon.utilities.JsonHandler; - -@SuppressWarnings("unused") -public class FiducialLimelight { - private final String hostname; - private final LoggedDashboardNumber tv, pipeline, ledMode, driverCam, snapshot; - private final LoggedDashboardString json; - - /** - * Constructs a new Limelight. - * - * @param hostname the name of the Limelight - */ - public FiducialLimelight(String hostname) { - this.hostname = hostname; - - tv = new LoggedDashboardNumber(hostname + "/tv"); - json = new LoggedDashboardString(hostname + "/json"); - pipeline = new LoggedDashboardNumber(hostname + "/pipeline"); - ledMode = new LoggedDashboardNumber(hostname + "/ledMode"); - driverCam = new LoggedDashboardNumber(hostname + "/camMode"); - snapshot = new LoggedDashboardNumber(hostname + "/snapshot"); - } - - /** - * @return the last result's timestamp - */ - public double getLastResultTimestamp() { - return getJsonDump().Results.ts; - } - - /** - * @return true if the limelight has any visible targets, false otherwise - */ - public boolean hasResults() { - return tv.get() > 0; - } - - /** - * Gets the ty value of the target with the given id, from the json dump. - * - * @param id the target april tag's id - * @return the vertical offset from the crosshair to the target (-20.5 degrees to 20.5 degrees) - */ - public double getTy(int id) { - final LimelightJsonDump.Results.Fiducial fiducial = getFiducialFromId(id); - - return fiducial == null ? 0 : fiducial.ty; - } - - /** - * Gets the tx value of the target with the given id, from the json dump. - * - * @param id the target april tag's id - * @return the horizontal offset from the crosshair to the target (-27 degrees to 27 degrees) - */ - public double getTx(int id) { - final LimelightJsonDump.Results.Fiducial fiducial = getFiducialFromId(id); - - return fiducial == null ? 0 : fiducial.tx; - } - - /** - * Gets the ta value of the target with the given id, from the json dump. - * - * @param id the target april tag's id - * @return target's area (from 0% of the image to 100% of the image) - */ - public double getTa(int id) { - final LimelightJsonDump.Results.Fiducial fiducial = getFiducialFromId(id); - - return fiducial == null ? 0 : fiducial.ta; - } - - /** - * @return true if the driver cam is used, false if the vision cam is used - */ - public boolean isDriverCam() { - return driverCam.get() == 1; - } - - /** - * Sets the camera mode. - * - * @param useDriverCam true for driver camera, false for vision processing - */ - public void setCam(boolean useDriverCam) { - driverCam.set(useDriverCam ? 1 : 0); - } - - /** - * @return the current LedMode - */ - public LedMode getLedMode() { - return LedMode.getLedModeFromValue((int) ledMode.get()); - } - - /** - * Sets the led mode. - * - * @param mode the wanted LedMode - */ - public void setLedMode(LedMode mode) { - ledMode.set(mode.index); - } - - /** - * @return the current pipeline (0-9) - */ - public double getPipeline() { - return pipeline.get(); - } - - /** - * Sets the pipeline. - * - * @param pipeline (0-9) - */ - public void setPipeline(int pipeline) { - this.pipeline.set(pipeline); - } - - /** - * Takes a snapshot (To test the vision pipelines on stored snapshots). - */ - public void takeSnapshot() { - snapshot.set(1); - } - - /** - * @return the robot's pose, as reported by the Limelight - */ - public Pose3d getRobotPoseFromJsonDump() { - final double[] robotPoseArray = getJsonDump().Results.botpose_wpiblue; - if (robotPoseArray.length != 6) - return null; - - return robotPoseArrayToPose3d(robotPoseArray); - } - - /** - * @return the name of the Limelight - */ - public String getName() { - return hostname; - } - - /** - * @return the json dump of the Limelight - */ - public LimelightJsonDump getJsonDump() { - final String jsonString = json.get(); - if (jsonString.isEmpty()) - return new LimelightJsonDump(); - - return JsonHandler.parseJsonStringToObject( - jsonString, - LimelightJsonDump.class - ); - } - - private Pose3d robotPoseArrayToPose3d(double[] robotPoseArray) { - final Translation3d robotTranslation = new Translation3d( - robotPoseArray[0], - robotPoseArray[1], - robotPoseArray[2] - ); - final Rotation3d robotRotation = new Rotation3d( - robotPoseArray[3], - robotPoseArray[4], - robotPoseArray[5] - ); - - return new Pose3d(robotTranslation, robotRotation); - } - - private LimelightJsonDump.Results.Fiducial getFiducialFromId(int id) { - final LimelightJsonDump.Results results = getJsonDump().Results; - if (results.Fiducial == null) - return null; - - return results.getFiducialFromId(id); - } - - public enum LedMode { - USE_LED_MODE(0), - FORCE_OFF(1), - FORCE_BLINK(2), - FORCE_ON(3); - - public final int index; - - LedMode(int index) { - this.index = index; - } - - /** - * Returns what LedMode has the given index. - * - * @param index the index of the LedMode - * @return the LedMode with the given index. (If there is no LedMode with that index, returns null) - */ - public static LedMode getLedModeFromValue(int index) { - return values()[index]; - } - } - - private static class LimelightJsonDump { - private final Results Results = new Results(); - - private static class Results { - private double[] Classifier; - private double[] Detector; - private Fiducial[] Fiducial; - private double[] retro; - private double[] botpose, botpose_wpiblue, botpose_wpired; - private double pID, tl, ts, v; - - private Fiducial getFiducialFromId(int id) { - for (Fiducial fiducial : Fiducial) { - if (fiducial.fID == id) - return fiducial; - } - return null; - } - - private static class Fiducial { - private int fID; - private String fam; - private double[] pts; - private double[] skew; - private double[] t6c_ts, t6r_fs, t6r_ts, t6t_cs, t6t_rs; - private double ta, tx, txp, ty, typ; - } - } - } -} diff --git a/src/main/java/org/trigon/utilities/LimelightHelpers.java b/src/main/java/org/trigon/utilities/LimelightHelpers.java index 6f63a335..c1654207 100644 --- a/src/main/java/org/trigon/utilities/LimelightHelpers.java +++ b/src/main/java/org/trigon/utilities/LimelightHelpers.java @@ -1,4 +1,4 @@ -//LimelightHelpers v1.2.1 (March 1, 2023) +//LimelightHelpers v1.9 (REQUIRES 2024.9.1) package org.trigon.utilities; @@ -10,34 +10,37 @@ import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.*; 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 java.util.concurrent.ConcurrentHashMap; public class LimelightHelpers { + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + public static class LimelightTarget_Retro { @JsonProperty("t6c_ts") - private final double[] cameraPose_TargetSpace; + private double[] cameraPose_TargetSpace; @JsonProperty("t6r_fs") - private final double[] robotPose_FieldSpace; + private double[] robotPose_FieldSpace; @JsonProperty("t6r_ts") - private final double[] robotPose_TargetSpace; + private double[] robotPose_TargetSpace; @JsonProperty("t6t_cs") - private final double[] targetPose_CameraSpace; + private double[] targetPose_CameraSpace; @JsonProperty("t6t_rs") - private final double[] targetPose_RobotSpace; + private double[] targetPose_RobotSpace; public Pose3d getCameraPose_TargetSpace() { return toPose3D(cameraPose_TargetSpace); @@ -116,19 +119,19 @@ public static class LimelightTarget_Fiducial { public String fiducialFamily; @JsonProperty("t6c_ts") - private final double[] cameraPose_TargetSpace; + private double[] cameraPose_TargetSpace; @JsonProperty("t6r_fs") - private final double[] robotPose_FieldSpace; + private double[] robotPose_FieldSpace; @JsonProperty("t6r_ts") - private final double[] robotPose_TargetSpace; + private double[] robotPose_TargetSpace; @JsonProperty("t6t_cs") - private final double[] targetPose_CameraSpace; + private double[] targetPose_CameraSpace; @JsonProperty("t6t_rs") - private final double[] targetPose_RobotSpace; + private double[] targetPose_RobotSpace; public Pose3d getCameraPose_TargetSpace() { return toPose3D(cameraPose_TargetSpace); @@ -261,7 +264,9 @@ public LimelightTarget_Detector() { } } - public static class Results { + public static class LimelightResults { + + public String error; @JsonProperty("pID") public double pipelineID; @@ -293,6 +298,18 @@ public static class Results { @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; @@ -335,7 +352,7 @@ public Pose2d getBotPose2d_wpiBlue() { @JsonProperty("Barcode") public LimelightTarget_Barcode[] targets_Barcode; - public Results() { + public LimelightResults() { botpose = new double[6]; botpose_wpired = new double[6]; botpose_wpiblue = new double[6]; @@ -347,17 +364,106 @@ public Results() { targets_Barcode = new LimelightTarget_Barcode[0]; } + + } - public static class LimelightResults { - @JsonProperty("Results") - public Results targetingResults; + 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 LimelightResults() { - targetingResults = new Results(); + + 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; /** @@ -372,9 +478,9 @@ static final String sanitizeName(String name) { return name; } - private static Pose3d toPose3D(double[] inData) { + public static Pose3d toPose3D(double[] inData) { if (inData.length < 6) { - System.err.println("Bad LL 3D Pose Data!"); + //System.err.println("Bad LL 3D Pose Data!"); return new Pose3d(); } return new Pose3d( @@ -383,9 +489,9 @@ private static Pose3d toPose3D(double[] inData) { Units.degreesToRadians(inData[5]))); } - private static Pose2d toPose2D(double[] inData) { + public static Pose2d toPose2D(double[] inData) { if (inData.length < 6) { - System.err.println("Bad LL 2D Pose Data!"); + //System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -393,14 +499,206 @@ private static Pose2d toPose2D(double[] inData) { 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); } @@ -417,10 +715,16 @@ public static double[] getLimelightNTDoubleArray(String tableName, String entryN 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; @@ -447,6 +751,44 @@ 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"); } @@ -459,6 +801,10 @@ 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"); } @@ -532,8 +878,12 @@ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); } ///// @@ -592,6 +942,28 @@ public static Pose2d getBotPose2d_wpiBlue(String 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) @@ -606,6 +978,28 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { } + /** + * 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) @@ -631,6 +1025,11 @@ 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. @@ -663,14 +1062,6 @@ public static void setStreamMode_PiPSecondary(String limelightName) { setLimelightNTDouble(limelightName, "stream", 2); } - public static void setCameraMode_Processor(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 0); - } - - public static void setCameraMode_Driver(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 1); - } - /** * Sets the crop window. The crop window in the UI must be completely open for @@ -685,6 +1076,85 @@ public static void setCropWindow(String limelightName, double cropXMin, double c 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; @@ -746,7 +1216,7 @@ private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) public static LimelightResults getLatestResults(String limelightName) { long start = System.nanoTime(); - LimelightResults results = new LimelightResults(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); if (mapper == null) { mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); } @@ -754,12 +1224,12 @@ public static LimelightResults getLatestResults(String limelightName) { try { results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); + results.error = "lljson error: " + e.getMessage(); } long end = System.nanoTime(); double millis = (end - start) * .000001; - results.targetingResults.latency_jsonParse = millis; + results.latency_jsonParse = millis; if (profileJSON) { System.out.printf("lljson: %.2f\r\n", millis); }