From 38f94b324f69eacd9e674dad45e8945a56d5e1f1 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Mon, 6 Oct 2025 23:00:08 -0700 Subject: [PATCH 01/13] Add vision project and PhotonVisionPosePublisher (from Robot2025) --- build.gradle | 4 + settings.gradle | 1 + vision/build.gradle | 39 ++++ .../lib2813/vision/CameraConstants.java | 9 + .../vision/PhotonVisionPosePublisher.java | 73 +++++++ .../vision/TimestampedStructPublisher.java | 64 ++++++ .../lib2813/vision/TimestampedValue.java | 81 ++++++++ .../lib2813/vision/VisionNetworkTables.java | 63 ++++++ .../lib2813/vision/IsolatedNetworkTable.java | 45 +++++ .../TimestampedStructPublisherTest.java | 188 ++++++++++++++++++ 10 files changed, 567 insertions(+) create mode 100644 vision/build.gradle create mode 100644 vision/src/main/java/com/team2813/lib2813/vision/CameraConstants.java create mode 100644 vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java create mode 100644 vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java create mode 100644 vision/src/main/java/com/team2813/lib2813/vision/TimestampedValue.java create mode 100644 vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java create mode 100644 vision/src/test/java/com/team2813/lib2813/vision/IsolatedNetworkTable.java create mode 100644 vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java diff --git a/build.gradle b/build.gradle index 6235c730..c6c84d7b 100644 --- a/build.gradle +++ b/build.gradle @@ -13,5 +13,9 @@ allprojects { // REV Robotics url 'https://maven.revrobotics.com/' } + maven { + name 'photonvisionRepositoryRepository' + url 'https://maven.photonvision.org/releases' + } } } \ No newline at end of file diff --git a/settings.gradle b/settings.gradle index 536f46a8..74df31fd 100644 --- a/settings.gradle +++ b/settings.gradle @@ -10,4 +10,5 @@ rootProject.name = 'lib2813' include('lib') include('limelight') +include('vision') include('testing') diff --git a/vision/build.gradle b/vision/build.gradle new file mode 100644 index 00000000..27137dc9 --- /dev/null +++ b/vision/build.gradle @@ -0,0 +1,39 @@ +plugins { + id 'java-common-conventions' + id "edu.wpi.first.GradleRIO" version "2025.1.1" + id 'idea' +} + +idea { + module { + downloadJavadoc = true + downloadSources = true + } +} + +dependencies { + implementation wpi.java.deps.wpilib() + implementation wpi.java.vendor.java() + implementation 'org.photonvision:photonlib-java:v2025.3.2' + implementation 'org.photonvision:photontargeting-java:v2025.3.2' + implementation project(':lib') + + testImplementation 'junit:junit:4.13.2' + testImplementation 'com.google.truth:truth:1.4.4' + testImplementation project(':testing') + + 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() +} + +wpi.java.configureTestTasks(test) + +test { + // Use JUnit 4 for tests + useJUnit() +} diff --git a/vision/src/main/java/com/team2813/lib2813/vision/CameraConstants.java b/vision/src/main/java/com/team2813/lib2813/vision/CameraConstants.java new file mode 100644 index 00000000..f88a811f --- /dev/null +++ b/vision/src/main/java/com/team2813/lib2813/vision/CameraConstants.java @@ -0,0 +1,9 @@ +package com.team2813.lib2813.vision; + +class CameraConstants { + public static final String LIMELIGHT_CAMERA_NAME = "limelight"; + + private CameraConstants() { + throw new AssertionError("Not instantiable"); + } +} diff --git a/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java new file mode 100644 index 00000000..b7c8dfbc --- /dev/null +++ b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java @@ -0,0 +1,73 @@ +package com.team2813.lib2813.vision; + +import static com.team2813.lib2813.vision.VisionNetworkTables.APRIL_TAG_POSE_TOPIC; +import static com.team2813.lib2813.vision.VisionNetworkTables.POSE_ESTIMATE_TOPIC; +import static com.team2813.lib2813.vision.VisionNetworkTables.getTableForCamera; + +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.StructTopic; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj.Timer; +import java.util.List; +import java.util.function.Supplier; +import java.util.stream.Stream; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; + +/** Published timestamped pose estimates from a camera. */ +public final class PhotonVisionPosePublisher { + private final TimestampedStructPublisher publisher; + private final TimestampedStructPublisher tagPublisher; + private final AprilTagFieldLayout aprilTagFieldLayout; + + /** Creates a publisher for the provided camera and field layout. */ + public PhotonVisionPosePublisher(PhotonCamera camera, AprilTagFieldLayout aprilTagFieldLayout) { + this(camera, aprilTagFieldLayout, Timer::getFPGATimestamp); + } + + /** Package-scoped constructor (for unit testing). */ + PhotonVisionPosePublisher( + PhotonCamera camera, + AprilTagFieldLayout aprilTagFieldLayout, + Supplier fpgaTimestampSupplier) { + this.aprilTagFieldLayout = aprilTagFieldLayout; + NetworkTable table = getTableForCamera(camera); + StructTopic topic = table.getStructTopic(POSE_ESTIMATE_TOPIC, Pose3d.struct); + publisher = new TimestampedStructPublisher<>(topic, Pose3d.kZero, fpgaTimestampSupplier); + topic = table.getStructTopic(APRIL_TAG_POSE_TOPIC, Pose3d.struct); + tagPublisher = new TimestampedStructPublisher<>(topic, Pose3d.kZero, fpgaTimestampSupplier); + } + + /** + * Publishes the estimated positions to network tables. + * + * @param poseEstimates The estimated locations (with the blue driver station as the origin). + */ + public void publish(List poseEstimates) { + publisher.publish( + poseEstimates.stream() + .map(PhotonVisionPosePublisher::toRobotPoseTimestampedValue) + .toList()); + tagPublisher.publish( + poseEstimates.stream().flatMap(this::toAprilTagPoseTimestampedValue).toList()); + } + + private static TimestampedValue toRobotPoseTimestampedValue(EstimatedRobotPose pose) { + return TimestampedValue.withFpgaTimestamp( + pose.timestampSeconds, Units.Seconds, pose.estimatedPose); + } + + private Stream> toAprilTagPoseTimestampedValue(EstimatedRobotPose pose) { + if (pose.targetsUsed.isEmpty()) { + return Stream.empty(); + } + return aprilTagFieldLayout + .getTagPose(pose.targetsUsed.get(0).fiducialId) + .map( + tagPose -> + TimestampedValue.withFpgaTimestamp(pose.timestampSeconds, Units.Seconds, tagPose)) + .stream(); + } +} diff --git a/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java new file mode 100644 index 00000000..76f67da6 --- /dev/null +++ b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java @@ -0,0 +1,64 @@ +package com.team2813.lib2813.vision; + +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.networktables.StructTopic; +import edu.wpi.first.wpilibj.TimedRobot; +import java.util.List; +import java.util.function.Supplier; + +/** Publishes timestamped data to a network tables topic. */ +final class TimestampedStructPublisher { + private static final long MICROS_PER_SECOND = 1_000_000; + static final long EXPECTED_UPDATE_FREQUENCY_MICROS = + (long) (TimedRobot.kDefaultPeriod * MICROS_PER_SECOND); + // PhotonVision appears to produce estimates every 80ms, so treat values older than 0.1s as stale. + static final long PUBLISHED_VALUE_VALID_MICROS = MICROS_PER_SECOND / 10; + + private final StructPublisher publisher; + private final Supplier fpgaTimestampSupplier; + private final S zeroValue; + private long lastUpdateTimeMicros; + private boolean publishedZeroValue; + + /** + * Creates a publisher. + * + * @param topic Topic to publish to. + * @param zeroValue Value to publish when data is determined to be stale. + * @param fpgaTimestampSupplier Supplies FPGA timestamps in seconds. + */ + TimestampedStructPublisher( + StructTopic topic, S zeroValue, Supplier fpgaTimestampSupplier) { + this.fpgaTimestampSupplier = fpgaTimestampSupplier; + this.zeroValue = zeroValue; + this.publisher = topic.publish(); + this.publisher.set(zeroValue, 1); + publishedZeroValue = true; + } + + /** Publishes the values to network tables. */ + public void publish(List> timestampedValues) { + if (timestampedValues.isEmpty()) { + if (!publishedZeroValue) { + long currentTimeMicros = currentTimeMicros(); + long microsSinceLastUpdate = currentTimeMicros - lastUpdateTimeMicros; + if (microsSinceLastUpdate > PUBLISHED_VALUE_VALID_MICROS) { + long timestamp = lastUpdateTimeMicros + EXPECTED_UPDATE_FREQUENCY_MICROS; + publisher.set(zeroValue, timestamp); + publishedZeroValue = true; + } + } + } else { + for (var timestampedValue : timestampedValues) { + long timestampMicros = timestampedValue.networkTablesTimestampMicros(); + lastUpdateTimeMicros = Math.max(lastUpdateTimeMicros, timestampMicros); + publisher.set(timestampedValue.value(), timestampMicros); + } + publishedZeroValue = false; + } + } + + private long currentTimeMicros() { + return (long) (fpgaTimestampSupplier.get() * MICROS_PER_SECOND); + } +} diff --git a/vision/src/main/java/com/team2813/lib2813/vision/TimestampedValue.java b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedValue.java new file mode 100644 index 00000000..f40fe065 --- /dev/null +++ b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedValue.java @@ -0,0 +1,81 @@ +package com.team2813.lib2813.vision; + +import edu.wpi.first.networktables.StructSubscriber; +import edu.wpi.first.networktables.TimestampedObject; +import edu.wpi.first.units.TimeUnit; +import edu.wpi.first.units.Units; +import java.util.Arrays; +import java.util.List; +import java.util.Objects; + +/** + * Holder for a value to publish to network tables with a provided timestamp. + * + * @param type of the value to publish. + */ +final class TimestampedValue { + private final long networkTablesTimestamp; + private final T value; + + public static TimestampedValue withFpgaTimestamp( + double fpgaTimestamp, TimeUnit timeUnit, T value) { + return withFpgaTimestampMicros( + (long) Units.Microseconds.convertFrom(fpgaTimestamp, timeUnit), value); + } + + public static TimestampedValue withFpgaTimestampMicros(long fpgaTimestamp, T value) { + return new TimestampedValue<>(fpgaTimestamp, value); + } + + public static TimestampedValue fromTimestampedObject( + TimestampedObject timestampedObject) { + return withFpgaTimestampMicros(timestampedObject.timestamp, timestampedObject.value); + } + + private TimestampedValue(long fpgaTimestamp, T value) { + this.networkTablesTimestamp = fpgaTimestamp; + this.value = Objects.requireNonNull(value); + } + + static List> fromSubscriberQueue(StructSubscriber subscriber) { + return Arrays.stream(subscriber.readQueue()) + .map(TimestampedValue::fromTimestampedObject) + .toList(); + } + + /** + * Gets the network tables timestamp value for this instance. + * + * @return the FPGA timestamp in microseconds + */ + public long networkTablesTimestampMicros() { + // Note: Per the WPILib documentation at + // https://docs.wpilib.org/en/stable/docs/software/networktables/networktables-intro.html#timestamps + // timestamps in NetworkTables are measured in integer microseconds. When the RoboRIO is the + // NetworkTables server, the server timestamp is the same as the FPGA timestamp returned by + // Timer.getFPGATimestamp() (except the units are different: NetworkTables uses microseconds, + // while getFPGATimestamp() returns seconds). + return networkTablesTimestamp; + } + + public T value() { + return value; + } + + @Override + public boolean equals(Object other) { + if (this == other) { + return true; + } + if (other instanceof TimestampedValue that) { + return networkTablesTimestamp == that.networkTablesTimestamp && value.equals(that.value); + } + + return false; + } + + @Override + public int hashCode() { + return Objects.hash(networkTablesTimestamp, value); + } +} diff --git a/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java b/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java new file mode 100644 index 00000000..9205f82e --- /dev/null +++ b/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java @@ -0,0 +1,63 @@ +package com.team2813.lib2813.vision; + +import static com.team2813.lib2813.vision.CameraConstants.LIMELIGHT_CAMERA_NAME; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import org.photonvision.PhotonCamera; + +/** + * Contains methods and constants for publishing data from robot vision systems to network tables. + */ +final class VisionNetworkTables { + /** Topic name to use when publishing whether a camera has current data. */ + public static final String HAS_DATA_TOPIC = "hasData"; + + /** Topic name to use when publishing positions of detected AprilTags as a Pose3d array. */ + public static final String VISIBLE_APRIL_TAG_POSES_TOPIC = "visibleAprilTagPoses"; + + /** Topic name to use when publishing the Pose3d position of a camera. */ + static final String CAMERA_POSE_TOPIC = "cameraPose"; + + /** Topic name to use when publishing the estimated robot position as a Pose2d value */ + static final String POSE_ESTIMATE_TOPIC = "poseEstimate"; + + /** Topic name to use when publishing the position of the detected AprilTag as a Pose2d value. */ + static final String APRIL_TAG_POSE_TOPIC = "aprilTagPose"; + + private static final String TABLE_NAME = "Vision"; + + /** + * Gets the network table for the camera with the given name + * + *

The key of the network table will be `Vision/[cameraName]`. + * + * @param ntInstance network table instance to publish to. + * @param cameraName name of the camera. + */ + public static NetworkTable getTableForCamera(NetworkTableInstance ntInstance, String cameraName) { + return ntInstance.getTable(TABLE_NAME).getSubTable(cameraName); + } + + /** + * Gets the network table for the provided photon vision camera. + * + *

The key of the network table will be `Vision/[cameraName]`. + */ + public static NetworkTable getTableForCamera(PhotonCamera camera) { + return getTableForCamera(camera.getCameraTable().getInstance(), camera.getName()); + } + + /** + * Gets the network table for the limelight camera. + * + * @param ntInstance network table instance to publish to. + */ + public static NetworkTable getTableForLimelight(NetworkTableInstance ntInstance) { + return getTableForCamera(ntInstance, LIMELIGHT_CAMERA_NAME); + } + + private VisionNetworkTables() { + throw new AssertionError("Not instantiable"); + } +} diff --git a/vision/src/test/java/com/team2813/lib2813/vision/IsolatedNetworkTable.java b/vision/src/test/java/com/team2813/lib2813/vision/IsolatedNetworkTable.java new file mode 100644 index 00000000..82b588a7 --- /dev/null +++ b/vision/src/test/java/com/team2813/lib2813/vision/IsolatedNetworkTable.java @@ -0,0 +1,45 @@ +package com.team2813.lib2813.vision; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.Preferences; +import org.junit.rules.ExternalResource; + +/** + * A JUnit rule that creates a temporary {@link NetworkTableInstance} for each test. + * + *

The rule also updates the {@code NetworkTableInstance} used by {@link Preferences} to use the + * same temporary NetworkTableInstance. + */ +final class IsolatedNetworkTable extends ExternalResource { + private NetworkTableInstance tempInstance; + + /** Gets the temporary {@link NetworkTableInstance}. */ + public NetworkTableInstance getNetworkTableInstance() { + return tempInstance; + } + + /** Gets the {@link NetworkTable} that contains the preference values. */ + public NetworkTable getPreferencesTable() { + return tempInstance.getTable("Preferences"); + } + + @Override + protected void before() { + NetworkTableInstance.getDefault(); + tempInstance = NetworkTableInstance.create(); + tempInstance.startLocal(); + Preferences.setNetworkTableInstance(tempInstance); + } + + @Override + protected void after() { + if (!tempInstance.waitForListenerQueue(.1)) { + System.err.println( + "Timed out waiting for the NetworkTableInstance listener queue to empty (waited 100ms);" + + " JVM may crash"); + } + Preferences.setNetworkTableInstance(NetworkTableInstance.getDefault()); + tempInstance.close(); + } +} diff --git a/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java b/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java new file mode 100644 index 00000000..ef3305fa --- /dev/null +++ b/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java @@ -0,0 +1,188 @@ +package com.team2813.lib2813.vision; + +import static com.google.common.truth.Truth.assertThat; +import static com.team2813.lib2813.vision.TimestampedStructPublisher.EXPECTED_UPDATE_FREQUENCY_MICROS; +import static com.team2813.lib2813.vision.TimestampedStructPublisher.PUBLISHED_VALUE_VALID_MICROS; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.networktables.*; +import edu.wpi.first.units.Units; +import java.util.*; +import java.util.function.Supplier; +import org.junit.Rule; +import org.junit.Test; + +/** Tests for {@link TimestampedStructPublisher}. */ +public class TimestampedStructPublisherTest { + private static final long MICROSECONDS_PER_SECOND = 1_000_000; + private static final Translation2d DEFAULT_VALUE = new Translation2d(28, 13); + private static final String TABLE_NAME = "gearHeads"; + private static final String TOPIC_NAME = "championships"; + + @Rule public final IsolatedNetworkTable isolatedNetworkTable = new IsolatedNetworkTable(); + + private final FakeClock fakeClock = new FakeClock(); + + private TimestampedStructPublisher createPublisher() { + NetworkTable table = isolatedNetworkTable.getNetworkTableInstance().getTable(TABLE_NAME); + return new TimestampedStructPublisher<>( + table.getStructTopic(TOPIC_NAME, Translation2d.struct), Translation2d.kZero, fakeClock); + } + + @Test + public void constructorPublishesZeroValue() { + // Arrange + var topic = getTopic(); + + try (StructSubscriber subscriber = topic.subscribe(DEFAULT_VALUE)) { + // Act + createPublisher(); + + // Assert + List> publishedValues = + TimestampedValue.fromSubscriberQueue(subscriber); + TimestampedValue expectedValue = + TimestampedValue.withFpgaTimestampMicros(1, Translation2d.kZero); + assertThat(publishedValues).containsExactly(expectedValue); + } + } + + @Test + public void publish_withOneValue() { + // Arrange + var topic = getTopic(); + + try (StructSubscriber subscriber = topic.subscribe(DEFAULT_VALUE)) { + TimestampedStructPublisher publisher = createPublisher(); + long firstFpgaTimestampMillis = 25; + Translation2d value = new Translation2d(7.35, 0.708); + TimestampedValue valueToPublish = + TimestampedValue.withFpgaTimestamp(firstFpgaTimestampMillis, Units.Milliseconds, value); + + // Act + publisher.publish(List.of(valueToPublish)); + + // Assert + List> publishedValues = + TimestampedValue.fromSubscriberQueue(subscriber); + var expectedValue = + TimestampedValue.withFpgaTimestampMicros(firstFpgaTimestampMillis * 1_000, value); + assertThat(publishedValues).containsExactly(expectedValue); + } + } + + @Test + public void publish_withManyValues() { + // Arrange + var topic = getTopic(); + + try (StructSubscriber subscriber = + topic.subscribe(DEFAULT_VALUE, PubSubOption.pollStorage(5))) { + TimestampedStructPublisher publisher = createPublisher(); + long firstFpgaTimestampMicros = 25; + + List> valuesToPublish = new ArrayList<>(3); + for (int i = 0; i < 3; i++) { + Translation2d value = new Translation2d(7.35 + i, 0.708); + TimestampedValue valueToPublish = + TimestampedValue.withFpgaTimestampMicros(firstFpgaTimestampMicros + i * 10, value); + valuesToPublish.add(valueToPublish); + } + assertThat(subscriber.readQueue()).hasLength(1); + + // Act + publisher.publish(valuesToPublish); + + // Assert + List> publishedValues = + TimestampedValue.fromSubscriberQueue(subscriber); + + assertThat(publishedValues).containsExactlyElementsIn(valuesToPublish); + } + } + + @Test + public void publish_withEmptyList_withStalePreviousValue() { + // Arrange + var topic = getTopic(); + + try (StructSubscriber subscriber = + topic.subscribe(DEFAULT_VALUE, PubSubOption.pollStorage(5))) { + TimestampedStructPublisher publisher = createPublisher(); + long firstFpgaTimestampMicros = 25; + Translation2d value = new Translation2d(7.35, 0.708); + TimestampedValue valueToPublish = + TimestampedValue.withFpgaTimestampMicros(firstFpgaTimestampMicros, value); + assertThat(subscriber.readQueue()).hasLength(1); // queued by constructor + publisher.publish(List.of(valueToPublish)); + assertThat(subscriber.readQueue()).hasLength(1); + // Advance the clock so that the previously-published data will be considered stale. + fakeClock.setFpgaTimestampMicros(firstFpgaTimestampMicros); + fakeClock.incrementFpgaTimestampMicros(PUBLISHED_VALUE_VALID_MICROS + 1); + + // Act + publisher.publish(List.of()); + + // Assert + List> publishedValues = + TimestampedValue.fromSubscriberQueue(subscriber); + TimestampedValue expectedValue = + TimestampedValue.withFpgaTimestampMicros( + firstFpgaTimestampMicros + EXPECTED_UPDATE_FREQUENCY_MICROS, Translation2d.kZero); + assertThat(publishedValues).containsExactly(expectedValue); + } + } + + @Test + public void publish_withEmptyList_withNonStalePreviousValue() { + // Arrange + var topic = getTopic(); + + try (StructSubscriber subscriber = + topic.subscribe(DEFAULT_VALUE, PubSubOption.pollStorage(5))) { + TimestampedStructPublisher publisher = createPublisher(); + long firstFpgaTimestampMicros = 25; + + Translation2d value = new Translation2d(7.35, 0.708); + TimestampedValue valueToPublish = + TimestampedValue.withFpgaTimestampMicros(firstFpgaTimestampMicros, value); + assertThat(subscriber.readQueue()).hasLength(1); // queued by constructor + publisher.publish(List.of(valueToPublish)); + assertThat(subscriber.readQueue()).hasLength(1); + // Advance the clock, but not as far so that the previously-published data would be considered + // stale. + fakeClock.setFpgaTimestampMicros(firstFpgaTimestampMicros); + fakeClock.incrementFpgaTimestampMicros(PUBLISHED_VALUE_VALID_MICROS - 1); + + // Act + publisher.publish(List.of()); + + // Assert + List> publishedValues = + TimestampedValue.fromSubscriberQueue(subscriber); + assertThat(publishedValues).isEmpty(); + } + } + + private StructTopic getTopic() { + NetworkTable table = isolatedNetworkTable.getNetworkTableInstance().getTable(TABLE_NAME); + return table.getStructTopic(TOPIC_NAME, Translation2d.struct); + } + + private static class FakeClock implements Supplier { + private double fpgaTimestampSeconds = 2.0; + + @Override + public Double get() { + return fpgaTimestampSeconds; + } + + void setFpgaTimestampMicros(double micros) { + fpgaTimestampSeconds = micros / MICROSECONDS_PER_SECOND; + } + + void incrementFpgaTimestampMicros(double micros) { + fpgaTimestampSeconds += micros / MICROSECONDS_PER_SECOND; + } + } +} From 73206bfdc90ac027854c5310d34e36987ff9660b Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Mon, 6 Oct 2025 23:27:56 -0700 Subject: [PATCH 02/13] Improve Javadoc --- .../vision/TimestampedStructPublisher.java | 18 +++++++++++-- .../lib2813/vision/TimestampedValue.java | 27 +++++++++++++++++++ 2 files changed, 43 insertions(+), 2 deletions(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java index 76f67da6..5de44e00 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java @@ -6,7 +6,15 @@ import java.util.List; import java.util.function.Supplier; -/** Publishes timestamped data to a network tables topic. */ +/** + * Publishes timestamped data to a network tables topic. + * + *

If an empty list is passed to the {@link #publish(List)} method, then a zero value is + * published only if the most recent published data is too far in the past. This can reduce + * flickering when the data is displayed in tools like AdvantageScope. It is particularly useful for + * data that takes longer to produce than the frequency of the robot's event loop (for example, + * estimated robot pose data produced by a camera). + */ final class TimestampedStructPublisher { private static final long MICROS_PER_SECOND = 1_000_000; static final long EXPECTED_UPDATE_FREQUENCY_MICROS = @@ -36,7 +44,13 @@ final class TimestampedStructPublisher { publishedZeroValue = true; } - /** Publishes the values to network tables. */ + /** + * Publishes the values to network tables. + * + *

This should be called in a periodic + * method once per loop, even if no data is currently available. + */ public void publish(List> timestampedValues) { if (timestampedValues.isEmpty()) { if (!publishedZeroValue) { diff --git a/vision/src/main/java/com/team2813/lib2813/vision/TimestampedValue.java b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedValue.java index f40fe065..20816733 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/TimestampedValue.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedValue.java @@ -17,16 +17,42 @@ final class TimestampedValue { private final long networkTablesTimestamp; private final T value; + /** + * Creates a value associated with a provided FPGA timestamp. + * + * @param fpgaTimestamp time from the FPGA hardware clock + * @param timeUnit the time unit of the fpgaTimestamp parameter + * @param value the value record or computed at the provided timestamp. + * @return timestamped value with the provided parameters + * @param Type of the value + * @see edu.wpi.first.wpilibj.Timer#getFPGATimestamp() + */ public static TimestampedValue withFpgaTimestamp( double fpgaTimestamp, TimeUnit timeUnit, T value) { return withFpgaTimestampMicros( (long) Units.Microseconds.convertFrom(fpgaTimestamp, timeUnit), value); } + /** + * Creates a value associated with a provided FPGA timestamp in microseconds. + * + * @param fpgaTimestamp time from the FPGA hardware clock, in microseconds + * @param value the value record or computed at the provided timestamp. + * @return timestamped value with the provided parameters + * @param type of the value + * @see edu.wpi.first.wpilibj.Timer#getFPGATimestamp() + */ public static TimestampedValue withFpgaTimestampMicros(long fpgaTimestamp, T value) { return new TimestampedValue<>(fpgaTimestamp, value); } + /** + * Creates a value from a NetworkTables timestamped object. + * + * @param timestampedObject timestampted object to copy from + * @return timestamped value + * @param type of the value + */ public static TimestampedValue fromTimestampedObject( TimestampedObject timestampedObject) { return withFpgaTimestampMicros(timestampedObject.timestamp, timestampedObject.value); @@ -58,6 +84,7 @@ public long networkTablesTimestampMicros() { return networkTablesTimestamp; } + /** Gets the underlying value. */ public T value() { return value; } From 635f00e1f7e164141d093a85c255e1ea89cd0083 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Tue, 7 Oct 2025 20:52:04 -0700 Subject: [PATCH 03/13] Remove unused constants, methods and classes --- .../lib2813/vision/CameraConstants.java | 9 ----- .../lib2813/vision/VisionNetworkTables.java | 37 ++----------------- 2 files changed, 3 insertions(+), 43 deletions(-) delete mode 100644 vision/src/main/java/com/team2813/lib2813/vision/CameraConstants.java diff --git a/vision/src/main/java/com/team2813/lib2813/vision/CameraConstants.java b/vision/src/main/java/com/team2813/lib2813/vision/CameraConstants.java deleted file mode 100644 index f88a811f..00000000 --- a/vision/src/main/java/com/team2813/lib2813/vision/CameraConstants.java +++ /dev/null @@ -1,9 +0,0 @@ -package com.team2813.lib2813.vision; - -class CameraConstants { - public static final String LIMELIGHT_CAMERA_NAME = "limelight"; - - private CameraConstants() { - throw new AssertionError("Not instantiable"); - } -} diff --git a/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java b/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java index 9205f82e..966b28a4 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java @@ -1,7 +1,5 @@ package com.team2813.lib2813.vision; -import static com.team2813.lib2813.vision.CameraConstants.LIMELIGHT_CAMERA_NAME; - import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import org.photonvision.PhotonCamera; @@ -10,15 +8,6 @@ * Contains methods and constants for publishing data from robot vision systems to network tables. */ final class VisionNetworkTables { - /** Topic name to use when publishing whether a camera has current data. */ - public static final String HAS_DATA_TOPIC = "hasData"; - - /** Topic name to use when publishing positions of detected AprilTags as a Pose3d array. */ - public static final String VISIBLE_APRIL_TAG_POSES_TOPIC = "visibleAprilTagPoses"; - - /** Topic name to use when publishing the Pose3d position of a camera. */ - static final String CAMERA_POSE_TOPIC = "cameraPose"; - /** Topic name to use when publishing the estimated robot position as a Pose2d value */ static final String POSE_ESTIMATE_TOPIC = "poseEstimate"; @@ -28,33 +17,13 @@ final class VisionNetworkTables { private static final String TABLE_NAME = "Vision"; /** - * Gets the network table for the camera with the given name - * - *

The key of the network table will be `Vision/[cameraName]`. - * - * @param ntInstance network table instance to publish to. - * @param cameraName name of the camera. - */ - public static NetworkTable getTableForCamera(NetworkTableInstance ntInstance, String cameraName) { - return ntInstance.getTable(TABLE_NAME).getSubTable(cameraName); - } - - /** - * Gets the network table for the provided photon vision camera. + * Gets the network table for the provided photon vision camera to use for publishing data. * *

The key of the network table will be `Vision/[cameraName]`. */ public static NetworkTable getTableForCamera(PhotonCamera camera) { - return getTableForCamera(camera.getCameraTable().getInstance(), camera.getName()); - } - - /** - * Gets the network table for the limelight camera. - * - * @param ntInstance network table instance to publish to. - */ - public static NetworkTable getTableForLimelight(NetworkTableInstance ntInstance) { - return getTableForCamera(ntInstance, LIMELIGHT_CAMERA_NAME); + NetworkTableInstance ntInstance = camera.getCameraTable().getInstance(); + return ntInstance.getTable(TABLE_NAME).getSubTable(camera.getName()); } private VisionNetworkTables() { From 69aaa7618c43d3f844440b86292041705ed0a40f Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 12 Oct 2025 13:45:05 -0700 Subject: [PATCH 04/13] More Javadoc improvements --- .../vision/PhotonVisionPosePublisher.java | 21 +++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java index b7c8dfbc..902602e6 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java @@ -16,13 +16,26 @@ import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; -/** Published timestamped pose estimates from a camera. */ +/** + * Publishes timestamped pose estimates from a camera. + * + *

This is useful for publishing {@link EstimatedRobotPose} values from a PhotonVision camera in + * a way that can be visualized in tools like AdvantageScope without pose locations flickering. + * Estimated robot positions are published to NetworkTables using the timestamp in the {@code + * EstimatedRobotPose}. If no data is available, a position of (0, 0, 0) is published only when the + * previous available value is older than the expected latency of producing vision estimates. + */ public final class PhotonVisionPosePublisher { private final TimestampedStructPublisher publisher; private final TimestampedStructPublisher tagPublisher; private final AprilTagFieldLayout aprilTagFieldLayout; - /** Creates a publisher for the provided camera and field layout. */ + /** + * Creates a publisher for the provided camera and field layout. + * + * @param camera Camera to use to get the Network Tables name to publish to. + * @param aprilTagFieldLayout Layout of AprilTags on a field. + */ public PhotonVisionPosePublisher(PhotonCamera camera, AprilTagFieldLayout aprilTagFieldLayout) { this(camera, aprilTagFieldLayout, Timer::getFPGATimestamp); } @@ -43,6 +56,10 @@ public PhotonVisionPosePublisher(PhotonCamera camera, AprilTagFieldLayout aprilT /** * Publishes the estimated positions to network tables. * + *

This should be called in a periodic + * method once per loop, even if no data is currently available. + * * @param poseEstimates The estimated locations (with the blue driver station as the origin). */ public void publish(List poseEstimates) { From bd411dd2779b6985718e21a99d9506ed48dfa92e Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 12 Oct 2025 13:51:16 -0700 Subject: [PATCH 05/13] Add TimestampedStructPublisher.setTimeUntilStale() --- .../vision/PhotonVisionPosePublisher.java | 8 ++++++++ .../vision/TimestampedStructPublisher.java | 18 +++++++++++++++--- .../vision/TimestampedStructPublisherTest.java | 6 +++--- 3 files changed, 26 insertions(+), 6 deletions(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java index 902602e6..0a9032b0 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java @@ -26,6 +26,12 @@ * previous available value is older than the expected latency of producing vision estimates. */ public final class PhotonVisionPosePublisher { + /** + * How much time we expect to pass between receiving pose estimates from PhotonVision when an + * AprilTag is visible. Empirically, this is 0.1 seconds. + */ + private static final long EXPECTED_MILLIS_BETWEEN_POSE_ESTIMATES = 100; + private final TimestampedStructPublisher publisher; private final TimestampedStructPublisher tagPublisher; private final AprilTagFieldLayout aprilTagFieldLayout; @@ -49,8 +55,10 @@ public PhotonVisionPosePublisher(PhotonCamera camera, AprilTagFieldLayout aprilT NetworkTable table = getTableForCamera(camera); StructTopic topic = table.getStructTopic(POSE_ESTIMATE_TOPIC, Pose3d.struct); publisher = new TimestampedStructPublisher<>(topic, Pose3d.kZero, fpgaTimestampSupplier); + publisher.setTimeUntilStale(EXPECTED_MILLIS_BETWEEN_POSE_ESTIMATES, Units.Milliseconds); topic = table.getStructTopic(APRIL_TAG_POSE_TOPIC, Pose3d.struct); tagPublisher = new TimestampedStructPublisher<>(topic, Pose3d.kZero, fpgaTimestampSupplier); + tagPublisher.setTimeUntilStale(EXPECTED_MILLIS_BETWEEN_POSE_ESTIMATES, Units.Milliseconds); } /** diff --git a/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java index 5de44e00..e369af3b 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java @@ -2,6 +2,8 @@ import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.networktables.StructTopic; +import edu.wpi.first.units.TimeUnit; +import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.TimedRobot; import java.util.List; import java.util.function.Supplier; @@ -19,8 +21,8 @@ final class TimestampedStructPublisher { private static final long MICROS_PER_SECOND = 1_000_000; static final long EXPECTED_UPDATE_FREQUENCY_MICROS = (long) (TimedRobot.kDefaultPeriod * MICROS_PER_SECOND); - // PhotonVision appears to produce estimates every 80ms, so treat values older than 0.1s as stale. - static final long PUBLISHED_VALUE_VALID_MICROS = MICROS_PER_SECOND / 10; + static final long DEFAULT_PUBLISHED_VALUE_VALID_MICROS = 2 * EXPECTED_UPDATE_FREQUENCY_MICROS; + private long publishedValueValidMicros = DEFAULT_PUBLISHED_VALUE_VALID_MICROS; private final StructPublisher publisher; private final Supplier fpgaTimestampSupplier; @@ -44,6 +46,16 @@ final class TimestampedStructPublisher { publishedZeroValue = true; } + /** + * Sets the maximum amount of time that can pass before a published value can be considered stale. + * + * @param time Amount of time. + * @param timeUnit Units for the time parameter. + */ + public void setTimeUntilStale(long time, TimeUnit timeUnit) { + publishedValueValidMicros = (long) Math.floor(Units.Microseconds.convertFrom(time, timeUnit)); + } + /** * Publishes the values to network tables. * @@ -56,7 +68,7 @@ public void publish(List> timestampedValues) { if (!publishedZeroValue) { long currentTimeMicros = currentTimeMicros(); long microsSinceLastUpdate = currentTimeMicros - lastUpdateTimeMicros; - if (microsSinceLastUpdate > PUBLISHED_VALUE_VALID_MICROS) { + if (microsSinceLastUpdate > publishedValueValidMicros) { long timestamp = lastUpdateTimeMicros + EXPECTED_UPDATE_FREQUENCY_MICROS; publisher.set(zeroValue, timestamp); publishedZeroValue = true; diff --git a/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java b/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java index ef3305fa..070c6947 100644 --- a/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java +++ b/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java @@ -1,8 +1,8 @@ package com.team2813.lib2813.vision; import static com.google.common.truth.Truth.assertThat; +import static com.team2813.lib2813.vision.TimestampedStructPublisher.DEFAULT_PUBLISHED_VALUE_VALID_MICROS; import static com.team2813.lib2813.vision.TimestampedStructPublisher.EXPECTED_UPDATE_FREQUENCY_MICROS; -import static com.team2813.lib2813.vision.TimestampedStructPublisher.PUBLISHED_VALUE_VALID_MICROS; import edu.wpi.first.math.geometry.*; import edu.wpi.first.networktables.*; @@ -118,7 +118,7 @@ public void publish_withEmptyList_withStalePreviousValue() { assertThat(subscriber.readQueue()).hasLength(1); // Advance the clock so that the previously-published data will be considered stale. fakeClock.setFpgaTimestampMicros(firstFpgaTimestampMicros); - fakeClock.incrementFpgaTimestampMicros(PUBLISHED_VALUE_VALID_MICROS + 1); + fakeClock.incrementFpgaTimestampMicros(DEFAULT_PUBLISHED_VALUE_VALID_MICROS + 1); // Act publisher.publish(List.of()); @@ -152,7 +152,7 @@ public void publish_withEmptyList_withNonStalePreviousValue() { // Advance the clock, but not as far so that the previously-published data would be considered // stale. fakeClock.setFpgaTimestampMicros(firstFpgaTimestampMicros); - fakeClock.incrementFpgaTimestampMicros(PUBLISHED_VALUE_VALID_MICROS - 1); + fakeClock.incrementFpgaTimestampMicros(DEFAULT_PUBLISHED_VALUE_VALID_MICROS - 1); // Act publisher.publish(List.of()); From c9e364354151e674dff2b14aafbf94aa6d730380 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 12 Oct 2025 13:57:40 -0700 Subject: [PATCH 06/13] Rename fields in PhotonVisionPosePublisher --- .../vision/PhotonVisionPosePublisher.java | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java index 0a9032b0..e868c83c 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java @@ -32,8 +32,8 @@ public final class PhotonVisionPosePublisher { */ private static final long EXPECTED_MILLIS_BETWEEN_POSE_ESTIMATES = 100; - private final TimestampedStructPublisher publisher; - private final TimestampedStructPublisher tagPublisher; + private final TimestampedStructPublisher robotPosePublisher; + private final TimestampedStructPublisher aprilTagPosePublisher; private final AprilTagFieldLayout aprilTagFieldLayout; /** @@ -54,11 +54,15 @@ public PhotonVisionPosePublisher(PhotonCamera camera, AprilTagFieldLayout aprilT this.aprilTagFieldLayout = aprilTagFieldLayout; NetworkTable table = getTableForCamera(camera); StructTopic topic = table.getStructTopic(POSE_ESTIMATE_TOPIC, Pose3d.struct); - publisher = new TimestampedStructPublisher<>(topic, Pose3d.kZero, fpgaTimestampSupplier); - publisher.setTimeUntilStale(EXPECTED_MILLIS_BETWEEN_POSE_ESTIMATES, Units.Milliseconds); + robotPosePublisher = + new TimestampedStructPublisher<>(topic, Pose3d.kZero, fpgaTimestampSupplier); + robotPosePublisher.setTimeUntilStale( + EXPECTED_MILLIS_BETWEEN_POSE_ESTIMATES, Units.Milliseconds); topic = table.getStructTopic(APRIL_TAG_POSE_TOPIC, Pose3d.struct); - tagPublisher = new TimestampedStructPublisher<>(topic, Pose3d.kZero, fpgaTimestampSupplier); - tagPublisher.setTimeUntilStale(EXPECTED_MILLIS_BETWEEN_POSE_ESTIMATES, Units.Milliseconds); + aprilTagPosePublisher = + new TimestampedStructPublisher<>(topic, Pose3d.kZero, fpgaTimestampSupplier); + aprilTagPosePublisher.setTimeUntilStale( + EXPECTED_MILLIS_BETWEEN_POSE_ESTIMATES, Units.Milliseconds); } /** @@ -71,11 +75,11 @@ public PhotonVisionPosePublisher(PhotonCamera camera, AprilTagFieldLayout aprilT * @param poseEstimates The estimated locations (with the blue driver station as the origin). */ public void publish(List poseEstimates) { - publisher.publish( + robotPosePublisher.publish( poseEstimates.stream() .map(PhotonVisionPosePublisher::toRobotPoseTimestampedValue) .toList()); - tagPublisher.publish( + aprilTagPosePublisher.publish( poseEstimates.stream().flatMap(this::toAprilTagPoseTimestampedValue).toList()); } From cc6ba72d88e28a184361d378afd1b8971e4597f0 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 12 Oct 2025 13:57:59 -0700 Subject: [PATCH 07/13] Extract MIN_NETWORK_TABLES_TIMESTAMP --- .../lib2813/vision/TimestampedStructPublisher.java | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java index e369af3b..94135043 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java @@ -19,6 +19,13 @@ */ final class TimestampedStructPublisher { private static final long MICROS_PER_SECOND = 1_000_000; + + /** + * Minimum "real" value that can be passed to StructPublisher.set() (per the Javadoc, "0 indicates + * current NT time should be used") + */ + private static final long MIN_NETWORK_TABLES_TIMESTAMP = 1; + static final long EXPECTED_UPDATE_FREQUENCY_MICROS = (long) (TimedRobot.kDefaultPeriod * MICROS_PER_SECOND); static final long DEFAULT_PUBLISHED_VALUE_VALID_MICROS = 2 * EXPECTED_UPDATE_FREQUENCY_MICROS; @@ -42,7 +49,7 @@ final class TimestampedStructPublisher { this.fpgaTimestampSupplier = fpgaTimestampSupplier; this.zeroValue = zeroValue; this.publisher = topic.publish(); - this.publisher.set(zeroValue, 1); + this.publisher.set(zeroValue, MIN_NETWORK_TABLES_TIMESTAMP); publishedZeroValue = true; } From 661d4285467f3f90248b57b65a47f58bbaf03b7b Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 12 Oct 2025 14:08:20 -0700 Subject: [PATCH 08/13] Extract local in toAprilTagPoseTimestampedValue() --- .../lib2813/vision/PhotonVisionPosePublisher.java | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java index e868c83c..ff58af08 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java @@ -11,6 +11,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.wpilibj.Timer; import java.util.List; +import java.util.Optional; import java.util.function.Supplier; import java.util.stream.Stream; import org.photonvision.EstimatedRobotPose; @@ -92,8 +93,12 @@ private Stream> toAprilTagPoseTimestampedValue(Estimate if (pose.targetsUsed.isEmpty()) { return Stream.empty(); } - return aprilTagFieldLayout - .getTagPose(pose.targetsUsed.get(0).fiducialId) + + // Find the pose of the "best" AprilTag used for pose estimation and convert it to a timestamped + // value. + Optional poseOfBestAprilTag = + aprilTagFieldLayout.getTagPose(pose.targetsUsed.get(0).fiducialId); + return poseOfBestAprilTag .map( tagPose -> TimestampedValue.withFpgaTimestamp(pose.timestampSeconds, Units.Seconds, tagPose)) From 5caa2b2a7eec03afd64685fc4093b70930791a37 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 12 Oct 2025 14:17:25 -0700 Subject: [PATCH 09/13] Rename fromSubscriberQueue() to readQueue() --- .../team2813/lib2813/vision/TimestampedValue.java | 12 +++++++++++- .../vision/TimestampedStructPublisherTest.java | 10 +++++----- 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/TimestampedValue.java b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedValue.java index 20816733..0bcfca2e 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/TimestampedValue.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedValue.java @@ -63,7 +63,17 @@ private TimestampedValue(long fpgaTimestamp, T value) { this.value = Objects.requireNonNull(value); } - static List> fromSubscriberQueue(StructSubscriber subscriber) { + /** + * Reads all valid value changes since the last call to {@code readQueue()}. + * + *

This is a convenience method for use in tests. It calls {@link StructSubscriber#readQueue()} + * and converts the values to {@code TimestampedValue} values. + * + * @param subscriber NetworkTables struct-encoded value subscriber to read from. + * @return Timestamped values; empty if no valid new changes have been published since the + * previous call. + */ + public static List> readQueue(StructSubscriber subscriber) { return Arrays.stream(subscriber.readQueue()) .map(TimestampedValue::fromTimestampedObject) .toList(); diff --git a/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java b/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java index 070c6947..71217182 100644 --- a/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java +++ b/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java @@ -40,7 +40,7 @@ public void constructorPublishesZeroValue() { // Assert List> publishedValues = - TimestampedValue.fromSubscriberQueue(subscriber); + TimestampedValue.readQueue(subscriber); TimestampedValue expectedValue = TimestampedValue.withFpgaTimestampMicros(1, Translation2d.kZero); assertThat(publishedValues).containsExactly(expectedValue); @@ -64,7 +64,7 @@ public void publish_withOneValue() { // Assert List> publishedValues = - TimestampedValue.fromSubscriberQueue(subscriber); + TimestampedValue.readQueue(subscriber); var expectedValue = TimestampedValue.withFpgaTimestampMicros(firstFpgaTimestampMillis * 1_000, value); assertThat(publishedValues).containsExactly(expectedValue); @@ -95,7 +95,7 @@ public void publish_withManyValues() { // Assert List> publishedValues = - TimestampedValue.fromSubscriberQueue(subscriber); + TimestampedValue.readQueue(subscriber); assertThat(publishedValues).containsExactlyElementsIn(valuesToPublish); } @@ -125,7 +125,7 @@ public void publish_withEmptyList_withStalePreviousValue() { // Assert List> publishedValues = - TimestampedValue.fromSubscriberQueue(subscriber); + TimestampedValue.readQueue(subscriber); TimestampedValue expectedValue = TimestampedValue.withFpgaTimestampMicros( firstFpgaTimestampMicros + EXPECTED_UPDATE_FREQUENCY_MICROS, Translation2d.kZero); @@ -159,7 +159,7 @@ public void publish_withEmptyList_withNonStalePreviousValue() { // Assert List> publishedValues = - TimestampedValue.fromSubscriberQueue(subscriber); + TimestampedValue.readQueue(subscriber); assertThat(publishedValues).isEmpty(); } } From 6668afd474ebb838fc39eac071f55d092a303f1b Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sun, 12 Oct 2025 20:34:23 -0700 Subject: [PATCH 10/13] Migrate tests to JUnit Jupiter --- vision/build.gradle | 9 ++-- .../lib2813/vision/IsolatedNetworkTable.java | 45 ------------------ .../TimestampedStructPublisherTest.java | 47 ++++++++++--------- 3 files changed, 30 insertions(+), 71 deletions(-) delete mode 100644 vision/src/test/java/com/team2813/lib2813/vision/IsolatedNetworkTable.java diff --git a/vision/build.gradle b/vision/build.gradle index 27137dc9..f8bcc45f 100644 --- a/vision/build.gradle +++ b/vision/build.gradle @@ -18,9 +18,12 @@ dependencies { implementation 'org.photonvision:photontargeting-java:v2025.3.2' implementation project(':lib') - testImplementation 'junit:junit:4.13.2' + testImplementation(platform('org.junit:junit-bom:5.13.1')) + testImplementation('org.junit.jupiter:junit-jupiter') testImplementation 'com.google.truth:truth:1.4.4' testImplementation project(':testing') + testRuntimeOnly('org.junit.platform:junit-platform-launcher') + testRuntimeOnly('org.junit.vintage:junit-vintage-engine') nativeDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.desktop) nativeDebug wpi.java.vendor.jniDebug(wpi.platforms.desktop) @@ -34,6 +37,6 @@ dependencies { wpi.java.configureTestTasks(test) test { - // Use JUnit 4 for tests - useJUnit() + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' } diff --git a/vision/src/test/java/com/team2813/lib2813/vision/IsolatedNetworkTable.java b/vision/src/test/java/com/team2813/lib2813/vision/IsolatedNetworkTable.java deleted file mode 100644 index 82b588a7..00000000 --- a/vision/src/test/java/com/team2813/lib2813/vision/IsolatedNetworkTable.java +++ /dev/null @@ -1,45 +0,0 @@ -package com.team2813.lib2813.vision; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.Preferences; -import org.junit.rules.ExternalResource; - -/** - * A JUnit rule that creates a temporary {@link NetworkTableInstance} for each test. - * - *

The rule also updates the {@code NetworkTableInstance} used by {@link Preferences} to use the - * same temporary NetworkTableInstance. - */ -final class IsolatedNetworkTable extends ExternalResource { - private NetworkTableInstance tempInstance; - - /** Gets the temporary {@link NetworkTableInstance}. */ - public NetworkTableInstance getNetworkTableInstance() { - return tempInstance; - } - - /** Gets the {@link NetworkTable} that contains the preference values. */ - public NetworkTable getPreferencesTable() { - return tempInstance.getTable("Preferences"); - } - - @Override - protected void before() { - NetworkTableInstance.getDefault(); - tempInstance = NetworkTableInstance.create(); - tempInstance.startLocal(); - Preferences.setNetworkTableInstance(tempInstance); - } - - @Override - protected void after() { - if (!tempInstance.waitForListenerQueue(.1)) { - System.err.println( - "Timed out waiting for the NetworkTableInstance listener queue to empty (waited 100ms);" - + " JVM may crash"); - } - Preferences.setNetworkTableInstance(NetworkTableInstance.getDefault()); - tempInstance.close(); - } -} diff --git a/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java b/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java index 71217182..8a1e52e1 100644 --- a/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java +++ b/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java @@ -4,39 +4,40 @@ import static com.team2813.lib2813.vision.TimestampedStructPublisher.DEFAULT_PUBLISHED_VALUE_VALID_MICROS; import static com.team2813.lib2813.vision.TimestampedStructPublisher.EXPECTED_UPDATE_FREQUENCY_MICROS; +import com.team2813.lib2813.testing.junit.jupiter.IsolatedNetworkTablesExtension; import edu.wpi.first.math.geometry.*; import edu.wpi.first.networktables.*; import edu.wpi.first.units.Units; import java.util.*; import java.util.function.Supplier; -import org.junit.Rule; -import org.junit.Test; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.extension.ExtendWith; /** Tests for {@link TimestampedStructPublisher}. */ +@ExtendWith(IsolatedNetworkTablesExtension.class) public class TimestampedStructPublisherTest { private static final long MICROSECONDS_PER_SECOND = 1_000_000; private static final Translation2d DEFAULT_VALUE = new Translation2d(28, 13); private static final String TABLE_NAME = "gearHeads"; private static final String TOPIC_NAME = "championships"; - @Rule public final IsolatedNetworkTable isolatedNetworkTable = new IsolatedNetworkTable(); - private final FakeClock fakeClock = new FakeClock(); - private TimestampedStructPublisher createPublisher() { - NetworkTable table = isolatedNetworkTable.getNetworkTableInstance().getTable(TABLE_NAME); + private TimestampedStructPublisher createPublisher( + NetworkTableInstance ntInstance) { + NetworkTable table = ntInstance.getTable(TABLE_NAME); return new TimestampedStructPublisher<>( table.getStructTopic(TOPIC_NAME, Translation2d.struct), Translation2d.kZero, fakeClock); } @Test - public void constructorPublishesZeroValue() { + public void constructorPublishesZeroValue(NetworkTableInstance ntInstance) { // Arrange - var topic = getTopic(); + var topic = getTopic(ntInstance); try (StructSubscriber subscriber = topic.subscribe(DEFAULT_VALUE)) { // Act - createPublisher(); + createPublisher(ntInstance); // Assert List> publishedValues = @@ -48,12 +49,12 @@ public void constructorPublishesZeroValue() { } @Test - public void publish_withOneValue() { + public void publish_withOneValue(NetworkTableInstance ntInstance) { // Arrange - var topic = getTopic(); + var topic = getTopic(ntInstance); try (StructSubscriber subscriber = topic.subscribe(DEFAULT_VALUE)) { - TimestampedStructPublisher publisher = createPublisher(); + TimestampedStructPublisher publisher = createPublisher(ntInstance); long firstFpgaTimestampMillis = 25; Translation2d value = new Translation2d(7.35, 0.708); TimestampedValue valueToPublish = @@ -72,13 +73,13 @@ public void publish_withOneValue() { } @Test - public void publish_withManyValues() { + public void publish_withManyValues(NetworkTableInstance ntInstance) { // Arrange - var topic = getTopic(); + var topic = getTopic(ntInstance); try (StructSubscriber subscriber = topic.subscribe(DEFAULT_VALUE, PubSubOption.pollStorage(5))) { - TimestampedStructPublisher publisher = createPublisher(); + TimestampedStructPublisher publisher = createPublisher(ntInstance); long firstFpgaTimestampMicros = 25; List> valuesToPublish = new ArrayList<>(3); @@ -102,13 +103,13 @@ public void publish_withManyValues() { } @Test - public void publish_withEmptyList_withStalePreviousValue() { + public void publish_withEmptyList_withStalePreviousValue(NetworkTableInstance ntInstance) { // Arrange - var topic = getTopic(); + var topic = getTopic(ntInstance); try (StructSubscriber subscriber = topic.subscribe(DEFAULT_VALUE, PubSubOption.pollStorage(5))) { - TimestampedStructPublisher publisher = createPublisher(); + TimestampedStructPublisher publisher = createPublisher(ntInstance); long firstFpgaTimestampMicros = 25; Translation2d value = new Translation2d(7.35, 0.708); TimestampedValue valueToPublish = @@ -134,13 +135,13 @@ public void publish_withEmptyList_withStalePreviousValue() { } @Test - public void publish_withEmptyList_withNonStalePreviousValue() { + public void publish_withEmptyList_withNonStalePreviousValue(NetworkTableInstance ntInstance) { // Arrange - var topic = getTopic(); + var topic = getTopic(ntInstance); try (StructSubscriber subscriber = topic.subscribe(DEFAULT_VALUE, PubSubOption.pollStorage(5))) { - TimestampedStructPublisher publisher = createPublisher(); + TimestampedStructPublisher publisher = createPublisher(ntInstance); long firstFpgaTimestampMicros = 25; Translation2d value = new Translation2d(7.35, 0.708); @@ -164,8 +165,8 @@ public void publish_withEmptyList_withNonStalePreviousValue() { } } - private StructTopic getTopic() { - NetworkTable table = isolatedNetworkTable.getNetworkTableInstance().getTable(TABLE_NAME); + private StructTopic getTopic(NetworkTableInstance ntInstance) { + NetworkTable table = ntInstance.getTable(TABLE_NAME); return table.getStructTopic(TOPIC_NAME, Translation2d.struct); } From 391b9a021f66caf8974c37ff7a5b76f954391854 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Mon, 13 Oct 2025 00:37:26 -0700 Subject: [PATCH 11/13] Rename private methods and locals; extract locals; add comments --- .../vision/PhotonVisionPosePublisher.java | 48 +++++++++++-------- 1 file changed, 29 insertions(+), 19 deletions(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java index ff58af08..2fab4c58 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java @@ -13,9 +13,9 @@ import java.util.List; import java.util.Optional; import java.util.function.Supplier; -import java.util.stream.Stream; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonTrackedTarget; /** * Publishes timestamped pose estimates from a camera. @@ -76,32 +76,42 @@ public PhotonVisionPosePublisher(PhotonCamera camera, AprilTagFieldLayout aprilT * @param poseEstimates The estimated locations (with the blue driver station as the origin). */ public void publish(List poseEstimates) { - robotPosePublisher.publish( + // Publish all the estimated robot positions. + List> robotPoses = + poseEstimates.stream().map(this::getRobotPoseFromEstimatedRobotPose).toList(); + robotPosePublisher.publish(robotPoses); + + // Publish the location of the AprilTags used for the above estimated positions. + List> aprilTagPoses = poseEstimates.stream() - .map(PhotonVisionPosePublisher::toRobotPoseTimestampedValue) - .toList()); - aprilTagPosePublisher.publish( - poseEstimates.stream().flatMap(this::toAprilTagPoseTimestampedValue).toList()); + .map(this::getBestVisibleAprilTag) + .flatMap(Optional::stream) // Convert Stream> -> Stream + .toList(); + aprilTagPosePublisher.publish(aprilTagPoses); } - private static TimestampedValue toRobotPoseTimestampedValue(EstimatedRobotPose pose) { + /** Gets the robot pose from the EstimatedRobotPose and converts it to a timestamped value. */ + private TimestampedValue getRobotPoseFromEstimatedRobotPose( + EstimatedRobotPose estimatedRobotPose) { return TimestampedValue.withFpgaTimestamp( - pose.timestampSeconds, Units.Seconds, pose.estimatedPose); + estimatedRobotPose.timestampSeconds, Units.Seconds, estimatedRobotPose.estimatedPose); } - private Stream> toAprilTagPoseTimestampedValue(EstimatedRobotPose pose) { - if (pose.targetsUsed.isEmpty()) { - return Stream.empty(); + /** Gets the highest-quality AprilTag used to estimate the position of the robot. */ + private Optional> getBestVisibleAprilTag( + EstimatedRobotPose estimatedRobotPose) { + List visibleAprilTags = estimatedRobotPose.targetsUsed; + if (visibleAprilTags.isEmpty()) { + // Not sure how we would have a pose without a target visible, but best to avoid the + // IndexOutOfBoundsException that get(0) would throw. + return Optional.empty(); } - // Find the pose of the "best" AprilTag used for pose estimation and convert it to a timestamped - // value. Optional poseOfBestAprilTag = - aprilTagFieldLayout.getTagPose(pose.targetsUsed.get(0).fiducialId); - return poseOfBestAprilTag - .map( - tagPose -> - TimestampedValue.withFpgaTimestamp(pose.timestampSeconds, Units.Seconds, tagPose)) - .stream(); + aprilTagFieldLayout.getTagPose(visibleAprilTags.get(0).fiducialId); + return poseOfBestAprilTag.map( + aprilTagPose -> + TimestampedValue.withFpgaTimestamp( + estimatedRobotPose.timestampSeconds, Units.Seconds, aprilTagPose)); } } From 3b8b6086aca08c88880d74088594173fea29eb2b Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Tue, 14 Oct 2025 21:31:59 -0700 Subject: [PATCH 12/13] Add javadoc arguments to allow linking to the PhotonVision javadoc --- buildSrc/src/main/groovy/java-common-conventions.gradle | 2 ++ 1 file changed, 2 insertions(+) diff --git a/buildSrc/src/main/groovy/java-common-conventions.gradle b/buildSrc/src/main/groovy/java-common-conventions.gradle index eedb6e45..6c0dab93 100644 --- a/buildSrc/src/main/groovy/java-common-conventions.gradle +++ b/buildSrc/src/main/groovy/java-common-conventions.gradle @@ -115,5 +115,7 @@ javadoc { links += "https://github.wpilib.org/allwpilib/docs/2027/java/" // REVLib links += "https://codedocs.revrobotics.com/java/" + // PhotonVision + links += "https://javadocs.photonvision.org/release/" } } From 508b555edbc7032526b7a90f147c300a6aa99341 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sat, 18 Oct 2025 18:21:27 -0700 Subject: [PATCH 13/13] Make quoting in vision/build.gradle consistent --- vision/build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/vision/build.gradle b/vision/build.gradle index f8bcc45f..9d400bfa 100644 --- a/vision/build.gradle +++ b/vision/build.gradle @@ -1,6 +1,6 @@ plugins { id 'java-common-conventions' - id "edu.wpi.first.GradleRIO" version "2025.1.1" + id 'edu.wpi.first.GradleRIO' version '2025.1.1' id 'idea' }