From 6b64441fba7ee847029db058edabd60f2f948117 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sat, 5 Jul 2025 21:38:50 -0700 Subject: [PATCH 1/2] Make Spotless config for 'limelight' the same as for 'lib' --- limelight/build.gradle | 7 +++++-- limelight/greclipse-gradle.properties | 1 + 2 files changed, 6 insertions(+), 2 deletions(-) create mode 100644 limelight/greclipse-gradle.properties diff --git a/limelight/build.gradle b/limelight/build.gradle index 3581b7fa..6dd74254 100644 --- a/limelight/build.gradle +++ b/limelight/build.gradle @@ -59,13 +59,16 @@ spotless { indentWithSpaces() endWithNewline() } + groovyGradle { + greclipse().configFile('greclipse-gradle.properties') + } java { - // importOrder() // Use the default importOrder configuration (can override) + importOrder() // Use the default importOrder configuration (can override) removeUnusedImports() // Use Google Java Format // - aosp() causes it to use a four-space indent - // googleJavaFormat('1.24.0').reflowLongStrings() + googleJavaFormat('1.24.0').reflowLongStrings() formatAnnotations() // fix formatting of type annotations } diff --git a/limelight/greclipse-gradle.properties b/limelight/greclipse-gradle.properties new file mode 100644 index 00000000..9913ef41 --- /dev/null +++ b/limelight/greclipse-gradle.properties @@ -0,0 +1 @@ +org.eclipse.jdt.core.formatter.tabulation.char=space From a4d4065808f069fc266c666fe1d65f0dad67ab30 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sat, 5 Jul 2025 22:01:07 -0700 Subject: [PATCH 2/2] Run './gradlew :limelight:spotlessApply' --- limelight/build.gradle | 88 +- .../limelight/AprilTagMapPoseHelper.java | 27 +- .../lib2813/limelight/BotPoseEstimate.java | 1 - .../lib2813/limelight/DataCollection.java | 98 +- .../lib2813/limelight/JSONHelper.java | 125 +- .../team2813/lib2813/limelight/Limelight.java | 126 +- .../lib2813/limelight/LimelightClient.java | 3 +- .../lib2813/limelight/LimelightHelpers.java | 3094 +++++++++-------- .../lib2813/limelight/LocationalData.java | 176 +- .../limelight/NetworkTablesLimelight.java | 37 +- .../team2813/lib2813/limelight/Optionals.java | 2 +- .../lib2813/limelight/RestLimelight.java | 533 +-- .../lib2813/limelight/StubLocationalData.java | 1 - .../limelight/apriltag_map/Fiducial.java | 2 + .../apriltag_map/FiducialRetriever.java | 10 +- .../lib2813/limelight/FakeLimelight.java | 148 +- .../lib2813/limelight/LimelightTestCase.java | 526 ++- .../limelight/NetworkTablesLimelightTest.java | 20 +- .../lib2813/limelight/RestLimelightTest.java | 129 +- .../limelight/truth/Pose2dSubject.java | 85 +- .../limelight/truth/Pose3dSubject.java | 108 +- .../limelight/truth/Rotation2dSubject.java | 7 +- .../limelight/truth/Rotation3dSubject.java | 19 +- .../limelight/truth/TolerantComparison.java | 6 +- .../limelight/truth/Translation2dSubject.java | 7 +- .../limelight/truth/Translation3dSubject.java | 7 +- 26 files changed, 2727 insertions(+), 2658 deletions(-) diff --git a/limelight/build.gradle b/limelight/build.gradle index 6dd74254..3ec00d4c 100644 --- a/limelight/build.gradle +++ b/limelight/build.gradle @@ -1,75 +1,75 @@ plugins { - id 'java-library' - id "edu.wpi.first.GradleRIO" version "2025.1.1" - id 'idea' - // Spotless code formatter. - id 'com.diffplug.spotless' version '6.25.0' + id 'java-library' + id "edu.wpi.first.GradleRIO" version "2025.1.1" + id 'idea' + // Spotless code formatter. + id 'com.diffplug.spotless' version '6.25.0' } idea { - module { - downloadJavadoc = true - downloadSources = true - } + module { + downloadJavadoc = true + downloadSources = true + } } java { - sourceCompatibility = JavaVersion.VERSION_17 + sourceCompatibility = JavaVersion.VERSION_17 targetCompatibility = JavaVersion.VERSION_17 } repositories { - mavenCentral() + mavenCentral() } dependencies { - implementation wpi.java.deps.wpilib() - implementation wpi.java.vendor.java() + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() - api "org.json:json:20240205" + api "org.json:json:20240205" - testImplementation 'junit:junit:4.13.2' - testImplementation 'com.google.truth:truth:1.4.4' - implementation 'com.google.code.gson:gson:2.12.1' + testImplementation 'junit:junit:4.13.2' + testImplementation 'com.google.truth:truth:1.4.4' + implementation 'com.google.code.gson:gson:2.12.1' - nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) - nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) - simulationDebug wpi.sim.enableDebug() + nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) + nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) + simulationDebug wpi.sim.enableDebug() nativeRelease wpi.java.deps.wpilibJniRelease(wpi.platforms.desktop) - nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) - simulationRelease wpi.sim.enableRelease() + nativeRelease wpi.java.vendor.jniRelease(wpi.platforms.desktop) + simulationRelease wpi.sim.enableRelease() } wpi.java.configureTestTasks(test) tasks.named('test') { - // Use JUnit 4 for tests - useJUnit() + // Use JUnit 4 for tests + useJUnit() } spotless { - // See https://github.com/diffplug/spotless/tree/main/plugin-gradle - format 'misc', { - // define the files to apply `misc` to - target '.gitattributes', '.gitignore' + // See https://github.com/diffplug/spotless/tree/main/plugin-gradle + format 'misc', { + // define the files to apply `misc` to + target '.gitattributes', '.gitignore' - // define the steps to apply to those files - trimTrailingWhitespace() - indentWithSpaces() - endWithNewline() - } - groovyGradle { - greclipse().configFile('greclipse-gradle.properties') - } - java { - importOrder() // Use the default importOrder configuration (can override) - removeUnusedImports() + // define the steps to apply to those files + trimTrailingWhitespace() + indentWithSpaces() + endWithNewline() + } + groovyGradle { + greclipse().configFile('greclipse-gradle.properties') + } + java { + importOrder() // Use the default importOrder configuration (can override) + removeUnusedImports() - // Use Google Java Format - // - aosp() causes it to use a four-space indent - googleJavaFormat('1.24.0').reflowLongStrings() + // Use Google Java Format + // - aosp() causes it to use a four-space indent + googleJavaFormat('1.24.0').reflowLongStrings() - formatAnnotations() // fix formatting of type annotations - } + formatAnnotations() // fix formatting of type annotations + } } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/AprilTagMapPoseHelper.java b/limelight/src/main/java/com/team2813/lib2813/limelight/AprilTagMapPoseHelper.java index 83867540..7b82a2e2 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/AprilTagMapPoseHelper.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/AprilTagMapPoseHelper.java @@ -3,7 +3,6 @@ import com.team2813.lib2813.limelight.apriltag_map.Fiducial; import com.team2813.lib2813.limelight.apriltag_map.FiducialRetriever; import edu.wpi.first.math.geometry.Pose3d; - import java.io.ByteArrayInputStream; import java.io.IOException; import java.io.InputStream; @@ -19,13 +18,16 @@ class AprilTagMapPoseHelper { private final LimelightClient limelightClient; private FiducialRetriever retriever; - private static final HttpClient client = HttpClient.newBuilder().connectTimeout(Duration.ofMillis(20)) - .executor(Executors.newFixedThreadPool(1)).build(); - + private static final HttpClient client = + HttpClient.newBuilder() + .connectTimeout(Duration.ofMillis(20)) + .executor(Executors.newFixedThreadPool(1)) + .build(); + public AprilTagMapPoseHelper(LimelightClient client) { this.limelightClient = client; } - + public void setFieldMap(InputStream stream, boolean updateLimelight) throws IOException { if (!updateLimelight) { retriever = new FiducialRetriever(stream); @@ -34,12 +36,15 @@ public void setFieldMap(InputStream stream, boolean updateLimelight) throws IOEx retriever = new FiducialRetriever(new ByteArrayInputStream(bytes)); HttpRequest.BodyPublisher publisher = HttpRequest.BodyPublishers.ofByteArray(bytes); - HttpRequest request = limelightClient.newRequestBuilder("/upload-fieldmap").POST(publisher).build(); + HttpRequest request = + limelightClient.newRequestBuilder("/upload-fieldmap").POST(publisher).build(); try { client.send(request, HttpResponse.BodyHandlers.discarding()); } catch (InterruptedException e) { Thread.currentThread().interrupt(); - throw new RuntimeException(String.format("Thread interrupted while trying to send request to %s", request.uri()), e); + throw new RuntimeException( + String.format("Thread interrupted while trying to send request to %s", request.uri()), + e); } } } @@ -49,15 +54,15 @@ public List getVisibleTagPoses(Set ids) { return List.of(); } return retriever.getFiducialMap().values().stream() - .filter(fidicual -> ids.contains(fidicual.getId())) - .map(Fiducial::getPosition).toList(); + .filter(fidicual -> ids.contains(fidicual.getId())) + .map(Fiducial::getPosition) + .toList(); } public Optional getTagPose(int id) { if (retriever == null) { return Optional.empty(); } - return Optional.ofNullable(retriever.getFiducialMap().get(id)) - .map(Fiducial::getPosition); + return Optional.ofNullable(retriever.getFiducialMap().get(id)).map(Fiducial::getPosition); } } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/BotPoseEstimate.java b/limelight/src/main/java/com/team2813/lib2813/limelight/BotPoseEstimate.java index 706c0bad..675a71d9 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/BotPoseEstimate.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/BotPoseEstimate.java @@ -1,7 +1,6 @@ package com.team2813.lib2813.limelight; import edu.wpi.first.math.geometry.Pose2d; - import java.util.Set; /** diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/DataCollection.java b/limelight/src/main/java/com/team2813/lib2813/limelight/DataCollection.java index d04e0ff9..9b99332d 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/DataCollection.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/DataCollection.java @@ -1,5 +1,7 @@ package com.team2813.lib2813.limelight; +import static com.ctre.phoenix6.Utils.getCurrentTimeSeconds; + import java.net.http.HttpClient; import java.net.http.HttpRequest; import java.net.http.HttpResponse; @@ -12,62 +14,64 @@ import java.time.Duration; import java.util.Optional; import java.util.concurrent.Executors; - import org.json.JSONObject; -import static com.ctre.phoenix6.Utils.getCurrentTimeSeconds; - class DataCollection implements Runnable { - private static final HttpClient client = HttpClient.newBuilder().connectTimeout(Duration.ofMillis(20)) - .executor(Executors.newFixedThreadPool(2)).build(); - private final HttpRequest dumpRequest; + private static final HttpClient client = + HttpClient.newBuilder() + .connectTimeout(Duration.ofMillis(20)) + .executor(Executors.newFixedThreadPool(2)) + .build(); + private final HttpRequest dumpRequest; - public DataCollection(LimelightClient limelightClient) { - lastResult = Optional.empty(); - try { - dumpRequest = limelightClient.newRequestBuilder("/results").GET().build(); - } catch (LimelightClient.HttpRequestException e) { - throw new RuntimeException(e); - } - } + public DataCollection(LimelightClient limelightClient) { + lastResult = Optional.empty(); + try { + dumpRequest = limelightClient.newRequestBuilder("/results").GET().build(); + } catch (LimelightClient.HttpRequestException e) { + throw new RuntimeException(e); + } + } - record Result(JSONObject json, double responseTimestamp) {} + record Result(JSONObject json, double responseTimestamp) {} - private volatile Optional lastResult; + private volatile Optional lastResult; - private static class JSONHandler implements BodyHandler { - @Override - public BodySubscriber apply(ResponseInfo responseInfo) { - // Get the timestamp before we parse the JSON. - double responseTimestamp = getCurrentTimeSeconds(); + private static class JSONHandler implements BodyHandler { + @Override + public BodySubscriber apply(ResponseInfo responseInfo) { + // Get the timestamp before we parse the JSON. + double responseTimestamp = getCurrentTimeSeconds(); - return BodySubscribers.mapping(BodyHandlers.ofString(Charset.defaultCharset()).apply(responseInfo), body -> { - return new Result(new JSONObject(body), responseTimestamp); - }); - } - } + return BodySubscribers.mapping( + BodyHandlers.ofString(Charset.defaultCharset()).apply(responseInfo), + body -> { + return new Result(new JSONObject(body), responseTimestamp); + }); + } + } - private static final JSONHandler handler = new JSONHandler(); + private static final JSONHandler handler = new JSONHandler(); - private void updateJSON(HttpResponse obj) { - Result result = obj.body(); - lastResult = Optional.of(result); - } + private void updateJSON(HttpResponse obj) { + Result result = obj.body(); + lastResult = Optional.of(result); + } - @Override - public void run() { - try { - updateJSON(client.send(dumpRequest, handler)); - } catch (InterruptedException e) { - lastResult = Optional.empty(); - // background thread canceled - Thread.currentThread().interrupt(); - } catch (Exception e) { - lastResult = Optional.empty(); - } - } + @Override + public void run() { + try { + updateJSON(client.send(dumpRequest, handler)); + } catch (InterruptedException e) { + lastResult = Optional.empty(); + // background thread canceled + Thread.currentThread().interrupt(); + } catch (Exception e) { + lastResult = Optional.empty(); + } + } - public Optional getMostRecent() { - return lastResult; - } -} \ No newline at end of file + public Optional getMostRecent() { + return lastResult; + } +} diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/JSONHelper.java b/limelight/src/main/java/com/team2813/lib2813/limelight/JSONHelper.java index b33e5d89..1cb97cc6 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/JSONHelper.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/JSONHelper.java @@ -2,76 +2,75 @@ import java.util.Optional; import java.util.function.Function; - import org.json.JSONArray; import org.json.JSONException; import org.json.JSONObject; class JSONHelper { - static Optional getBooleanFromInt(JSONObject obj, String key) { - if (!obj.has(key)) { - return Optional.empty(); - } - try { - return Optional.of(obj.getInt(key) == 1); - } catch (JSONException e) { - return Optional.empty(); - } - } + static Optional getBooleanFromInt(JSONObject obj, String key) { + if (!obj.has(key)) { + return Optional.empty(); + } + try { + return Optional.of(obj.getInt(key) == 1); + } catch (JSONException e) { + return Optional.empty(); + } + } + + static Function> getJSONObject(String key) { + return (j) -> { + if (!j.has(key)) { + return Optional.empty(); + } + try { + return Optional.of(j.getJSONObject(key)); + } catch (JSONException e) { + return Optional.empty(); + } + }; + } - static Function> getJSONObject(String key) { - return (j) -> { - if (!j.has(key)) { - return Optional.empty(); - } - try { - return Optional.of(j.getJSONObject(key)); - } catch (JSONException e) { - return Optional.empty(); - } - }; - } + static JSONObject getRoot(JSONObject json) { + if (json.has("Results")) { + // This JSON was provided by an older version of the limelight code, + // which had a "Results" root node. + return json.getJSONObject("Results"); + } else { + return json; + } + } - static JSONObject getRoot(JSONObject json) { - if (json.has("Results")) { - // This JSON was provided by an older version of the limelight code, - // which had a "Results" root node. - return json.getJSONObject("Results"); - } else { - return json; - } - } + static Optional getLong(JSONObject obj, String key) { + if (!obj.has(key)) { + return Optional.empty(); + } + try { + return Optional.of(obj.getLong(key)); + } catch (JSONException e) { + return Optional.empty(); + } + } - static Optional getLong(JSONObject obj, String key) { - if (!obj.has(key)) { - return Optional.empty(); - } - try { - return Optional.of(obj.getLong(key)); - } catch (JSONException e) { - return Optional.empty(); - } - } + static Optional getDouble(JSONObject obj, String key) { + if (!obj.has(key)) { + return Optional.empty(); + } + try { + return Optional.of(obj.getDouble(key)); + } catch (JSONException e) { + return Optional.empty(); + } + } - static Optional getDouble(JSONObject obj, String key) { - if (!obj.has(key)) { - return Optional.empty(); - } - try { - return Optional.of(obj.getDouble(key)); - } catch (JSONException e) { - return Optional.empty(); - } - } - - static Optional getArr(JSONObject obj, String key) { - if (!obj.has(key)) { - return Optional.empty(); - } - try { - return Optional.of(obj.getJSONArray(key)); - } catch (JSONException e) { - return Optional.empty(); - } - } + static Optional getArr(JSONObject obj, String key) { + if (!obj.has(key)) { + return Optional.empty(); + } + try { + return Optional.of(obj.getJSONArray(key)); + } catch (JSONException e) { + return Optional.empty(); + } + } } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/Limelight.java b/limelight/src/main/java/com/team2813/lib2813/limelight/Limelight.java index 5dbbb0fd..7caae133 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/Limelight.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/Limelight.java @@ -2,79 +2,79 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.wpilibj.Filesystem; -import org.json.JSONObject; - import java.io.*; import java.util.List; import java.util.Optional; import java.util.OptionalDouble; import java.util.Set; +import org.json.JSONObject; public interface Limelight { - - /** - * Gets the limelight with the default name. - * @return the {@link Limelight} object for interfacing with the limelight - */ - static Limelight getDefaultLimelight() { - return RestLimelight.getDefaultLimelight(); - } - /** - * @deprecated use methods in {@link LocationalData} that return a - * {@link BotPoseEstimate}. - */ - @Deprecated - OptionalDouble getTimestamp(); + /** + * Gets the limelight with the default name. + * + * @return the {@link Limelight} object for interfacing with the limelight + */ + static Limelight getDefaultLimelight() { + return RestLimelight.getDefaultLimelight(); + } + + /** + * @deprecated use methods in {@link LocationalData} that return a {@link BotPoseEstimate}. + */ + @Deprecated + OptionalDouble getTimestamp(); + + /** + * Returns {@code true} if the limelight has identified a target. + * + * @deprecated use {@link LocationalData#hasTarget()} + */ + @Deprecated + boolean hasTarget(); + + /** Gets an object for getting locational data. */ + LocationalData getLocationalData(); + + void setFieldMap(InputStream stream, boolean updateLimelight) throws IOException; - /** - * Returns {@code true} if the limelight has identified a target. - * - * @deprecated use {@link LocationalData#hasTarget()} - */ - @Deprecated - boolean hasTarget(); + /** + * Sets the field map for the limelight with a file in the deploy directory. Additionally, this + * may also upload the field map to the Limelight if desired. This will likely be a slow + * operation, and should not be regularly called. + * + * @param filepath The path to the file from the deploy directory (using UNIX file seperators) + * @param updateLimelight If the limelight should be updated with this field map + * @throws IOException If the given filepath does not exist in the deploy directory or could not + * be read + */ + default void setFieldMap(String filepath, boolean updateLimelight) throws IOException { + File file = new File(Filesystem.getDeployDirectory(), filepath); + try (var stream = new FileInputStream(file)) { + setFieldMap(stream, updateLimelight); + } + } - /** Gets an object for getting locational data. */ - LocationalData getLocationalData(); - - void setFieldMap(InputStream stream, boolean updateLimelight) throws IOException; - - /** - * Sets the field map for the limelight with a file in the deploy directory. - * Additionally, this may also upload the field map to the Limelight if desired. - * This will likely be a slow operation, and should not be regularly called. - * @param filepath The path to the file from the deploy directory (using UNIX file seperators) - * @param updateLimelight If the limelight should be updated with this field map - * @throws IOException If the given filepath does not exist in the deploy directory or could not be read - */ - default void setFieldMap(String filepath, boolean updateLimelight) throws IOException { - File file = new File(Filesystem.getDeployDirectory(), filepath); - try (var stream = new FileInputStream(file)) { - setFieldMap(stream, updateLimelight); - } - } - - /** - * Gets the locations of the given AprilTags. - * - * @deprecated use {@link LocationalData#getVisibleAprilTagPoses()} - */ - @Deprecated - List getLocatedAprilTags(Set visibleTags); + /** + * Gets the locations of the given AprilTags. + * + * @deprecated use {@link LocationalData#getVisibleAprilTagPoses()} + */ + @Deprecated + List getLocatedAprilTags(Set visibleTags); - /** - * @deprecated use {@link LocationalData#getCaptureLatency()} - */ - @Deprecated - OptionalDouble getCaptureLatency(); + /** + * @deprecated use {@link LocationalData#getCaptureLatency()} + */ + @Deprecated + OptionalDouble getCaptureLatency(); - /** - * Gets the most recent JSON from the Limelight. - * Does not work for all implementations. - * - * @deprecated use {@link LocationalData#isValid()}. - */ - @Deprecated - Optional getJsonDump(); + /** + * Gets the most recent JSON from the Limelight. Does not work for all implementations. + * + * @deprecated use {@link LocationalData#isValid()}. + */ + @Deprecated + Optional getJsonDump(); } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightClient.java b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightClient.java index 5e7f01a2..aa83e253 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightClient.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightClient.java @@ -35,7 +35,8 @@ HttpRequest.Builder newRequestBuilder(String path) throws HttpRequestException { URI uri = new URI("http", null, hostname, port, path, null, null); return HttpRequest.newBuilder(uri); } catch (URISyntaxException e) { - throw new HttpRequestException(String.format("Could not create URI to %s:%d for '%s'", hostname, port, path), e); + throw new HttpRequestException( + String.format("Could not create URI to %s:%d for '%s'", hostname, port, path), e); } } } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java index 83543238..5b5901f6 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/LimelightHelpers.java @@ -1,1652 +1,1698 @@ -//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) +// LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) package com.team2813.lib2813.limelight; -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 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 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 edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +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 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; /** - * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. - * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. - * + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision + * cameras in FRC. This library supports all Limelight features including AprilTag tracking, Neural + * Networks, and standard color/retroreflective tracking. + * *

Coped from github.com/LimelightVision/limelightlib-wpijava

+ * >github.com/LimelightVision/limelightlib-wpijava */ class LimelightHelpers { - private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); - - /** - * Represents a Color/Retroreflective Target Result extracted from JSON Output - */ - 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("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @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]; - } - - } - - /** - * Represents an AprilTag/Fiducial Target Result extracted from JSON Output - */ - 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("ty") - public double ty; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; - - @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]; - } - } - - /** - * Represents a Barcode Target Result extracted from JSON Output - */ - public static class LimelightTarget_Barcode { - - /** - * Barcode family type (e.g. "QR", "DataMatrix", etc.) - */ - @JsonProperty("fam") - public String family; - - /** - * Gets the decoded data content of the barcode - */ - @JsonProperty("data") - public String data; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("tx_nocross") - public double tx_nocrosshair; - - @JsonProperty("ty_nocross") - public double ty_nocrosshair; + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); - @JsonProperty("ta") - public double ta; + /** Represents a Color/Retroreflective Target Result extracted from JSON Output */ + public static class LimelightTarget_Retro { - @JsonProperty("pts") - public double[][] corners; + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; - public LimelightTarget_Barcode() { - } + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; - public String getFamily() { - return family; - } - } - - /** - * Represents a Neural Classifier Pipeline Result extracted from JSON Output - */ - public static class LimelightTarget_Classifier { + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; - @JsonProperty("class") - public String className; + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; - @JsonProperty("classID") - public double classID; + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; - @JsonProperty("conf") - public double confidence; + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } - @JsonProperty("zone") - public double zone; + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } - @JsonProperty("tx") - public double tx; + public Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); + } - @JsonProperty("txp") - public double tx_pixels; + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); + } - @JsonProperty("ty") - public double ty; + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); + } - @JsonProperty("typ") - public double ty_pixels; + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); + } - public LimelightTarget_Classifier() { - } + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); } - /** - * Represents a Neural Detector Pipeline Result extracted from JSON Output - */ - public static class LimelightTarget_Detector { + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } - @JsonProperty("class") - public String className; + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); + } - @JsonProperty("classID") - public double classID; + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); + } - @JsonProperty("conf") - public double confidence; + @JsonProperty("ta") + public double ta; - @JsonProperty("ta") - public double ta; + @JsonProperty("tx") + public double tx; - @JsonProperty("tx") - public double tx; + @JsonProperty("ty") + public double ty; - @JsonProperty("ty") - public double ty; + @JsonProperty("txp") + public double tx_pixels; - @JsonProperty("txp") - public double tx_pixels; + @JsonProperty("typ") + public double ty_pixels; - @JsonProperty("typ") - public double ty_pixels; + @JsonProperty("tx_nocross") + public double tx_nocrosshair; - @JsonProperty("tx_nocross") - public double tx_nocrosshair; + @JsonProperty("ty_nocross") + public double ty_nocrosshair; - @JsonProperty("ty_nocross") - public double ty_nocrosshair; + @JsonProperty("ts") + public double ts; - public LimelightTarget_Detector() { - } + 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]; } + } - /** - * Limelight Results object, parsed from a Limelight's JSON results output. - */ - public static class LimelightResults { - - public String error; - - @JsonProperty("pID") - public double pipelineID; + /** Represents an AprilTag/Fiducial Target Result extracted from JSON Output */ + public static class LimelightTarget_Fiducial { - @JsonProperty("tl") - public double latency_pipeline; + @JsonProperty("fID") + public double fiducialID; - @JsonProperty("cl") - public double latency_capture; + @JsonProperty("fam") + public String fiducialFamily; - public double latency_jsonParse; + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; - @JsonProperty("v") - @JsonFormat(shape = Shape.NUMBER) - public boolean valid; + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; - @JsonProperty("botpose") - public double[] botpose; + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; + public Pose3d getCameraPose_TargetSpace() { + return toPose3D(cameraPose_TargetSpace); + } - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; + public Pose3d getRobotPose_FieldSpace() { + return toPose3D(robotPose_FieldSpace); + } - @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]; - - } - - - } - - /** - * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. - */ - 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; - } - } - - /** - * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. - */ - 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; - } - } - - /** - * Represents a 3D Pose Estimate. - */ - 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; - public boolean isMegaTag2; - - /** - * Instantiates 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[]{}; - this.isMegaTag2 = false; - } - - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, - int tagCount, double tagSpan, double avgTagDist, - double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { - - this.pose = pose; - this.timestampSeconds = timestampSeconds; - this.latency = latency; - this.tagCount = tagCount; - this.tagSpan = tagSpan; - this.avgTagDist = avgTagDist; - this.avgTagArea = avgTagArea; - this.rawFiducials = rawFiducials; - this.isMegaTag2 = isMegaTag2; - } - - } - - /** - * Encapsulates the state of an internal Limelight IMU. - */ - public static class IMUData { - public double robotYaw = 0.0; - public double Roll = 0.0; - public double Pitch = 0.0; - public double Yaw = 0.0; - public double gyroX = 0.0; - public double gyroY = 0.0; - public double gyroZ = 0.0; - public double accelX = 0.0; - public double accelY = 0.0; - public double accelZ = 0.0; - - public IMUData() {} - - public IMUData(double[] imuData) { - if (imuData != null && imuData.length >= 10) { - this.robotYaw = imuData[0]; - this.Roll = imuData[1]; - this.Pitch = imuData[2]; - this.Yaw = imuData[3]; - this.gyroX = imuData[4]; - this.gyroY = imuData[5]; - this.gyroZ = imuData[6]; - this.accelX = imuData[7]; - this.accelY = imuData[8]; - this.accelZ = imuData[9]; - } - } - } - - - 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 == null || name.isEmpty()) { - return "limelight"; - } - return name; - } - - /** - * Takes a 6-length array of pose data and converts it to a Pose3d object. - * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. - * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] - * @return Pose3d object representing the pose, or empty Pose3d if invalid data - */ - 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]))); - } - - /** - * Takes a 6-length array of pose data and converts it to a Pose2d object. - * Uses only x, y, and yaw components, ignoring z, roll, and pitch. - * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. - * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] - * @return Pose2d object representing the pose, or empty Pose2d if invalid data - */ - 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 in the format [x, y, z, roll, pitch, yaw]. - * Translation components are in meters, rotation components are in degrees. - * - * @param pose The Pose3d object to convert - * @return A 6-element array containing [x, y, z, roll, pitch, yaw] - */ - 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 in the format [x, y, z, roll, pitch, yaw]. - * Translation components are in meters, rotation components are in degrees. - * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. - * - * @param pose The Pose2d object to convert - * @return A 6-element array containing [x, y, 0, 0, 0, yaw] - */ - 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, boolean isMegaTag2) { - 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, isMegaTag2); - } - - /** - * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. - * - * @param limelightName Name/identifier of the Limelight - * @return Array of RawFiducial objects containing detection details - */ - public 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; - } - - /** - * Gets the latest raw neural detector results from NetworkTables - * - * @param limelightName Name/identifier of the Limelight - * @return Array of RawDetection objects containing detection details - */ - public static RawDetection[] getRawDetections(String limelightName) { - var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); - var rawDetectionArray = entry.getDoubleArray(new double[0]); - int valsPerEntry = 12; - 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; - } - - /** - * Prints detailed information about a PoseEstimate to standard output. - * Includes timestamp, latency, tag count, tag span, average tag distance, - * average tag area, and detailed information about each detected fiducial. - * - * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." - */ - 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.printf("Is MegaTag2: %b%n", pose.isMegaTag2); - 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 Boolean validPoseEstimate(PoseEstimate pose) { - return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; - } - - 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 Pose3d getRobotPose_TargetSpace() { + return toPose3D(robotPose_TargetSpace); } - - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + + public Pose3d getTargetPose_CameraSpace() { + return toPose3D(targetPose_CameraSpace); } - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); + public Pose3d getTargetPose_RobotSpace() { + return toPose3D(targetPose_RobotSpace); } - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + public Pose2d getCameraPose_TargetSpace2D() { + return toPose2D(cameraPose_TargetSpace); } - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + public Pose2d getRobotPose_FieldSpace2D() { + return toPose2D(robotPose_FieldSpace); } + public Pose2d getRobotPose_TargetSpace2D() { + return toPose2D(robotPose_TargetSpace); + } - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); + public Pose2d getTargetPose_CameraSpace2D() { + return toPose2D(targetPose_CameraSpace); } - public static String[] getLimelightNTStringArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + public Pose2d getTargetPose_RobotSpace2D() { + return toPose2D(targetPose_RobotSpace); } + @JsonProperty("ta") + public double ta; - 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; - } - ///// - ///// + @JsonProperty("tx") + public double tx; - /** - * Does the Limelight have a valid target? - * @param limelightName Name of the Limelight camera ("" for default) - * @return True if a valid target is present, false otherwise - */ - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); - } + @JsonProperty("ty") + public double ty; - /** - * Gets the horizontal offset from the crosshair to the target in degrees. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees - */ - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); - } + @JsonProperty("txp") + public double tx_pixels; - /** - * Gets the vertical offset from the crosshair to the target in degrees. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees - */ - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } + @JsonProperty("typ") + public double ty_pixels; - /** - * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Horizontal offset angle in degrees - */ - public static double getTXNC(String limelightName) { - return getLimelightNTDouble(limelightName, "txnc"); - } + @JsonProperty("tx_nocross") + public double tx_nocrosshair; - /** - * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. - * @param limelightName Name of the Limelight camera ("" for default) - * @return Vertical offset angle in degrees - */ - public static double getTYNC(String limelightName) { - return getLimelightNTDouble(limelightName, "tync"); - } + @JsonProperty("ty_nocross") + public double ty_nocrosshair; - /** - * Gets the target area as a percentage of the image (0-100%). - * @param limelightName Name of the Limelight camera ("" for default) - * @return Target area percentage (0-100) - */ - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } + @JsonProperty("ts") + public double ts; - /** - * T2D is an array that contains several targeting metrcis - * @param limelightName Name of the Limelight camera - * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, - * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] - */ - public static double[] getT2DArray(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "t2d"); - } - - /** - * Gets the number of targets currently detected. - * @param limelightName Name of the Limelight camera - * @return Number of detected targets - */ - public static int getTargetCount(String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[1]; - } - return 0; + 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]; } + } - /** - * Gets the classifier class index from the currently running neural classifier pipeline - * @param limelightName Name of the Limelight camera - * @return Class index from classifier pipeline - */ - public static int getClassifierClassIndex (String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[10]; - } - return 0; - } + /** Represents a Barcode Target Result extracted from JSON Output */ + public static class LimelightTarget_Barcode { - /** - * Gets the detector class index from the primary result of the currently running neural detector pipeline. - * @param limelightName Name of the Limelight camera - * @return Class index from detector pipeline - */ - public static int getDetectorClassIndex (String limelightName) { - double[] t2d = getT2DArray(limelightName); - if(t2d.length == 17) - { - return (int)t2d[11]; - } - return 0; - } + /** Barcode family type (e.g. "QR", "DataMatrix", etc.) */ + @JsonProperty("fam") + public String family; - /** - * Gets the current neural classifier result class name. - * @param limelightName Name of the Limelight camera - * @return Class name string from classifier pipeline - */ - public static String getClassifierClass (String limelightName) { - return getLimelightNTString(limelightName, "tcclass"); - } + /** Gets the decoded data content of the barcode */ + @JsonProperty("data") + public String data; - /** - * Gets the primary neural detector result class name. - * @param limelightName Name of the Limelight camera - * @return Class name string from detector pipeline - */ - public static String getDetectorClass (String limelightName) { - return getLimelightNTString(limelightName, "tdclass"); - } + @JsonProperty("txp") + public double tx_pixels; - /** - * Gets the pipeline's processing latency contribution. - * @param limelightName Name of the Limelight camera - * @return Pipeline latency in milliseconds - */ - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); - } + @JsonProperty("typ") + public double ty_pixels; - /** - * Gets the capture latency. - * @param limelightName Name of the Limelight camera - * @return Capture latency in milliseconds - */ - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "cl"); - } + @JsonProperty("tx") + public double tx; - /** - * Gets the active pipeline index. - * @param limelightName Name of the Limelight camera - * @return Current pipeline index (0-9) - */ - public static double getCurrentPipelineIndex(String limelightName) { - return getLimelightNTDouble(limelightName, "getpipe"); - } + @JsonProperty("ty") + public double ty; - /** - * Gets the current pipeline type. - * @param limelightName Name of the Limelight camera - * @return Pipeline type string (e.g. "retro", "apriltag", etc) - */ - public static String getCurrentPipelineType(String limelightName) { - return getLimelightNTString(limelightName, "getpipetype"); - } + @JsonProperty("tx_nocross") + public double tx_nocrosshair; - /** - * Gets the full JSON results dump. - * @param limelightName Name of the Limelight camera - * @return JSON string containing all current results - */ - public static String getJSONDump(String limelightName) { - return getLimelightNTString(limelightName, "json"); - } + @JsonProperty("ty_nocross") + public double ty_nocrosshair; - /** - * Switch to getBotPose - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } + @JsonProperty("ta") + public double ta; - /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } + @JsonProperty("pts") + public double[][] corners; - /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } + public LimelightTarget_Barcode() {} - public static double[] getBotPose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); + public String getFamily() { + return family; } + } - public static double[] getBotPose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } + /** Represents a Neural Classifier Pipeline Result extracted from JSON Output */ + public static class LimelightTarget_Classifier { - public static double[] getBotPose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } + @JsonProperty("class") + public String className; - public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } + @JsonProperty("classID") + public double classID; - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } + @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() {} + } + + /** Represents a Neural Detector Pipeline Result extracted from JSON Output */ + 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("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + public LimelightTarget_Detector() {} + } + + /** Limelight Results object, parsed from a Limelight's JSON results output. */ + public static class LimelightResults { + + public String error; - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + @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 static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); } - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); } - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); + public Pose2d getBotPose2d() { + return toPose2D(botpose); } - public static String getNeuralClassID(String limelightName) { - return getLimelightNTString(limelightName, "tclass"); + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); } - public static String[] getRawBarcodeData(String limelightName) { - return getLimelightNTStringArray(limelightName, "rawbarcodes"); + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); } - ///// - ///// + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - return toPose3D(poseArray); + @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]; + } + } + + /** Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. */ + 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; + } + } + + /** Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. */ + 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; + } + } + + /** Represents a 3D Pose Estimate. */ + 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; + public boolean isMegaTag2; + + /** Instantiates 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[] {}; + this.isMegaTag2 = false; + } + + public PoseEstimate( + Pose2d pose, + double timestampSeconds, + double latency, + int tagCount, + double tagSpan, + double avgTagDist, + double avgTagArea, + RawFiducial[] rawFiducials, + boolean isMegaTag2) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; + } + } + + /** Encapsulates the state of an internal Limelight IMU. */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } } - - /** - * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Red Alliance field space - */ - public static Pose3d getBotPose3d_wpiRed(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - return toPose3D(poseArray); + } + + 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 == null || name.isEmpty()) { + return "limelight"; + } + return name; + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. Array format: [x, y, z, + * roll, pitch, yaw] where angles are in degrees. + * + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + 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]))); + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. Uses only x, y, and yaw + * components, ignoring z, roll, and pitch. Array format: [x, y, z, roll, pitch, yaw] where angles + * are in degrees. + * + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + 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 in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + 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 in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. Note: z, roll, and + * pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + 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; } - - /** - * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space - */ - public static Pose3d getBotPose3d_wpiBlue(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - return toPose3D(poseArray); + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate( + String limelightName, String entryName, boolean isMegaTag2) { + 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); + } } - /** - * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the robot's position and orientation relative to the target - */ - public static Pose3d getBotPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - return toPose3D(poseArray); - } + return new PoseEstimate( + pose, + adjustedTimestamp, + latency, + tagCount, + tagSpan, + tagDist, + tagArea, + rawFiducials, + isMegaTag2); + } + + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public 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; + } + + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + 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; + } + + /** + * Prints detailed information about a PoseEstimate to standard output. Includes timestamp, + * latency, tag count, tag span, average tag distance, average tag area, and detailed information + * about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + 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.printf("Is MegaTag2: %b%n", pose.isMegaTag2); + 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 Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + + 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; + } + + ///// + ///// + + /** + * Does the Limelight have a valid target? + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the + * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable + * crosshair functionality. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the + * most accurate 2d metric if you are using a calibrated camera and you don't need adjustable + * crosshair functionality. + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + /** + * T2D is an array that contains several targeting metrcis + * + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, + * txnc, tync, ta, tid, targetClassIndexDetector, targetClassIndexClassifier, + * targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, + * targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector + * pipeline. + * + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if (t2d.length == 17) { + return (int) t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass(String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass(String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + /** + * Gets the capture latency. + * + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + /** + * Gets the active pipeline index. + * + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + /** + * Gets the current pipeline type. + * + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ + 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); + } + + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field + * space + */ + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field + * space + */ + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ + 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 MegaTag1 Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); + } + + /** + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) in the WPILib Blue alliance coordinate system. Make sure you are calling + * setRobotOrientation() before calling this method. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); + } + + /** + * 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); + } + + public static PoseEstimate getBotPoseEstimate(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose", false); + } + + /** + * 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", false); + } + + /** + * 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", true); + } + + /** + * 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); + } + + /** + * Gets the current IMU data from NetworkTables. IMU data is formatted as [robotYaw, Roll, Pitch, + * Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. Returns all zeros if data is invalid or + * unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * Sets LED mode to be controlled by the current pipeline. + * + * @param limelightName Name of the Limelight camera + */ + 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); + } + + /** + * Enables standard side-by-side stream mode. + * + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + /** + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) + */ + 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); + } + + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ + 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(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); + } + + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + 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); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. Tags not in this list will + * be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ + 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); + } + + /** + * Sets the downscaling factor for AprilTag detection. Increasing downscale can improve + * performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to + * 0 for pipeline control. + */ + 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); + } + + /** + * Sets the camera pose relative to the robot. + * + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ + 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.isEmpty()) { + connection.setRequestProperty("snapname", snapshotName); + } - /** - * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the target - */ - public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - return toPose3D(poseArray); + 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; + } - /** - * Gets the target's 3D pose with respect to the camera's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the camera - */ - public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - return toPose3D(poseArray); - } + /** + * Gets the latest JSON results output and returns a LimelightResults object. + * + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data + */ + public static LimelightResults getLatestResults(String limelightName) { - /** - * Gets the target's 3D pose with respect to the robot's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the target's position and orientation relative to the robot - */ - public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - return toPose3D(poseArray); + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = + new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); } - /** - * Gets the camera's 3D pose with respect to the robot's coordinate system. - * @param limelightName Name/identifier of the Limelight - * @return Pose3d object representing the camera's position and orientation relative to the robot - */ - 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 MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); - } - - /** - * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. - * Make sure you are calling setRobotOrientation() before calling this method. - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); - } - - /** - * 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); - - } - - public static PoseEstimate getBotPoseEstimate(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose", false); - } - - /** - * 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", false); + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); } - /** - * 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", true); - } - - /** - * 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); - - } - - /** - * Gets the current IMU data from NetworkTables. - * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. - * Returns all zeros if data is invalid or unavailable. - * - * @param limelightName Name/identifier of the Limelight - * @return IMUData object containing all current IMU data - */ - public static IMUData getIMUData(String limelightName) { - double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); - if (imuData == null || imuData.length < 10) { - return new IMUData(); // Returns object with all zeros - } - return new IMUData(imuData); - } - - ///// - ///// - - public static void setPipelineIndex(String limelightName, int pipelineIndex) { - setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); - } - - - public static void setPriorityTagID(String limelightName, int ID) { - setLimelightNTDouble(limelightName, "priorityid", ID); - } - - /** - * Sets LED mode to be controlled by the current pipeline. - * @param limelightName Name of the Limelight camera - */ - 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); - } - - /** - * Enables standard side-by-side stream mode. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); - } - - /** - * Enables Picture-in-Picture mode with secondary stream in the corner. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } - - /** - * Enables Picture-in-Picture mode with primary stream in the corner. - * @param limelightName Name of the Limelight camera - */ - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } - - - /** - * Sets the crop window for the camera. The crop window in the UI must be completely open. - * @param limelightName Name of the Limelight camera - * @param cropXMin Minimum X value (-1 to 1) - * @param cropXMax Maximum X value (-1 to 1) - * @param cropYMin Minimum Y value (-1 to 1) - * @param cropYMax Maximum Y value (-1 to 1) - */ - 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); - } - - /** - * Sets robot orientation values used by MegaTag2 localization algorithm. - * - * @param limelightName Name/identifier of the Limelight - * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC - * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second - * @param pitch (Unnecessary) Robot pitch in degrees - * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second - * @param roll (Unnecessary) Robot roll in degrees - * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second - */ - 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(); - } - } - - /** - * Configures the IMU mode for MegaTag2 Localization - * - * @param limelightName Name/identifier of the Limelight - * @param mode IMU mode. - */ - public static void SetIMUMode(String limelightName, int mode) { - setLimelightNTDouble(limelightName, "imumode_set", mode); - } - - /** - * Sets the 3D point-of-interest offset for the current fiducial pipeline. - * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking - * - * @param limelightName Name/identifier of the Limelight - * @param x X offset in meters - * @param y Y offset in meters - * @param z Z offset in meters - */ - 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); - } - - /** - * Overrides the valid AprilTag IDs that will be used for localization. - * Tags not in this list will be ignored for robot pose estimation. - * - * @param limelightName Name/identifier of the Limelight - * @param validIDs Array of valid AprilTag IDs to track - */ - 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); - } - - /** - * Sets the downscaling factor for AprilTag detection. - * Increasing downscale can improve performance at the cost of potentially reduced detection range. - * - * @param limelightName Name/identifier of the Limelight - * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. - */ - 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); - } - - /** - * Sets the camera pose relative to the robot. - * @param limelightName Name of the Limelight camera - * @param forward Forward offset in meters - * @param side Side offset in meters - * @param up Up offset in meters - * @param roll Roll angle in degrees - * @param pitch Pitch angle in degrees - * @param yaw Yaw angle in degrees - */ - 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); - }); + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); } - 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.isEmpty()) { - 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; - } - - /** - * Gets the latest JSON results output and returns a LimelightResults object. - * @param limelightName Name of the Limelight camera - * @return LimelightResults object containing all current target data - */ - 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; - } -} \ No newline at end of file + return results; + } +} diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/LocationalData.java b/limelight/src/main/java/com/team2813/lib2813/limelight/LocationalData.java index c3c3bb25..fa74f822 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/LocationalData.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/LocationalData.java @@ -1,104 +1,100 @@ package com.team2813.lib2813.limelight; +import edu.wpi.first.math.geometry.Pose3d; import java.util.Map; import java.util.Optional; import java.util.OptionalDouble; import java.util.Set; -import edu.wpi.first.math.geometry.Pose3d; - /** * Get positional data from limelight + * * @see Limelight */ public interface LocationalData { - /** Returns {@code true} if the limelight has identified a target. */ - boolean hasTarget(); - - /** Returns {@code true} if the limelight has provided a valid response. */ - boolean isValid(); - - /** - * Gets the position of the robot with the center of the field as the origin. - * @return The position of the robot - */ - Optional getBotpose(); - - /** - * Gets the estimated position of the robot with the center of the field as the origin. - */ - Optional getBotPoseEstimate(); - - /** - * Gets the position of the robot with the blue driverstation as the origin - * @return The position of the robot - */ - Optional getBotposeBlue(); - - /** - * Gets the estimated position of the robot with the blue driverstation as the origin. - */ - Optional getBotPoseEstimateBlue(); - - /** - * Gets the position of the robot with the red driverstation as the origin - * @return The position of the robot - */ - Optional getBotposeRed(); - - /** - * Gets the estimated position of the robot with the red driverstation as the origin. - */ - Optional getBotPoseEstimateRed(); - - /** - * Capture latency in milliseconds. - * - *

