From 93b497eeafbaf930607ba03cd979fc76502d96af Mon Sep 17 00:00:00 2001 From: Ishan <98932677+Ishan1522@users.noreply.github.com> Date: Tue, 25 Feb 2025 12:13:32 -0500 Subject: [PATCH 1/2] records --- TigerHelpers.java | 55 ++++++++++++++++++++++++----------------------- 1 file changed, 28 insertions(+), 27 deletions(-) diff --git a/TigerHelpers.java b/TigerHelpers.java index 89a550c..194970f 100644 --- a/TigerHelpers.java +++ b/TigerHelpers.java @@ -31,7 +31,7 @@ public class TigerHelpers { private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); /** Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. */ - public static class RawFiducial { + public record RawFiducial { /** The id of the april tag */ public int id = 0; @@ -120,7 +120,7 @@ public boolean equals(Object obj) { * Represents a Limelight pose estimate from Limelight's NetworkTables output. This is for any * botpose estimate, so MegaTag1 or MegaTag2 */ - public static class PoseEstimate { + public record PoseEstimate { /** The estimated 2D pose of the robot, including position and rotation. */ public Pose2d pose; @@ -153,15 +153,16 @@ public static class PoseEstimate { /** Initializes an "empty" 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[] {}; - this.isMegaTag2 = false; + this(new Pose2d(), 0, 0, 0, 0, 0, 0, new RawFiducial[0], false); + } + + /** + * Returns an empty PoseEstimate. + * + * @return a PoseEstimate with default values + */ + public static PoseEstimate empty() { + return new PoseEstimate(); } /** @@ -1027,33 +1028,33 @@ public static void setBotPoseEstimate( // Because network tables don't support 2D arrays, we need to flatten the data into a 1D array // Calculates array size: 11 (for PoseEstimate data) + 7 * number of fiducials (for each raw // fiducial) - int fiducialCount = poseEstimate.rawFiducials.length; + int fiducialCount = poseEstimate.rawFiducials().length; double[] data = new double[11 + 7 * fiducialCount]; // Populates the PoseEstimate, matching unpackBotPoseEstimate - data[0] = poseEstimate.pose.getX(); // x - data[1] = poseEstimate.pose.getY(); // y + data[0] = poseEstimate.pose().getX(); // x + data[1] = poseEstimate.pose().getY(); // y data[2] = 0.0; // z (Pose2d doesn't use this) data[3] = 0.0; // roll (not used) data[4] = 0.0; // pitch (not used) - data[5] = poseEstimate.pose.getRotation().getDegrees(); // yaw - data[6] = poseEstimate.latency; // latency + data[5] = poseEstimate.pose().getRotation().getDegrees(); // yaw + data[6] = poseEstimate.latency(); // latency data[7] = fiducialCount; // tagCount (must match fiducials length) - data[8] = poseEstimate.tagSpan; // tagSpan - data[9] = poseEstimate.avgTagDist; // avgTagDist - data[10] = poseEstimate.avgTagArea; // avgTagArea + data[8] = poseEstimate.tagSpan(); // tagSpan + data[9] = poseEstimate.avgTagDist(); // avgTagDist + data[10] = poseEstimate.avgTagArea(); // avgTagArea // Add data for each fiducial for (int i = 0; i < fiducialCount; i++) { int baseIndex = 11 + (i * 7); - RawFiducial fid = poseEstimate.rawFiducials[i]; - data[baseIndex] = fid.id; // id (cast to double) - data[baseIndex + 1] = fid.txnc; // txnc - data[baseIndex + 2] = fid.tync; // tync - data[baseIndex + 3] = fid.ta; // ta - data[baseIndex + 4] = fid.distToCamera; // distToCamera - data[baseIndex + 5] = fid.distToRobot; // distToRobot - data[baseIndex + 6] = fid.ambiguity; // ambiguity + RawFiducial fid = poseEstimate.rawFiducials()[i]; + data[baseIndex] = fid.id(); // id (cast to double) + data[baseIndex + 1] = fid.txnc(); // txnc + data[baseIndex + 2] = fid.tync(); // tync + data[baseIndex + 3] = fid.ta(); // ta + data[baseIndex + 4] = fid.distToCamera(); // distToCamera + data[baseIndex + 5] = fid.distToRobot(); // distToRobot + data[baseIndex + 6] = fid.ambiguity(); // ambiguity } // Write the array to NetworkTables From 47b22e8cbede12a498e4ed6c1192fbd33ce0d123 Mon Sep 17 00:00:00 2001 From: Ishan Date: Tue, 25 Feb 2025 12:51:04 -0500 Subject: [PATCH 2/2] bruh --- TigerHelpers.java | 45 ++++++++++++--------------------------------- 1 file changed, 12 insertions(+), 33 deletions(-) diff --git a/TigerHelpers.java b/TigerHelpers.java index 194970f..f068927 100644 --- a/TigerHelpers.java +++ b/TigerHelpers.java @@ -30,43 +30,22 @@ public class TigerHelpers { private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); - /** Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. */ - public record RawFiducial { - /** The id of the april tag */ - public int id = 0; - - /** - * The horizontal offset of the target from the Limelight's principle pixel (the images central - * pixel) in degrees. Positive values mean the target is to the right of the crosshair. - */ - public double txnc = 0; - - /** - * The vertical offset of the target from the Limelight's principle pixel (the images central - * pixel) in degrees. Positive values mean the target is below the crosshair. - */ - public double tync = 0; - - /** - * The area of the target in the Limelight's view as a percentage of the total image area. So, - * if the target takes up 10% of the image, this value will be 0.1. - */ - public double ta = 0; - - /** The distance from the Limelight to the april tag in meters. */ - public double distToCamera = 0; - - /** The distance from the robot to the april tag in meters. */ - public double distToRobot = 0; - - /** - * The ambiguity of the april tag. This ranges from 0 to 1, a lower values meaning less + /** Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + * + * @param id The id of the april tag + * @param txnc The horizontal offset of the target from the Limelight's principle pixel (the images central pixel) in degrees. + * @param tync The vertical offset of the target from the Limelight's principle pixel (the images central pixel) in degrees. + * @param ta The area of the target in the Limelight's view as a percentage of the total image area. + * @param distToCamera The distance from the Limelight to the april tag in meters. + * @param distToRobot The distance from the robot to the april tag in meters. + * @param ambiguity The ambiguity of the april tag. This ranges from 0 to 1, a lower values meaning less * ambiguous. The higher this is, the more likely it is that you will see ambiguity flipping. * Ambiguity flipping is when there are multiple possible "solutions" for the pose estimate, so * if this is higher your estimate will be less reliable. The best way to reduce this it to make * the april tag look as trapezoidal as possible from the Limelight's perspective. - */ - public double ambiguity = 0; + */ + public record RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + /** * Initializes a RawFidicual with the provided values. This represents data from reading a