Per the Limelight docs, this is the time between the end of the exposure of the middle row - * to the beginning of the tracking loop. - * - * @deprecated Use {@link #getBotPoseEstimateBlue()} or {@link #getBotPoseEstimateRed()} - */ - @Deprecated - OptionalDouble getCaptureLatency(); - - /** - * Targeting latency in milliseconds. - * - *

Per the Limelight docs, this is the time consumed by the tracking loop this frame. - * - * @deprecated Use {@link #getBotPoseEstimateBlue()} or {@link #getBotPoseEstimateRed()} - */ - @Deprecated - OptionalDouble getTargetingLatency(); - - /** - * @deprecated use methods that return a {@link BotPoseEstimate}. - */ - @Deprecated - OptionalDouble getTimestamp(); - - @Deprecated - default OptionalDouble lastMSDelay(){ - OptionalDouble a = getCaptureLatency(); - OptionalDouble b = getTargetingLatency(); - if (a.isPresent() && b.isPresent()) { - return OptionalDouble.of(a.getAsDouble() + b.getAsDouble()); - } - return OptionalDouble.empty(); - } - - /** - * Gets the set of all visible tags - * @return The visible tags - * @deprecated use {@link #getVisibleAprilTagPoses()} - */ - @Deprecated - Set getVisibleTags(); - - /** - * Gets the visible AprilTags as a map from ID to position. - */ - Map getVisibleAprilTagPoses(); + /** Returns {@code true} if the limelight has identified a target. */ + boolean hasTarget(); + + /** Returns {@code true} if the limelight has provided a valid response. */ + boolean isValid(); + + /** + * Gets the position of the robot with the center of the field as the origin. + * + * @return The position of the robot + */ + Optional getBotpose(); + + /** Gets the estimated position of the robot with the center of the field as the origin. */ + Optional getBotPoseEstimate(); + + /** + * Gets the position of the robot with the blue driverstation as the origin + * + * @return The position of the robot + */ + Optional getBotposeBlue(); + + /** Gets the estimated position of the robot with the blue driverstation as the origin. */ + Optional getBotPoseEstimateBlue(); + + /** + * Gets the position of the robot with the red driverstation as the origin + * + * @return The position of the robot + */ + Optional getBotposeRed(); + + /** Gets the estimated position of the robot with the red driverstation as the origin. */ + Optional getBotPoseEstimateRed(); + + /** + * Capture latency in milliseconds. + * + *

Per the Limelight docs, this is the time between the end of the exposure of the middle row + * to the beginning of the tracking loop. + * + * @deprecated Use {@link #getBotPoseEstimateBlue()} or {@link #getBotPoseEstimateRed()} + */ + @Deprecated + OptionalDouble getCaptureLatency(); + + /** + * Targeting latency in milliseconds. + * + *

Per the Limelight docs, this is the time consumed by the tracking loop this frame. + * + * @deprecated Use {@link #getBotPoseEstimateBlue()} or {@link #getBotPoseEstimateRed()} + */ + @Deprecated + OptionalDouble getTargetingLatency(); + + /** + * @deprecated use methods that return a {@link BotPoseEstimate}. + */ + @Deprecated + OptionalDouble getTimestamp(); + + @Deprecated + default OptionalDouble lastMSDelay() { + OptionalDouble a = getCaptureLatency(); + OptionalDouble b = getTargetingLatency(); + if (a.isPresent() && b.isPresent()) { + return OptionalDouble.of(a.getAsDouble() + b.getAsDouble()); + } + return OptionalDouble.empty(); + } + + /** + * Gets the set of all visible tags + * + * @return The visible tags + * @deprecated use {@link #getVisibleAprilTagPoses()} + */ + @Deprecated + Set getVisibleTags(); + + /** Gets the visible AprilTags as a map from ID to position. */ + Map getVisibleAprilTagPoses(); } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/NetworkTablesLimelight.java b/limelight/src/main/java/com/team2813/lib2813/limelight/NetworkTablesLimelight.java index 88ec34a6..9579935a 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/NetworkTablesLimelight.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/NetworkTablesLimelight.java @@ -1,21 +1,20 @@ package com.team2813.lib2813.limelight; +import static java.util.Collections.unmodifiableMap; + +import com.team2813.lib2813.limelight.LimelightHelpers.LimelightResults; +import com.team2813.lib2813.limelight.LimelightHelpers.PoseEstimate; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import java.io.IOException; import java.io.InputStream; import java.util.*; -import java.util.stream.Collectors; import java.util.Arrays; import java.util.Optional; import java.util.OptionalDouble; - -import com.team2813.lib2813.limelight.LimelightHelpers.PoseEstimate; -import com.team2813.lib2813.limelight.LimelightHelpers.LimelightResults; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; +import java.util.stream.Collectors; import org.json.JSONObject; -import static java.util.Collections.unmodifiableMap; - class NetworkTablesLimelight implements Limelight { private static final double[] ZEROS = new double[6]; private final String limelightName; @@ -38,7 +37,8 @@ public boolean hasTarget() { @Override public void setFieldMap(InputStream stream, boolean updateLimelight) throws IOException { - // The updateLimelight assumes we have the hostname of the limelight, which we don't. For now, this won't update the limelight. + // The updateLimelight assumes we have the hostname of the limelight, which we don't. For now, + // this won't update the limelight. aprilTagMapPoseHelper.setFieldMap(stream, false); } @@ -76,16 +76,18 @@ public LocationalData getLocationalData() { return StubLocationalData.INVALID; } - private static Optional toBotPoseEstimate(PoseEstimate estimate, Set visibleAprilTags) { + private static Optional toBotPoseEstimate( + PoseEstimate estimate, Set visibleAprilTags) { if (estimate == null || estimate.tagCount == 0 || Pose2d.kZero.equals(estimate.pose)) { return Optional.empty(); } - return Optional.of(new BotPoseEstimate(estimate.pose, estimate.timestampSeconds, visibleAprilTags)); + return Optional.of( + new BotPoseEstimate(estimate.pose, estimate.timestampSeconds, visibleAprilTags)); } private Map getVisibleAprilTagPoses(LimelightResults results) { Map map = new HashMap<>(); - for (var fiducial: results.targets_Fiducials) { + for (var fiducial : results.targets_Fiducials) { int id = (int) fiducial.fiducialID; aprilTagMapPoseHelper.getTagPose(id).ifPresent(pose -> map.put(id, pose)); } @@ -100,8 +102,11 @@ private class NTLocationalData implements LocationalData { private final Map aprilTags; NTLocationalData( - LimelightResults results, Optional poseEstimate, Optional redPoseEstimate, - Optional bluePoseEstimate, Map aprilTags) { + LimelightResults results, + Optional poseEstimate, + Optional redPoseEstimate, + Optional bluePoseEstimate, + Map aprilTags) { this.results = results; this.poseEstimate = poseEstimate; this.redPoseEstimate = redPoseEstimate; @@ -166,7 +171,9 @@ public OptionalDouble getTimestamp() { @Override public Set getVisibleTags() { - return Arrays.stream(results.targets_Fiducials).map(fiducial -> (int) fiducial.fiducialID).collect(Collectors.toUnmodifiableSet()); + return Arrays.stream(results.targets_Fiducials) + .map(fiducial -> (int) fiducial.fiducialID) + .collect(Collectors.toUnmodifiableSet()); } @Override diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/Optionals.java b/limelight/src/main/java/com/team2813/lib2813/limelight/Optionals.java index 07708b12..1002184b 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/Optionals.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/Optionals.java @@ -13,7 +13,7 @@ private Optionals() { static OptionalLong unboxLong(Optional val) { return val.map(OptionalLong::of).orElseGet(OptionalLong::empty); } - + static OptionalDouble unboxDouble(Optional val) { return val.map(OptionalDouble::of).orElseGet(OptionalDouble::empty); } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/RestLimelight.java b/limelight/src/main/java/com/team2813/lib2813/limelight/RestLimelight.java index 517ff439..e104b672 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/RestLimelight.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/RestLimelight.java @@ -1,11 +1,13 @@ package com.team2813.lib2813.limelight; +import static com.team2813.lib2813.limelight.JSONHelper.*; +import static com.team2813.lib2813.limelight.Optionals.unboxDouble; +import static com.team2813.lib2813.limelight.Optionals.unboxLong; +import static java.util.Collections.*; + import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.wpilibj.DriverStation; -import org.json.JSONArray; -import org.json.JSONObject; - import java.io.IOException; import java.io.InputStream; import java.util.*; @@ -14,267 +16,270 @@ import java.util.concurrent.ScheduledFuture; import java.util.concurrent.TimeUnit; import java.util.function.Function; - -import static com.team2813.lib2813.limelight.JSONHelper.*; -import static com.team2813.lib2813.limelight.Optionals.unboxDouble; -import static com.team2813.lib2813.limelight.Optionals.unboxLong; -import static java.util.Collections.*; +import org.json.JSONArray; +import org.json.JSONObject; class RestLimelight implements Limelight { - private static final Map limelights = new HashMap<>(); - private final AprilTagMapPoseHelper aprilTagMapPoseHelper; - private final DataCollection collectionThread; - private static final ScheduledExecutorService executor = Executors.newScheduledThreadPool(2); - - static final String DEFAULT_ADDRESS = "limelight.local"; - - private ScheduledFuture thread; - - boolean started = false; - - RestLimelight(String address) { - var limelightClient = new LimelightClient(address); - collectionThread = new DataCollection(limelightClient); - aprilTagMapPoseHelper = new AprilTagMapPoseHelper(limelightClient); - } - - void start() { - if (!started) { - thread = executor.scheduleAtFixedRate(collectionThread, 20, 40, TimeUnit.MILLISECONDS); - started = true; - } - } - - void runThread() { - collectionThread.run(); - } - - @Override - public Optional getJsonDump() { - return collectionThread.getMostRecent().map(DataCollection.Result::json); - } - - public OptionalDouble getCaptureLatency() { - return getLocationalData().getCaptureLatency(); - } - - @Override - public OptionalDouble getTimestamp() { - return getLocationalData().getTimestamp(); - } - - /** - * Sets the field map for the limelight. Additionally, this may also upload the field map to the Limelight if desired. - * This will likely be a slow operation, and should not be regularly called. - * @param stream The reader which - * @param updateLimelight If the limelight should be updated with this field map - */ - @Override - public void setFieldMap(InputStream stream, boolean updateLimelight) throws IOException { - aprilTagMapPoseHelper.setFieldMap(stream, updateLimelight); - } - - @Override - public List getLocatedAprilTags(Set visibleTags) { - return aprilTagMapPoseHelper.getVisibleTagPoses(visibleTags); - } - - private static Function not(Function fnc) { - return (t) -> !fnc.apply(t); - } - - public boolean hasTarget() { - return getLocationalData().hasTarget(); - } - - public LocationalData getLocationalData() { - Optional locationalData = collectionThread.getMostRecent().map(RestLocationalData::new); - return locationalData.orElse(StubLocationalData.VALID); - } - - private void clean() { - try { - thread.cancel(true); - executor.awaitTermination(2, TimeUnit.SECONDS); - } catch (InterruptedException e) { - DriverStation.reportError(e.getMessage(), false); - } - } - - /** - * Gets the limelight with the default name. - * @return the {@link Limelight} object for interfacing with the limelight - */ - public static Limelight getDefaultLimelight() { - return getLimelight(DEFAULT_ADDRESS); - } - - /** - * Gets the limelight with the specified name. Calling with a blank {@code limelightName} - * is equivalent to calling {@link #getDefaultLimelight()} - * @param limelightAddress The hostname or ip address of the limelight - * @return the {@link Limelight} object for interfacing with the limelight - * @throws NullPointerException if {@code limelightName} is null - */ - public static Limelight getLimelight(String limelightAddress) { - String addr = Objects.requireNonNull(limelightAddress,"limelightAddress shouldn't be null"); - if (addr.isEmpty()) { - throw new IllegalArgumentException("limelightAddress shouldn't be empty"); - } - RestLimelight result = limelights.computeIfAbsent(addr, RestLimelight::new); - result.start(); - return result; - } - - static void eraseInstances() { - for (RestLimelight limelight : limelights.values()) { - limelight.clean(); - } - limelights.clear(); - } - - private class RestLocationalData implements LocationalData { - private final JSONObject root; - private final double responseTimestamp; - - RestLocationalData(DataCollection.Result result) { - this.root = getRoot(result.json()); - this.responseTimestamp = result.responseTimestamp(); - } - - @Override - public boolean isValid() { - return getBooleanFromInt(root, "v").orElse(false); - } - - @Override - public boolean hasTarget() { - return getArr(root, "Fiducial").map(not(JSONArray::isEmpty)).orElse(false); - } - - private boolean invalidArray(JSONArray arr) { - boolean simple = arr.length() != 6 || !hasTarget(); - if (simple) { - return true; - } - Integer intZero = 0; - Double doubleZero = 0.0; - for (Object o : arr) { - if (!intZero.equals(o) && !doubleZero.equals(o)) { - return false; - } - } - return true; - } - - private Optional parseArr(JSONArray arr) { - if (invalidArray(arr)) { - return Optional.empty(); - } - Rotation3d rotation = new Rotation3d( - Math.toRadians(arr.getDouble(3)), - Math.toRadians(arr.getDouble(4)), - Math.toRadians(arr.getDouble(5)) - ); - return Optional.of(new Pose3d(arr.getDouble(0), arr.getDouble(1), arr.getDouble(2), rotation)); - } - - @Override - public OptionalDouble getTimestamp() { - return unboxDouble(getDouble(root, "ts")); - } - - @Override - public OptionalDouble getCaptureLatency() { - return unboxDouble(getDouble(root, "cl")); - } - - @Override - public OptionalDouble getTargetingLatency() { - return unboxDouble(getDouble(root, "tl")); - } - - @Override - public Optional getBotpose() { - return getArr(root, "botpose").flatMap(this::parseArr); - } - - @Override - public Optional getBotPoseEstimate() { - return getArr(root, "botpose") - .flatMap(this::parseArr) - .map(this::toBotPoseEstimate); - } - - @Override - public Optional getBotposeBlue() { - return getArr(root, "botpose_wpiblue").flatMap(this::parseArr); - } - - @Override - public Optional getBotPoseEstimateBlue() { - return getArr(root, "botpose_wpiblue") - .flatMap(this::parseArr) - .map(this::toBotPoseEstimate); - } - - /** - * Gets the position of the robot with the red driverstation as the origin - * @return The position of the robot - */ - @Override - public Optional getBotposeRed() { - return getArr(root, "botpose_wpired").flatMap(this::parseArr); - } - - @Override - public Optional getBotPoseEstimateRed() { - return getArr(root, "botpose_wpired") - .flatMap(this::parseArr) - .map(this::toBotPoseEstimate); - } - - private BotPoseEstimate toBotPoseEstimate(Pose3d pose) { - // See https://www.chiefdelphi.com/t/timestamp-parameter-when-adding-limelight-vision-to-odometry - double latencyMillis = getCaptureLatency().orElse(0.0) + getTargetingLatency().orElse(0.0); - double timestampSeconds = responseTimestamp - (latencyMillis / 1000); - return new BotPoseEstimate(pose.toPose2d(), timestampSeconds, getVisibleAprilTagPoses().keySet()); - } - - /** - * Gets the id of the targeted tag. - */ - OptionalLong getTagID() { - return unboxLong(getLong(root, "pID")); - } - - @Override - public Set getVisibleTags() { - return getArr(root, "Fiducial").map(arr -> { - Set ints = new HashSet<>(); - for (int i = 0; i < arr.length(); i++) { - JSONObject obj = arr.optJSONObject(i); - if (obj != null && obj.has("fID")) { - ints.add(obj.getInt("fID")); - } - } - return unmodifiableSet(ints); - }).orElseGet(Set::of); - } - - @Override - public Map getVisibleAprilTagPoses() { - return getArr(root, "Fiducial").map(arr -> { - Map map = new HashMap<>(); - for (int i = 0; i < arr.length(); i++) { - JSONObject obj = arr.optJSONObject(i); - if (obj != null && obj.has("fID")) { - int id = obj.getInt("fID"); - aprilTagMapPoseHelper.getTagPose(id).ifPresent(pose -> map.put(id, pose)); - } - } - return unmodifiableMap(map); - }).orElse(emptyMap()); - } - } + private static final Map limelights = new HashMap<>(); + private final AprilTagMapPoseHelper aprilTagMapPoseHelper; + private final DataCollection collectionThread; + private static final ScheduledExecutorService executor = Executors.newScheduledThreadPool(2); + + static final String DEFAULT_ADDRESS = "limelight.local"; + + private ScheduledFuture thread; + + boolean started = false; + + RestLimelight(String address) { + var limelightClient = new LimelightClient(address); + collectionThread = new DataCollection(limelightClient); + aprilTagMapPoseHelper = new AprilTagMapPoseHelper(limelightClient); + } + + void start() { + if (!started) { + thread = executor.scheduleAtFixedRate(collectionThread, 20, 40, TimeUnit.MILLISECONDS); + started = true; + } + } + + void runThread() { + collectionThread.run(); + } + + @Override + public Optional getJsonDump() { + return collectionThread.getMostRecent().map(DataCollection.Result::json); + } + + public OptionalDouble getCaptureLatency() { + return getLocationalData().getCaptureLatency(); + } + + @Override + public OptionalDouble getTimestamp() { + return getLocationalData().getTimestamp(); + } + + /** + * Sets the field map for the limelight. Additionally, this may also upload the field map to the + * Limelight if desired. This will likely be a slow operation, and should not be regularly called. + * + * @param stream The reader which + * @param updateLimelight If the limelight should be updated with this field map + */ + @Override + public void setFieldMap(InputStream stream, boolean updateLimelight) throws IOException { + aprilTagMapPoseHelper.setFieldMap(stream, updateLimelight); + } + + @Override + public List getLocatedAprilTags(Set visibleTags) { + return aprilTagMapPoseHelper.getVisibleTagPoses(visibleTags); + } + + private static Function not(Function fnc) { + return (t) -> !fnc.apply(t); + } + + public boolean hasTarget() { + return getLocationalData().hasTarget(); + } + + public LocationalData getLocationalData() { + Optional locationalData = + collectionThread.getMostRecent().map(RestLocationalData::new); + return locationalData.orElse(StubLocationalData.VALID); + } + + private void clean() { + try { + thread.cancel(true); + executor.awaitTermination(2, TimeUnit.SECONDS); + } catch (InterruptedException e) { + DriverStation.reportError(e.getMessage(), false); + } + } + + /** + * Gets the limelight with the default name. + * + * @return the {@link Limelight} object for interfacing with the limelight + */ + public static Limelight getDefaultLimelight() { + return getLimelight(DEFAULT_ADDRESS); + } + + /** + * Gets the limelight with the specified name. Calling with a blank {@code limelightName} is + * equivalent to calling {@link #getDefaultLimelight()} + * + * @param limelightAddress The hostname or ip address of the limelight + * @return the {@link Limelight} object for interfacing with the limelight + * @throws NullPointerException if {@code limelightName} is null + */ + public static Limelight getLimelight(String limelightAddress) { + String addr = Objects.requireNonNull(limelightAddress, "limelightAddress shouldn't be null"); + if (addr.isEmpty()) { + throw new IllegalArgumentException("limelightAddress shouldn't be empty"); + } + RestLimelight result = limelights.computeIfAbsent(addr, RestLimelight::new); + result.start(); + return result; + } + + static void eraseInstances() { + for (RestLimelight limelight : limelights.values()) { + limelight.clean(); + } + limelights.clear(); + } + + private class RestLocationalData implements LocationalData { + private final JSONObject root; + private final double responseTimestamp; + + RestLocationalData(DataCollection.Result result) { + this.root = getRoot(result.json()); + this.responseTimestamp = result.responseTimestamp(); + } + + @Override + public boolean isValid() { + return getBooleanFromInt(root, "v").orElse(false); + } + + @Override + public boolean hasTarget() { + return getArr(root, "Fiducial").map(not(JSONArray::isEmpty)).orElse(false); + } + + private boolean invalidArray(JSONArray arr) { + boolean simple = arr.length() != 6 || !hasTarget(); + if (simple) { + return true; + } + Integer intZero = 0; + Double doubleZero = 0.0; + for (Object o : arr) { + if (!intZero.equals(o) && !doubleZero.equals(o)) { + return false; + } + } + return true; + } + + private Optional parseArr(JSONArray arr) { + if (invalidArray(arr)) { + return Optional.empty(); + } + Rotation3d rotation = + new Rotation3d( + Math.toRadians(arr.getDouble(3)), + Math.toRadians(arr.getDouble(4)), + Math.toRadians(arr.getDouble(5))); + return Optional.of( + new Pose3d(arr.getDouble(0), arr.getDouble(1), arr.getDouble(2), rotation)); + } + + @Override + public OptionalDouble getTimestamp() { + return unboxDouble(getDouble(root, "ts")); + } + + @Override + public OptionalDouble getCaptureLatency() { + return unboxDouble(getDouble(root, "cl")); + } + + @Override + public OptionalDouble getTargetingLatency() { + return unboxDouble(getDouble(root, "tl")); + } + + @Override + public Optional getBotpose() { + return getArr(root, "botpose").flatMap(this::parseArr); + } + + @Override + public Optional getBotPoseEstimate() { + return getArr(root, "botpose").flatMap(this::parseArr).map(this::toBotPoseEstimate); + } + + @Override + public Optional getBotposeBlue() { + return getArr(root, "botpose_wpiblue").flatMap(this::parseArr); + } + + @Override + public Optional getBotPoseEstimateBlue() { + return getArr(root, "botpose_wpiblue").flatMap(this::parseArr).map(this::toBotPoseEstimate); + } + + /** + * Gets the position of the robot with the red driverstation as the origin + * + * @return The position of the robot + */ + @Override + public Optional getBotposeRed() { + return getArr(root, "botpose_wpired").flatMap(this::parseArr); + } + + @Override + public Optional getBotPoseEstimateRed() { + return getArr(root, "botpose_wpired").flatMap(this::parseArr).map(this::toBotPoseEstimate); + } + + private BotPoseEstimate toBotPoseEstimate(Pose3d pose) { + // See + // https://www.chiefdelphi.com/t/timestamp-parameter-when-adding-limelight-vision-to-odometry + double latencyMillis = getCaptureLatency().orElse(0.0) + getTargetingLatency().orElse(0.0); + double timestampSeconds = responseTimestamp - (latencyMillis / 1000); + return new BotPoseEstimate( + pose.toPose2d(), timestampSeconds, getVisibleAprilTagPoses().keySet()); + } + + /** Gets the id of the targeted tag. */ + OptionalLong getTagID() { + return unboxLong(getLong(root, "pID")); + } + + @Override + public Set getVisibleTags() { + return getArr(root, "Fiducial") + .map( + arr -> { + Set ints = new HashSet<>(); + for (int i = 0; i < arr.length(); i++) { + JSONObject obj = arr.optJSONObject(i); + if (obj != null && obj.has("fID")) { + ints.add(obj.getInt("fID")); + } + } + return unmodifiableSet(ints); + }) + .orElseGet(Set::of); + } + + @Override + public Map getVisibleAprilTagPoses() { + return getArr(root, "Fiducial") + .map( + arr -> { + Map map = new HashMap<>(); + for (int i = 0; i < arr.length(); i++) { + JSONObject obj = arr.optJSONObject(i); + if (obj != null && obj.has("fID")) { + int id = obj.getInt("fID"); + aprilTagMapPoseHelper.getTagPose(id).ifPresent(pose -> map.put(id, pose)); + } + } + return unmodifiableMap(map); + }) + .orElse(emptyMap()); + } + } } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/StubLocationalData.java b/limelight/src/main/java/com/team2813/lib2813/limelight/StubLocationalData.java index 61edf5f3..a36d4075 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/StubLocationalData.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/StubLocationalData.java @@ -1,7 +1,6 @@ package com.team2813.lib2813.limelight; import edu.wpi.first.math.geometry.Pose3d; - import java.util.*; /** Implementation of LocationalData where all optional values return empty values. */ diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/Fiducial.java b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/Fiducial.java index 457cc80d..afa73bef 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/Fiducial.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/Fiducial.java @@ -7,9 +7,11 @@ public class Fiducial { private int id; private double[] transform; + public Pose3d getPosition() { return new Pose3d(new Matrix<>(Nat.N4(), Nat.N4(), transform)); } + public int getId() { return id; } diff --git a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FiducialRetriever.java b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FiducialRetriever.java index c672b29a..cbe78e56 100644 --- a/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FiducialRetriever.java +++ b/limelight/src/main/java/com/team2813/lib2813/limelight/apriltag_map/FiducialRetriever.java @@ -1,24 +1,24 @@ package com.team2813.lib2813.limelight.apriltag_map; -import com.google.gson.Gson; +import static java.nio.charset.StandardCharsets.UTF_8; +import com.google.gson.Gson; import java.io.InputStream; import java.io.InputStreamReader; import java.util.Arrays; import java.util.Map; import java.util.stream.Collectors; -import static java.nio.charset.StandardCharsets.UTF_8; - public class FiducialRetriever { private static final Gson gson = new Gson(); private final Map fiducialMap; public FiducialRetriever(InputStream stream) { FieldMap map = gson.fromJson(new InputStreamReader(stream, UTF_8), FieldMap.class); - fiducialMap = Arrays.stream(map.fiducials).collect(Collectors.toUnmodifiableMap(Fiducial::getId, f -> f)); + fiducialMap = + Arrays.stream(map.fiducials).collect(Collectors.toUnmodifiableMap(Fiducial::getId, f -> f)); } - + public Map getFiducialMap() { return fiducialMap; } diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/FakeLimelight.java b/limelight/src/test/java/com/team2813/lib2813/limelight/FakeLimelight.java index c0571227..0ef52898 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/FakeLimelight.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/FakeLimelight.java @@ -3,9 +3,6 @@ import com.sun.net.httpserver.HttpExchange; import com.sun.net.httpserver.HttpHandler; import com.sun.net.httpserver.HttpServer; -import org.json.JSONObject; -import org.junit.rules.ExternalResource; - import java.io.IOException; import java.io.InputStream; import java.io.OutputStream; @@ -13,84 +10,89 @@ import java.nio.charset.StandardCharsets; import java.util.concurrent.Executors; import java.util.logging.Logger; +import org.json.JSONObject; +import org.junit.rules.ExternalResource; public class FakeLimelight extends ExternalResource { - private static final Logger logger = Logger.getLogger("FakeLimelight"); - HttpServer server; - @Override - protected void before() throws Throwable { - server = HttpServer.create(new InetSocketAddress(5807), 0); - server.createContext("/results", resultsResponse); - server.createContext("/upload-fieldmap", fieldMapResponse); - server.setExecutor(Executors.newSingleThreadExecutor()); - server.start(); - } + private static final Logger logger = Logger.getLogger("FakeLimelight"); + HttpServer server; + + @Override + protected void before() throws Throwable { + server = HttpServer.create(new InetSocketAddress(5807), 0); + server.createContext("/results", resultsResponse); + server.createContext("/upload-fieldmap", fieldMapResponse); + server.setExecutor(Executors.newSingleThreadExecutor()); + server.start(); + } + + @Override + protected void after() { + server.stop(2); + server = null; + } + + private static class FakeGet implements HttpHandler { + private String body; + + public void setBody(String body) { + this.body = body; + } + + public String getBody() { + return body; + } - @Override - protected void after() { - server.stop(2); - server = null; - } + @Override + public void handle(HttpExchange exchange) throws IOException { + logger.info("Request for results received"); + if (!"GET".equals(exchange.getRequestMethod())) { + exchange.sendResponseHeaders(405, -1); + logger.warning(String.format("Unexpected request method: %s", exchange.getRequestMethod())); + return; + } + exchange.sendResponseHeaders(200, body.length()); + try (OutputStream os = exchange.getResponseBody()) { + os.write(body.getBytes()); + } + } + } - private static class FakeGet implements HttpHandler { - private String body; - public void setBody(String body) { - this.body = body; - } + private static class FakeFieldMap implements HttpHandler { + private String post; - public String getBody() { - return body; - } + @Override + public void handle(HttpExchange exchange) throws IOException { + logger.info("Request for field map received"); + if (!"POST".equals(exchange.getRequestMethod())) { + exchange.sendResponseHeaders(405, -1); + logger.warning(String.format("Unexpected request method: %s", exchange.getRequestMethod())); + return; + } + try (InputStream is = exchange.getRequestBody()) { + byte[] data = is.readAllBytes(); + post = new String(data, StandardCharsets.UTF_8); + } + exchange.sendResponseHeaders(200, -1); + } + } - @Override - public void handle(HttpExchange exchange) throws IOException { - logger.info("Request for results received"); - if (!"GET".equals(exchange.getRequestMethod())) { - exchange.sendResponseHeaders(405, -1); - logger.warning(String.format("Unexpected request method: %s", exchange.getRequestMethod())); - return; - } - exchange.sendResponseHeaders(200, body.length()); - try (OutputStream os = exchange.getResponseBody()) { - os.write(body.getBytes()); - } - } - } - - private static class FakeFieldMap implements HttpHandler { - private String post; - @Override - public void handle(HttpExchange exchange) throws IOException { - logger.info("Request for field map received"); - if (!"POST".equals(exchange.getRequestMethod())) { - exchange.sendResponseHeaders(405, -1); - logger.warning(String.format("Unexpected request method: %s", exchange.getRequestMethod())); - return; - } - try (InputStream is = exchange.getRequestBody()) { - byte[] data = is.readAllBytes(); - post = new String(data, StandardCharsets.UTF_8); - } - exchange.sendResponseHeaders(200, -1); - } - } + private final FakeGet resultsResponse = new FakeGet(); + private final FakeFieldMap fieldMapResponse = new FakeFieldMap(); - private final FakeGet resultsResponse = new FakeGet(); - private final FakeFieldMap fieldMapResponse = new FakeFieldMap(); + public void reset() { + resultsResponse.setBody(""); + } - public void reset() { - resultsResponse.setBody(""); - } + public void setResultsResponse(JSONObject response) { + resultsResponse.setBody(response.toString()); + } - public void setResultsResponse(JSONObject response) { - resultsResponse.setBody(response.toString()); - } - - public String getFieldMap() { - return fieldMapResponse.post; - } + public String getFieldMap() { + return fieldMapResponse.post; + } - public JSONObject getResultsResponse() { - return new JSONObject(resultsResponse.getBody()); - } + public JSONObject getResultsResponse() { + return new JSONObject(resultsResponse.getBody()); + } } diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java b/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java index 1a881786..5245c864 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/LimelightTestCase.java @@ -2,7 +2,6 @@ import static com.google.common.truth.Truth.assertThat; import static com.google.common.truth.Truth.assertWithMessage; - import static com.team2813.lib2813.limelight.truth.Pose2dSubject.assertThat; import static com.team2813.lib2813.limelight.truth.Pose3dSubject.assertThat; import static com.team2813.lib2813.limelight.truth.Translation3dSubject.assertThat; @@ -13,9 +12,6 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import org.json.JSONObject; -import org.junit.Test; - import java.io.BufferedReader; import java.io.FileNotFoundException; import java.io.IOException; @@ -27,267 +23,269 @@ import java.util.OptionalDouble; import java.util.Set; import java.util.stream.Collectors; +import org.json.JSONObject; +import org.junit.Test; abstract class LimelightTestCase { - @Test - public final void emptyValues() { - Limelight limelight = createLimelight(); - assertWithMessage("JSON should be empty") - .that(limelight.getCaptureLatency()).isEmpty(); - } - - @Test - public final void invalidDataTest() throws Exception { - JSONObject obj = readJSON("InvalidDataTest.json"); - setJson(obj); - - Limelight limelight = createLimelight(); - - LocationalData locationalData = limelight.getLocationalData(); - assertThat(locationalData.isValid()).isFalse(); - assertThat(locationalData.getBotpose()).isEmpty(); - OptionalDouble actualCaptureLatency = locationalData.getCaptureLatency(); - assertThat(actualCaptureLatency).isEmpty(); - OptionalDouble actualTargetingLatency = locationalData.getTargetingLatency(); - assertThat(actualTargetingLatency).isEmpty(); - } - - @Test - public final void absentTest1() throws Exception { - JSONObject obj = readJSON("AbsentTest1.json"); - setJson(obj); - - Limelight limelight = createLimelight(); - - LocationalData locationalData = limelight.getLocationalData(); - assertThat(locationalData.isValid()).isTrue(); - assertThat(locationalData.getBotpose()).isEmpty(); - OptionalDouble actualCaptureLatency = locationalData.getCaptureLatency(); - double expectedCaptureLatencyMs = 37.40; - assertAlmostEqual(expectedCaptureLatencyMs, actualCaptureLatency, 0.005); - OptionalDouble actualTargetingLatency = locationalData.getTargetingLatency(); - double expectedTargetingLatencyMs = 38.95; - assertAlmostEqual(expectedTargetingLatencyMs, actualTargetingLatency, 0.005); - assertThat(locationalData.getBotPoseEstimateBlue()).isEmpty(); - assertThat(locationalData.getBotPoseEstimateRed()).isEmpty(); - } - - @Test - public final void absentTest2() throws Exception { - JSONObject obj = readJSON("AbsentTest2.json"); - setJson(obj); - - Limelight limelight = createLimelight(); - - LocationalData locationalData = limelight.getLocationalData(); - assertThat(locationalData.isValid()).isTrue(); - assertThat(locationalData.getBotpose()).isEmpty(); - OptionalDouble actualCaptureLatency = locationalData.getCaptureLatency(); - double expectedCaptureLatencyMs = 37.40; - assertAlmostEqual(expectedCaptureLatencyMs, actualCaptureLatency, 0.005); - OptionalDouble actualTargetingLatency = locationalData.getTargetingLatency(); - double expectedTargetingLatencyMs = 54.64; - assertAlmostEqual(expectedTargetingLatencyMs, actualTargetingLatency, 0.005); - assertThat(locationalData.getBotPoseEstimateBlue()).isEmpty(); - assertThat(locationalData.getBotPoseEstimateRed()).isEmpty(); - } - - @Test - public final void presentTest1() throws Exception { - JSONObject obj = readJSON("PresentTest1.json"); - setJson(obj); - - Limelight limelight = createLimelight(); - - assertHasTarget(limelight); - LocationalData locationalData = limelight.getLocationalData(); - assertThat(locationalData.isValid()).isTrue(); - - OptionalDouble actualCaptureLatency = locationalData.getCaptureLatency(); - double expectedCaptureLatencyMs = 37.40; - assertAlmostEqual(expectedCaptureLatencyMs, actualCaptureLatency, 0.005); - OptionalDouble actualTargetingLatency = locationalData.getTargetingLatency(); - double expectedTargetingLatencyMs = 66.61; - assertAlmostEqual(expectedTargetingLatencyMs, actualTargetingLatency, 0.005); - - assertThat(locationalData.getBotpose()).isPresent(); - Pose3d actualPose = locationalData.getBotpose().get(); - Rotation3d rotation = new Rotation3d(Math.toRadians(6.82), Math.toRadians(-25.66), Math.toRadians(-173.14)); - Pose3d expectedPose = new Pose3d(7.35, 0.708, 0.91, rotation); - assertThat(actualPose).isWithin(0.005).of(expectedPose); - - assertThat(locationalData.getBotPoseEstimate()).isPresent(); - var poseEstimate = locationalData.getBotPoseEstimate().get(); - assertThat(poseEstimate.timestampSeconds()).isGreaterThan(0.0); - assertThat(poseEstimate.pose()).isWithin(0.005).of(expectedPose.toPose2d()); - - assertThat(locationalData.getBotPoseEstimateBlue()).isPresent(); - var blueEstimate = locationalData.getBotPoseEstimateBlue().get(); - assertThat(blueEstimate.timestampSeconds()).isGreaterThan(0.0); - var expectedPoseEstimate = new Pose2d(15.62, 4.52, rotation.toRotation2d()); - assertThat(blueEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate); - - assertThat(locationalData.getBotPoseEstimateRed()).isPresent(); - var redEstimate = locationalData.getBotPoseEstimateRed().get(); - assertThat(redEstimate.timestampSeconds()).isWithin(0.005).of(blueEstimate.timestampSeconds()); - expectedPoseEstimate = new Pose2d(0.92, 3.10, new Rotation2d(Math.toRadians(6.86))); - assertThat(redEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate); - } - - @Test - public final void presentTest2() throws Exception { - JSONObject obj = readJSON("PresentTest2.json"); - setJson(obj); - - Limelight limelight = createLimelight(); - - assertHasTarget(limelight); - LocationalData locationalData = limelight.getLocationalData(); - assertThat(locationalData.isValid()).isTrue(); - - OptionalDouble actualCaptureLatency = locationalData.getCaptureLatency(); - double expectedCaptureLatencyMs = 37.40; - assertAlmostEqual(expectedCaptureLatencyMs, actualCaptureLatency, 0.005); - OptionalDouble actualTargetingLatency = locationalData.getTargetingLatency(); - double expectedTargetingLatencyMs = 59.20; - assertAlmostEqual(expectedTargetingLatencyMs, actualTargetingLatency, 0.005); - - assertThat(locationalData.getBotpose()).isPresent(); - Pose3d actualPose = locationalData.getBotpose().get(); - Rotation3d rotation = new Rotation3d(Math.toRadians(-5.18), Math.toRadians(-24.32), Math.toRadians(-164.64)); - Pose3d expectedPose = new Pose3d(7.469, 0.81, 1.01, rotation); - assertThat(actualPose).isWithin(0.005).of(expectedPose); - - assertThat(locationalData.getBotPoseEstimateBlue()).isPresent(); - var blueEstimate = locationalData.getBotPoseEstimateBlue().get(); - assertThat(blueEstimate.timestampSeconds()).isGreaterThan(0.0); - var expectedPoseEstimate = new Pose2d(15.74, 4.81, rotation.toRotation2d()); - assertThat(blueEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate); - - assertThat(locationalData.getBotPoseEstimateRed()).isPresent(); - var redEstimate = locationalData.getBotPoseEstimateRed().get(); - expectedPoseEstimate = new Pose2d(0.80, 3.20, new Rotation2d(Math.toRadians(15.36))); - assertThat(redEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate); - assertThat(blueEstimate.timestampSeconds()).isWithin(0.005).of(redEstimate.timestampSeconds()); - } - - @Test - public final void getBotposeBlue() throws Exception { - JSONObject obj = readJSON("BotposeBlueRedTest.json"); - setJson(obj); - Limelight limelight = createLimelight(); - assertHasTarget(limelight); - - LocationalData locationalData = limelight.getLocationalData(); - Optional botposeBlue = locationalData.getBotposeBlue(); - assertThat(botposeBlue).isPresent(); - Pose3d actualPose = botposeBlue.get(); - Rotation3d expectedRotation = new Rotation3d(0, 0, Math.toRadians(-123.49)); - Pose3d expectedPose = new Pose3d(4.72, 5.20, 0, expectedRotation); - assertThat(actualPose).isWithin(0.005).of(expectedPose); - } - - @Test - public final void getBotposeRed() throws Exception { - JSONObject obj = readJSON("BotposeBlueRedTest.json"); - setJson(obj); - Limelight limelight = createLimelight(); - assertHasTarget(limelight); - - LocationalData locationalData = limelight.getLocationalData(); - Optional botposeBlue = locationalData.getBotposeRed(); - assertThat(botposeBlue).isPresent(); - Pose3d actualPose = botposeBlue.get(); - - Rotation3d expectedRotation = new Rotation3d(0, 0, Math.toRadians(56.51)); - Pose3d expectedPose = new Pose3d(11.83, 3.01, 0, expectedRotation); - assertThat(actualPose).isWithin(0.005).of(expectedPose); - } - - @Test - public final void getVisibleTags() throws Exception { - JSONObject obj = readJSON("BotposeBlueRedTest.json"); - setJson(obj); - Limelight limelight = createLimelight(); - assertHasTarget(limelight); - assertThat(limelight.getLocationalData().getVisibleTags()).containsExactly(20); - } - - @Test - public final void getVisibleAprilTagPoses() throws Exception { - JSONObject obj = readJSON("BotposeBlueRedTest.json"); - setJson(obj); - Limelight limelight = createLimelight(); - assertHasTarget(limelight); - uploadFieldMap(limelight); - - LocationalData locationalData = limelight.getLocationalData(); - Map tagMap = locationalData.getVisibleAprilTagPoses(); - assertThat(tagMap).containsKey(20); - Set tags = tagMap.keySet(); - Pose3d pose = tagMap.get(20); - assertThat(pose.getTranslation()) - .isWithin(0.005) - .of(new Translation3d(-3.87, 0.72, 0.31)); - assertThat(tagMap).hasSize(1); - - assertThat(locationalData.getBotPoseEstimate()).isPresent(); - assertThat(locationalData.getBotPoseEstimate().get().visibleAprilTags()).isEqualTo(tags); - - assertThat(locationalData.getBotPoseEstimateBlue()).isPresent(); - assertThat(locationalData.getBotPoseEstimateBlue().get().visibleAprilTags()).isEqualTo(tags); - - assertThat(locationalData.getBotPoseEstimateRed()).isPresent(); - assertThat(locationalData.getBotPoseEstimateRed().get().visibleAprilTags()).isEqualTo(tags); - } - - @Test - public final void visibleTagLocation() throws Exception { - JSONObject obj = readJSON("BotposeBlueRedTest.json"); - setJson(obj); - Limelight limelight = createLimelight(); - uploadFieldMap(limelight); - - Set visibleTags = limelight.getLocationalData().getVisibleTags(); - List aprilTags = limelight.getLocatedAprilTags(visibleTags); - assertThat(aprilTags).hasSize(1); - Pose3d pose = aprilTags.get(0); - assertThat(pose.getTranslation()).isWithin(0.005) - .of(new Translation3d(-3.87, 0.72, 0.31)); - } - - protected abstract Limelight createLimelight(); - - protected abstract void setJson(JSONObject json); - - private void uploadFieldMap(Limelight limelight) throws IOException { - boolean updateLimelight = false; - try (var stream = getClass().getResourceAsStream("frc2025r2.fmap")) { - limelight.setFieldMap(stream, updateLimelight); - } - } - - private JSONObject readJSON(String fileName) throws IOException { - try (InputStream is = getClass().getResourceAsStream(fileName)) { - if (is == null) { - throw new FileNotFoundException(fileName); - } - try (InputStreamReader isr = new InputStreamReader(is); - BufferedReader reader = new BufferedReader(isr)) { - return new JSONObject(reader.lines().collect(Collectors.joining(System.lineSeparator()))); - } - } - } - - private void assertHasTarget(Limelight limelight) { - assertWithMessage("Should have target").that(limelight.hasTarget()).isTrue(); - assertWithMessage("Should have target").that(limelight.getLocationalData().hasTarget()).isTrue(); - } - - protected static void assertAlmostEqual(double expected, OptionalDouble actual, double delta) { - actual.ifPresentOrElse( - d -> assertEquals(expected, d, delta), - () -> assertEquals(OptionalDouble.of(expected), actual)); - } + @Test + public final void emptyValues() { + Limelight limelight = createLimelight(); + assertWithMessage("JSON should be empty").that(limelight.getCaptureLatency()).isEmpty(); + } + + @Test + public final void invalidDataTest() throws Exception { + JSONObject obj = readJSON("InvalidDataTest.json"); + setJson(obj); + + Limelight limelight = createLimelight(); + + LocationalData locationalData = limelight.getLocationalData(); + assertThat(locationalData.isValid()).isFalse(); + assertThat(locationalData.getBotpose()).isEmpty(); + OptionalDouble actualCaptureLatency = locationalData.getCaptureLatency(); + assertThat(actualCaptureLatency).isEmpty(); + OptionalDouble actualTargetingLatency = locationalData.getTargetingLatency(); + assertThat(actualTargetingLatency).isEmpty(); + } + + @Test + public final void absentTest1() throws Exception { + JSONObject obj = readJSON("AbsentTest1.json"); + setJson(obj); + + Limelight limelight = createLimelight(); + + LocationalData locationalData = limelight.getLocationalData(); + assertThat(locationalData.isValid()).isTrue(); + assertThat(locationalData.getBotpose()).isEmpty(); + OptionalDouble actualCaptureLatency = locationalData.getCaptureLatency(); + double expectedCaptureLatencyMs = 37.40; + assertAlmostEqual(expectedCaptureLatencyMs, actualCaptureLatency, 0.005); + OptionalDouble actualTargetingLatency = locationalData.getTargetingLatency(); + double expectedTargetingLatencyMs = 38.95; + assertAlmostEqual(expectedTargetingLatencyMs, actualTargetingLatency, 0.005); + assertThat(locationalData.getBotPoseEstimateBlue()).isEmpty(); + assertThat(locationalData.getBotPoseEstimateRed()).isEmpty(); + } + + @Test + public final void absentTest2() throws Exception { + JSONObject obj = readJSON("AbsentTest2.json"); + setJson(obj); + + Limelight limelight = createLimelight(); + + LocationalData locationalData = limelight.getLocationalData(); + assertThat(locationalData.isValid()).isTrue(); + assertThat(locationalData.getBotpose()).isEmpty(); + OptionalDouble actualCaptureLatency = locationalData.getCaptureLatency(); + double expectedCaptureLatencyMs = 37.40; + assertAlmostEqual(expectedCaptureLatencyMs, actualCaptureLatency, 0.005); + OptionalDouble actualTargetingLatency = locationalData.getTargetingLatency(); + double expectedTargetingLatencyMs = 54.64; + assertAlmostEqual(expectedTargetingLatencyMs, actualTargetingLatency, 0.005); + assertThat(locationalData.getBotPoseEstimateBlue()).isEmpty(); + assertThat(locationalData.getBotPoseEstimateRed()).isEmpty(); + } + + @Test + public final void presentTest1() throws Exception { + JSONObject obj = readJSON("PresentTest1.json"); + setJson(obj); + + Limelight limelight = createLimelight(); + + assertHasTarget(limelight); + LocationalData locationalData = limelight.getLocationalData(); + assertThat(locationalData.isValid()).isTrue(); + + OptionalDouble actualCaptureLatency = locationalData.getCaptureLatency(); + double expectedCaptureLatencyMs = 37.40; + assertAlmostEqual(expectedCaptureLatencyMs, actualCaptureLatency, 0.005); + OptionalDouble actualTargetingLatency = locationalData.getTargetingLatency(); + double expectedTargetingLatencyMs = 66.61; + assertAlmostEqual(expectedTargetingLatencyMs, actualTargetingLatency, 0.005); + + assertThat(locationalData.getBotpose()).isPresent(); + Pose3d actualPose = locationalData.getBotpose().get(); + Rotation3d rotation = + new Rotation3d(Math.toRadians(6.82), Math.toRadians(-25.66), Math.toRadians(-173.14)); + Pose3d expectedPose = new Pose3d(7.35, 0.708, 0.91, rotation); + assertThat(actualPose).isWithin(0.005).of(expectedPose); + + assertThat(locationalData.getBotPoseEstimate()).isPresent(); + var poseEstimate = locationalData.getBotPoseEstimate().get(); + assertThat(poseEstimate.timestampSeconds()).isGreaterThan(0.0); + assertThat(poseEstimate.pose()).isWithin(0.005).of(expectedPose.toPose2d()); + + assertThat(locationalData.getBotPoseEstimateBlue()).isPresent(); + var blueEstimate = locationalData.getBotPoseEstimateBlue().get(); + assertThat(blueEstimate.timestampSeconds()).isGreaterThan(0.0); + var expectedPoseEstimate = new Pose2d(15.62, 4.52, rotation.toRotation2d()); + assertThat(blueEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate); + + assertThat(locationalData.getBotPoseEstimateRed()).isPresent(); + var redEstimate = locationalData.getBotPoseEstimateRed().get(); + assertThat(redEstimate.timestampSeconds()).isWithin(0.005).of(blueEstimate.timestampSeconds()); + expectedPoseEstimate = new Pose2d(0.92, 3.10, new Rotation2d(Math.toRadians(6.86))); + assertThat(redEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate); + } + + @Test + public final void presentTest2() throws Exception { + JSONObject obj = readJSON("PresentTest2.json"); + setJson(obj); + + Limelight limelight = createLimelight(); + + assertHasTarget(limelight); + LocationalData locationalData = limelight.getLocationalData(); + assertThat(locationalData.isValid()).isTrue(); + + OptionalDouble actualCaptureLatency = locationalData.getCaptureLatency(); + double expectedCaptureLatencyMs = 37.40; + assertAlmostEqual(expectedCaptureLatencyMs, actualCaptureLatency, 0.005); + OptionalDouble actualTargetingLatency = locationalData.getTargetingLatency(); + double expectedTargetingLatencyMs = 59.20; + assertAlmostEqual(expectedTargetingLatencyMs, actualTargetingLatency, 0.005); + + assertThat(locationalData.getBotpose()).isPresent(); + Pose3d actualPose = locationalData.getBotpose().get(); + Rotation3d rotation = + new Rotation3d(Math.toRadians(-5.18), Math.toRadians(-24.32), Math.toRadians(-164.64)); + Pose3d expectedPose = new Pose3d(7.469, 0.81, 1.01, rotation); + assertThat(actualPose).isWithin(0.005).of(expectedPose); + + assertThat(locationalData.getBotPoseEstimateBlue()).isPresent(); + var blueEstimate = locationalData.getBotPoseEstimateBlue().get(); + assertThat(blueEstimate.timestampSeconds()).isGreaterThan(0.0); + var expectedPoseEstimate = new Pose2d(15.74, 4.81, rotation.toRotation2d()); + assertThat(blueEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate); + + assertThat(locationalData.getBotPoseEstimateRed()).isPresent(); + var redEstimate = locationalData.getBotPoseEstimateRed().get(); + expectedPoseEstimate = new Pose2d(0.80, 3.20, new Rotation2d(Math.toRadians(15.36))); + assertThat(redEstimate.pose()).isWithin(0.005).of(expectedPoseEstimate); + assertThat(blueEstimate.timestampSeconds()).isWithin(0.005).of(redEstimate.timestampSeconds()); + } + + @Test + public final void getBotposeBlue() throws Exception { + JSONObject obj = readJSON("BotposeBlueRedTest.json"); + setJson(obj); + Limelight limelight = createLimelight(); + assertHasTarget(limelight); + + LocationalData locationalData = limelight.getLocationalData(); + Optional botposeBlue = locationalData.getBotposeBlue(); + assertThat(botposeBlue).isPresent(); + Pose3d actualPose = botposeBlue.get(); + Rotation3d expectedRotation = new Rotation3d(0, 0, Math.toRadians(-123.49)); + Pose3d expectedPose = new Pose3d(4.72, 5.20, 0, expectedRotation); + assertThat(actualPose).isWithin(0.005).of(expectedPose); + } + + @Test + public final void getBotposeRed() throws Exception { + JSONObject obj = readJSON("BotposeBlueRedTest.json"); + setJson(obj); + Limelight limelight = createLimelight(); + assertHasTarget(limelight); + + LocationalData locationalData = limelight.getLocationalData(); + Optional botposeBlue = locationalData.getBotposeRed(); + assertThat(botposeBlue).isPresent(); + Pose3d actualPose = botposeBlue.get(); + + Rotation3d expectedRotation = new Rotation3d(0, 0, Math.toRadians(56.51)); + Pose3d expectedPose = new Pose3d(11.83, 3.01, 0, expectedRotation); + assertThat(actualPose).isWithin(0.005).of(expectedPose); + } + + @Test + public final void getVisibleTags() throws Exception { + JSONObject obj = readJSON("BotposeBlueRedTest.json"); + setJson(obj); + Limelight limelight = createLimelight(); + assertHasTarget(limelight); + assertThat(limelight.getLocationalData().getVisibleTags()).containsExactly(20); + } + + @Test + public final void getVisibleAprilTagPoses() throws Exception { + JSONObject obj = readJSON("BotposeBlueRedTest.json"); + setJson(obj); + Limelight limelight = createLimelight(); + assertHasTarget(limelight); + uploadFieldMap(limelight); + + LocationalData locationalData = limelight.getLocationalData(); + Map tagMap = locationalData.getVisibleAprilTagPoses(); + assertThat(tagMap).containsKey(20); + Set tags = tagMap.keySet(); + Pose3d pose = tagMap.get(20); + assertThat(pose.getTranslation()).isWithin(0.005).of(new Translation3d(-3.87, 0.72, 0.31)); + assertThat(tagMap).hasSize(1); + + assertThat(locationalData.getBotPoseEstimate()).isPresent(); + assertThat(locationalData.getBotPoseEstimate().get().visibleAprilTags()).isEqualTo(tags); + + assertThat(locationalData.getBotPoseEstimateBlue()).isPresent(); + assertThat(locationalData.getBotPoseEstimateBlue().get().visibleAprilTags()).isEqualTo(tags); + + assertThat(locationalData.getBotPoseEstimateRed()).isPresent(); + assertThat(locationalData.getBotPoseEstimateRed().get().visibleAprilTags()).isEqualTo(tags); + } + + @Test + public final void visibleTagLocation() throws Exception { + JSONObject obj = readJSON("BotposeBlueRedTest.json"); + setJson(obj); + Limelight limelight = createLimelight(); + uploadFieldMap(limelight); + + Set visibleTags = limelight.getLocationalData().getVisibleTags(); + List aprilTags = limelight.getLocatedAprilTags(visibleTags); + assertThat(aprilTags).hasSize(1); + Pose3d pose = aprilTags.get(0); + assertThat(pose.getTranslation()).isWithin(0.005).of(new Translation3d(-3.87, 0.72, 0.31)); + } + + protected abstract Limelight createLimelight(); + + protected abstract void setJson(JSONObject json); + + private void uploadFieldMap(Limelight limelight) throws IOException { + boolean updateLimelight = false; + try (var stream = getClass().getResourceAsStream("frc2025r2.fmap")) { + limelight.setFieldMap(stream, updateLimelight); + } + } + + private JSONObject readJSON(String fileName) throws IOException { + try (InputStream is = getClass().getResourceAsStream(fileName)) { + if (is == null) { + throw new FileNotFoundException(fileName); + } + try (InputStreamReader isr = new InputStreamReader(is); + BufferedReader reader = new BufferedReader(isr)) { + return new JSONObject(reader.lines().collect(Collectors.joining(System.lineSeparator()))); + } + } + } + + private void assertHasTarget(Limelight limelight) { + assertWithMessage("Should have target").that(limelight.hasTarget()).isTrue(); + assertWithMessage("Should have target") + .that(limelight.getLocationalData().hasTarget()) + .isTrue(); + } + + protected static void assertAlmostEqual(double expected, OptionalDouble actual, double delta) { + actual.ifPresentOrElse( + d -> assertEquals(expected, d, delta), + () -> assertEquals(OptionalDouble.of(expected), actual)); + } } diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/NetworkTablesLimelightTest.java b/limelight/src/test/java/com/team2813/lib2813/limelight/NetworkTablesLimelightTest.java index 3d366e85..a975c261 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/NetworkTablesLimelightTest.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/NetworkTablesLimelightTest.java @@ -2,21 +2,23 @@ import edu.wpi.first.networktables.DoubleArrayEntry; import edu.wpi.first.networktables.NetworkTableEntry; +import java.util.List; import org.json.JSONArray; import org.json.JSONObject; import org.junit.After; -import java.util.List; - public class NetworkTablesLimelightTest extends LimelightTestCase { private static final String TABLE_NAME = "limelight"; private static final long ONE_HALF_MS_IN_MICROS = 500; private static final long ONE_SECOND_IN_MICROS = 1_000_000; private static long fakeTimestampMicros = 15 * ONE_SECOND_IN_MICROS; - private static final List BOT_POSE_ESTIMATE_ENTRIES = List.of( + private static final List BOT_POSE_ESTIMATE_ENTRIES = + List.of( "botpose", - "botpose_wpired", "botpose_orb_wpired", - "botpose_wpiblue", "botpose_orb_wpiblue"); + "botpose_wpired", + "botpose_orb_wpired", + "botpose_wpiblue", + "botpose_orb_wpiblue"); @After public void resetNetworkTables() { @@ -52,7 +54,8 @@ private static NetworkTableEntry getJsonNTEntry() { return LimelightHelpers.getLimelightNTTableEntry(TABLE_NAME, "json"); } - private static void setBotPoseEstimate(JSONObject resultsJson, String entryName, double latencyMillis) { + private static void setBotPoseEstimate( + JSONObject resultsJson, String entryName, double latencyMillis) { double[] estimate_array = getBotPoseEstimateArray(resultsJson, entryName, latencyMillis); DoubleArrayEntry tableEntry = LimelightHelpers.getLimelightDoubleArrayEntry(TABLE_NAME, entryName); @@ -61,11 +64,12 @@ private static void setBotPoseEstimate(JSONObject resultsJson, String entryName, private static void clearBotPoseEstimate(String entryName, long timestampMicros) { DoubleArrayEntry tableEntry = - LimelightHelpers.getLimelightDoubleArrayEntry(TABLE_NAME, entryName); + LimelightHelpers.getLimelightDoubleArrayEntry(TABLE_NAME, entryName); tableEntry.set(new double[0], timestampMicros); } - private static double[] getBotPoseEstimateArray(JSONObject resultsJson, String entryName, double latencyMillis) { + private static double[] getBotPoseEstimateArray( + JSONObject resultsJson, String entryName, double latencyMillis) { if (!resultsJson.has(entryName)) { return new double[0]; // getBotPoseEstimate_wpi{Blue,Red}() will return null } diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/RestLimelightTest.java b/limelight/src/test/java/com/team2813/lib2813/limelight/RestLimelightTest.java index 2af9fead..b1e5e1d7 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/RestLimelightTest.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/RestLimelightTest.java @@ -1,9 +1,7 @@ package com.team2813.lib2813.limelight; -import org.json.JSONObject; -import org.junit.After; -import org.junit.ClassRule; -import org.junit.Test; +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertNotNull; import java.io.InputStream; import java.net.URI; @@ -13,73 +11,76 @@ import java.net.http.HttpResponse.BodyHandlers; import java.nio.charset.StandardCharsets; import java.util.concurrent.Executors; - -import static org.junit.Assert.assertEquals; -import static org.junit.Assert.assertNotNull; +import org.json.JSONObject; +import org.junit.After; +import org.junit.ClassRule; +import org.junit.Test; public class RestLimelightTest extends LimelightTestCase { - @ClassRule - public static final FakeLimelight fakeLimelight = new FakeLimelight(); + @ClassRule public static final FakeLimelight fakeLimelight = new FakeLimelight(); + + @After + public void resetLimelights() { + RestLimelight.eraseInstances(); + } + + @After + public void resetFakeLimelight() { + fakeLimelight.reset(); + } - @After - public void resetLimelights() { - RestLimelight.eraseInstances(); - } + @Test + public void equality() { + Limelight a = RestLimelight.getDefaultLimelight(); + Limelight b = RestLimelight.getDefaultLimelight(); + assertEquals("Default limelight call returned different values", a, b); + Limelight c = RestLimelight.getLimelight(RestLimelight.DEFAULT_ADDRESS); + assertEquals("Default limelights not equal to limelights named \"limelight\" (default)", a, c); + } - @After - public void resetFakeLimelight() { - fakeLimelight.reset(); - } + @Test + public void fakeWorks() throws Exception { + fakeLimelight.setResultsResponse(new JSONObject().put("v", 1)); + HttpRequest req = + HttpRequest.newBuilder(new URI("http://localhost:5807/results")).GET().build(); + HttpClient client = + HttpClient.newBuilder().executor(Executors.newSingleThreadExecutor()).build(); + HttpResponse response = client.send(req, BodyHandlers.ofString()); + assertEquals("{\"v\":1}", response.body()); + } - @Test - public void equality() { - Limelight a = RestLimelight.getDefaultLimelight(); - Limelight b = RestLimelight.getDefaultLimelight(); - assertEquals("Default limelight call returned different values", a, b); - Limelight c = RestLimelight.getLimelight(RestLimelight.DEFAULT_ADDRESS); - assertEquals( - "Default limelights not equal to limelights named \"limelight\" (default)", - a, c); - } + @Test + public void setFieldMapWorks() throws Exception { + Limelight limelight = createLimelight(); + String resourceName = "frc2025r2.fmap"; + try (InputStream is = getClass().getResourceAsStream(resourceName)) { + limelight.setFieldMap(is, true); + } + String fieldMap = fakeLimelight.getFieldMap(); + assertNotNull(fieldMap); - @Test - public void fakeWorks() throws Exception { - fakeLimelight.setResultsResponse(new JSONObject().put("v", 1)); - HttpRequest req = HttpRequest.newBuilder(new URI("http://localhost:5807/results")).GET().build(); - HttpClient client = HttpClient.newBuilder().executor(Executors.newSingleThreadExecutor()).build(); - HttpResponse response = client.send(req, BodyHandlers.ofString()); - assertEquals("{\"v\":1}", response.body()); - } - - @Test - public void setFieldMapWorks() throws Exception { - Limelight limelight = createLimelight(); - String resourceName = "frc2025r2.fmap"; - try (InputStream is = getClass().getResourceAsStream(resourceName)) { - limelight.setFieldMap(is, true); - } - String fieldMap = fakeLimelight.getFieldMap(); - assertNotNull(fieldMap); - - String expectedFieldMap; - try (InputStream is = getClass().getResourceAsStream(resourceName)) { - expectedFieldMap = new String(is.readAllBytes(), StandardCharsets.UTF_8); - } - assertEquals(expectedFieldMap, fieldMap); + String expectedFieldMap; + try (InputStream is = getClass().getResourceAsStream(resourceName)) { + expectedFieldMap = new String(is.readAllBytes(), StandardCharsets.UTF_8); + } + assertEquals(expectedFieldMap, fieldMap); } - @Override - protected Limelight createLimelight() { - RestLimelight limelight = new RestLimelight("localhost"); - limelight.runThread(); - return limelight; - } + @Override + protected Limelight createLimelight() { + RestLimelight limelight = new RestLimelight("localhost"); + limelight.runThread(); + return limelight; + } - @Override - protected void setJson(JSONObject json) { - // limelight json schema has been updated to not json object "Results" in the root, and we want to test the new version, which does not. - // The new version just has all json that was in "Results" in the root, so the json object in "Results" will essentially be the new schema. - // Since we know that all the json objects in the resources folder have the "Results" json object, this should never fail. - fakeLimelight.setResultsResponse(json.getJSONObject("Results")); - } + @Override + protected void setJson(JSONObject json) { + // limelight json schema has been updated to not json object "Results" in the root, and we want + // to test the new version, which does not. + // The new version just has all json that was in "Results" in the root, so the json object in + // "Results" will essentially be the new schema. + // Since we know that all the json objects in the resources folder have the "Results" json + // object, this should never fail. + fakeLimelight.setResultsResponse(json.getJSONObject("Results")); + } } diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Pose2dSubject.java b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Pose2dSubject.java index f2f93671..d119af47 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Pose2dSubject.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Pose2dSubject.java @@ -1,65 +1,68 @@ package com.team2813.lib2813.limelight.truth; +import static com.google.common.truth.Fact.simpleFact; +import static com.google.common.truth.Truth.assertAbout; + import com.google.common.truth.FailureMetadata; import com.google.common.truth.Subject; import edu.wpi.first.math.geometry.Pose2d; - import javax.annotation.Nullable; -import static com.google.common.truth.Fact.simpleFact; -import static com.google.common.truth.Truth.assertAbout; - /** * Truth Subject for making assertions about {@link Pose2d} values. * - *

See Writing your own custom subject - * to learn about creating custom Truth subjects. + *

See Writing your own custom subject to learn about + * creating custom Truth subjects. */ public final class Pose2dSubject extends Subject { - // User-defined entry point - public static Pose2dSubject assertThat(@Nullable Pose2d pose) { - return assertAbout(pose2ds()).that(pose); - } + // User-defined entry point + public static Pose2dSubject assertThat(@Nullable Pose2d pose) { + return assertAbout(pose2ds()).that(pose); + } - // Static method for getting the subject factory (for use with assertAbout()) - public static Subject.Factory pose2ds() { - return Pose2dSubject::new; - } + // Static method for getting the subject factory (for use with assertAbout()) + public static Subject.Factory pose2ds() { + return Pose2dSubject::new; + } - private final Pose2d actual; + private final Pose2d actual; - private Pose2dSubject(FailureMetadata failureMetadata, @Nullable Pose2d subject) { - super(failureMetadata, subject); - this.actual = subject; - } + private Pose2dSubject(FailureMetadata failureMetadata, @Nullable Pose2d subject) { + super(failureMetadata, subject); + this.actual = subject; + } - // User-defined test assertion SPI below this point + // User-defined test assertion SPI below this point - public TolerantComparison isWithin(double tolerance) { - return new TolerantComparison() { - @Override - public void of(Pose2d expected) { - translation().isWithin(tolerance).of(expected.getTranslation()); - rotation().isWithin(tolerance).of(expected.getRotation()); - } - }; - } + public TolerantComparison isWithin(double tolerance) { + return new TolerantComparison() { + @Override + public void of(Pose2d expected) { + translation().isWithin(tolerance).of(expected.getTranslation()); + rotation().isWithin(tolerance).of(expected.getRotation()); + } + }; + } - public Translation2dSubject translation() { - return check("getTranslation()").about(Translation2dSubject.translation2ds()).that(nonNullActualPose().getTranslation()); - } + public Translation2dSubject translation() { + return check("getTranslation()") + .about(Translation2dSubject.translation2ds()) + .that(nonNullActualPose().getTranslation()); + } - public Rotation2dSubject rotation() { - return check("getRotation()").about(Rotation2dSubject.rotation2ds()).that(nonNullActualPose().getRotation()); - } + public Rotation2dSubject rotation() { + return check("getRotation()") + .about(Rotation2dSubject.rotation2ds()) + .that(nonNullActualPose().getRotation()); + } - // Helper methods below this point + // Helper methods below this point - private Pose2d nonNullActualPose() { - if (actual == null) { - failWithActual(simpleFact("expected a non-null Pose2d")); - } - return actual; + private Pose2d nonNullActualPose() { + if (actual == null) { + failWithActual(simpleFact("expected a non-null Pose2d")); } + return actual; + } } diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Pose3dSubject.java b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Pose3dSubject.java index 2952bc74..160a9899 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Pose3dSubject.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Pose3dSubject.java @@ -1,68 +1,70 @@ package com.team2813.lib2813.limelight.truth; +import static com.google.common.truth.Fact.simpleFact; +import static com.google.common.truth.Truth.assertAbout; + import com.google.common.truth.FailureMetadata; import com.google.common.truth.Subject; import edu.wpi.first.math.geometry.Pose3d; - import javax.annotation.Nullable; -import static com.google.common.truth.Fact.simpleFact; -import static com.google.common.truth.Truth.assertAbout; - /** * Truth Subject for making assertions about {@link Pose3d} values. * - *

See Writing your own custom subject - * to learn about creating custom Truth subjects. + *

See Writing your own custom subject to learn about + * creating custom Truth subjects. */ public final class Pose3dSubject extends Subject { - // User-defined entry point - public static Pose3dSubject assertThat(@Nullable Pose3d pose) { - return assertAbout(pose3ds()).that(pose); - } - - // Static method for getting the subject factory (for use with assertAbout()) - public static Subject.Factory pose3ds() { - return Pose3dSubject::new; - } - - private final Pose3d actual; - - private Pose3dSubject(FailureMetadata failureMetadata, @Nullable Pose3d subject) { - super(failureMetadata, subject); - this.actual = subject; + // User-defined entry point + public static Pose3dSubject assertThat(@Nullable Pose3d pose) { + return assertAbout(pose3ds()).that(pose); + } + + // Static method for getting the subject factory (for use with assertAbout()) + public static Subject.Factory pose3ds() { + return Pose3dSubject::new; + } + + private final Pose3d actual; + + private Pose3dSubject(FailureMetadata failureMetadata, @Nullable Pose3d subject) { + super(failureMetadata, subject); + this.actual = subject; + } + + // User-defined test assertion SPI below this point + + public TolerantComparison isWithin(double tolerance) { + return new TolerantComparison() { + @Override + public void of(Pose3d expected) { + translation().isWithin(tolerance).of(expected.getTranslation()); + rotation().isWithin(tolerance).of(expected.getRotation()); + } + }; + } + + // Chained subjects methods below this point + + public Translation3dSubject translation() { + return check("getTranslation()") + .about(Translation3dSubject.translation3ds()) + .that(nonNullActualPose().getTranslation()); + } + + public Rotation3dSubject rotation() { + return check("getRotation()") + .about(Rotation3dSubject.rotation3ds()) + .that(nonNullActualPose().getRotation()); + } + + // Helper methods below this point + + private Pose3d nonNullActualPose() { + if (actual == null) { + failWithActual(simpleFact("expected a non-null Pose3d")); } - - // User-defined test assertion SPI below this point - - public TolerantComparison isWithin(double tolerance) { - return new TolerantComparison() { - @Override - public void of(Pose3d expected) { - translation().isWithin(tolerance).of(expected.getTranslation()); - rotation().isWithin(tolerance).of(expected.getRotation()); - } - }; - } - - // Chained subjects methods below this point - - public Translation3dSubject translation() { - return check("getTranslation()").about(Translation3dSubject.translation3ds()).that(nonNullActualPose().getTranslation()); - } - - public Rotation3dSubject rotation() { - return check("getRotation()").about(Rotation3dSubject.rotation3ds()).that(nonNullActualPose().getRotation()); - } - - // Helper methods below this point - - private Pose3d nonNullActualPose() { - if (actual == null) { - failWithActual(simpleFact("expected a non-null Pose3d")); - } - return actual; - } - + return actual; + } } diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Rotation2dSubject.java b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Rotation2dSubject.java index 16c9c109..264352ec 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Rotation2dSubject.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Rotation2dSubject.java @@ -1,15 +1,14 @@ package com.team2813.lib2813.limelight.truth; +import static com.google.common.truth.Fact.simpleFact; +import static com.google.common.truth.Truth.assertAbout; + import com.google.common.truth.DoubleSubject; import com.google.common.truth.FailureMetadata; import com.google.common.truth.Subject; import edu.wpi.first.math.geometry.Rotation2d; - import javax.annotation.Nullable; -import static com.google.common.truth.Fact.simpleFact; -import static com.google.common.truth.Truth.assertAbout; - /** Truth Subject for making assertions about {@link Rotation2d} values. */ public final class Rotation2dSubject extends Subject { // User-defined entry point diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Rotation3dSubject.java b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Rotation3dSubject.java index ec0b3cf8..5503bda7 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Rotation3dSubject.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Rotation3dSubject.java @@ -1,15 +1,14 @@ package com.team2813.lib2813.limelight.truth; +import static com.google.common.truth.Fact.simpleFact; +import static com.google.common.truth.Truth.assertAbout; + import com.google.common.truth.DoubleSubject; import com.google.common.truth.FailureMetadata; import com.google.common.truth.Subject; import edu.wpi.first.math.geometry.Rotation3d; - import javax.annotation.Nullable; -import static com.google.common.truth.Fact.simpleFact; -import static com.google.common.truth.Truth.assertAbout; - /** Truth Subject for making assertions about {@link Rotation3d} values. */ public final class Rotation3dSubject extends Subject { // User-defined entry point @@ -51,24 +50,24 @@ public void isZero() { // Chained subjects methods below this point /** - * Returns a subject that can be used to make assertions about the - * counterclockwise rotation angle around the X axis (roll) in radians. + * Returns a subject that can be used to make assertions about the counterclockwise rotation angle + * around the X axis (roll) in radians. */ public DoubleSubject x() { return check("getX()").that(nonNullActual().getX()); } /** - * Returns a subject that can be used to make assertions about the - * counterclockwise rotation angle around the Y axis (pitch) in radians. + * Returns a subject that can be used to make assertions about the counterclockwise rotation angle + * around the Y axis (pitch) in radians. */ public DoubleSubject y() { return check("getY()").that(nonNullActual().getY()); } /** - * Returns a subject that can be used to make assertions about the - * counterclockwise rotation angle around the Z axis (yaw) in radians. + * Returns a subject that can be used to make assertions about the counterclockwise rotation angle + * around the Z axis (yaw) in radians. */ public DoubleSubject z() { return check("getZ()").that(nonNullActual().getZ()); diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/TolerantComparison.java b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/TolerantComparison.java index f57419a3..dd7bed52 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/TolerantComparison.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/TolerantComparison.java @@ -21,14 +21,14 @@ public abstract class TolerantComparison { /** * @throws UnsupportedOperationException always - * @deprecated {@link Object#equals(Object)} is not supported on TolerantComparison. If - * you meant to compare doubles, use {@link #of(T)} instead. + * @deprecated {@link Object#equals(Object)} is not supported on TolerantComparison. If you meant + * to compare doubles, use {@link #of(T)} instead. */ @Deprecated @Override public final boolean equals(@Nullable Object o) { throw new UnsupportedOperationException( - "If you meant to compare values, use TolerantComparison.of() instead."); + "If you meant to compare values, use TolerantComparison.of() instead."); } /** diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Translation2dSubject.java b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Translation2dSubject.java index f5aead11..6f7fdbf5 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Translation2dSubject.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Translation2dSubject.java @@ -1,15 +1,14 @@ package com.team2813.lib2813.limelight.truth; +import static com.google.common.truth.Fact.simpleFact; +import static com.google.common.truth.Truth.assertAbout; + import com.google.common.truth.DoubleSubject; import com.google.common.truth.FailureMetadata; import com.google.common.truth.Subject; import edu.wpi.first.math.geometry.Translation2d; - import javax.annotation.Nullable; -import static com.google.common.truth.Fact.simpleFact; -import static com.google.common.truth.Truth.assertAbout; - /** Truth Subject for making assertions about {@link Translation2d} values. */ public final class Translation2dSubject extends Subject { diff --git a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Translation3dSubject.java b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Translation3dSubject.java index 323f1019..6659c841 100644 --- a/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Translation3dSubject.java +++ b/limelight/src/test/java/com/team2813/lib2813/limelight/truth/Translation3dSubject.java @@ -1,15 +1,14 @@ package com.team2813.lib2813.limelight.truth; +import static com.google.common.truth.Fact.simpleFact; +import static com.google.common.truth.Truth.assertAbout; + import com.google.common.truth.DoubleSubject; import com.google.common.truth.FailureMetadata; import com.google.common.truth.Subject; import edu.wpi.first.math.geometry.Translation3d; - import javax.annotation.Nullable; -import static com.google.common.truth.Fact.simpleFact; -import static com.google.common.truth.Truth.assertAbout; - /** Truth Subject for making assertions about {@link Translation3d} values. */ public final class Translation3dSubject extends Subject {