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/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/" } } 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..9d400bfa --- /dev/null +++ b/vision/build.gradle @@ -0,0 +1,42 @@ +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(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) + 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 { + useJUnitPlatform() + systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true' +} 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..2fab4c58 --- /dev/null +++ b/vision/src/main/java/com/team2813/lib2813/vision/PhotonVisionPosePublisher.java @@ -0,0 +1,117 @@ +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.Optional; +import java.util.function.Supplier; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonTrackedTarget; + +/** + * 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 { + /** + * 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 robotPosePublisher; + private final TimestampedStructPublisher aprilTagPosePublisher; + private final AprilTagFieldLayout aprilTagFieldLayout; + + /** + * 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); + } + + /** 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); + 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); + aprilTagPosePublisher = + new TimestampedStructPublisher<>(topic, Pose3d.kZero, fpgaTimestampSupplier); + aprilTagPosePublisher.setTimeUntilStale( + EXPECTED_MILLIS_BETWEEN_POSE_ESTIMATES, Units.Milliseconds); + } + + /** + * 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) { + // 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(this::getBestVisibleAprilTag) + .flatMap(Optional::stream) // Convert Stream> -> Stream + .toList(); + aprilTagPosePublisher.publish(aprilTagPoses); + } + + /** Gets the robot pose from the EstimatedRobotPose and converts it to a timestamped value. */ + private TimestampedValue getRobotPoseFromEstimatedRobotPose( + EstimatedRobotPose estimatedRobotPose) { + return TimestampedValue.withFpgaTimestamp( + estimatedRobotPose.timestampSeconds, Units.Seconds, estimatedRobotPose.estimatedPose); + } + + /** 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(); + } + + Optional poseOfBestAprilTag = + aprilTagFieldLayout.getTagPose(visibleAprilTags.get(0).fiducialId); + return poseOfBestAprilTag.map( + aprilTagPose -> + TimestampedValue.withFpgaTimestamp( + estimatedRobotPose.timestampSeconds, Units.Seconds, aprilTagPose)); + } +} 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..94135043 --- /dev/null +++ b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedStructPublisher.java @@ -0,0 +1,97 @@ +package com.team2813.lib2813.vision; + +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; + +/** + * 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; + + /** + * 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; + private long publishedValueValidMicros = DEFAULT_PUBLISHED_VALUE_VALID_MICROS; + + 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, MIN_NETWORK_TABLES_TIMESTAMP); + 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. + * + *

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) { + long currentTimeMicros = currentTimeMicros(); + long microsSinceLastUpdate = currentTimeMicros - lastUpdateTimeMicros; + if (microsSinceLastUpdate > publishedValueValidMicros) { + 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..0bcfca2e --- /dev/null +++ b/vision/src/main/java/com/team2813/lib2813/vision/TimestampedValue.java @@ -0,0 +1,118 @@ +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; + + /** + * 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); + } + + private TimestampedValue(long fpgaTimestamp, T value) { + this.networkTablesTimestamp = fpgaTimestamp; + this.value = Objects.requireNonNull(value); + } + + /** + * 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(); + } + + /** + * 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; + } + + /** Gets the underlying value. */ + 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..966b28a4 --- /dev/null +++ b/vision/src/main/java/com/team2813/lib2813/vision/VisionNetworkTables.java @@ -0,0 +1,32 @@ +package com.team2813.lib2813.vision; + +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 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 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) { + NetworkTableInstance ntInstance = camera.getCameraTable().getInstance(); + return ntInstance.getTable(TABLE_NAME).getSubTable(camera.getName()); + } + + private VisionNetworkTables() { + throw new AssertionError("Not instantiable"); + } +} 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..8a1e52e1 --- /dev/null +++ b/vision/src/test/java/com/team2813/lib2813/vision/TimestampedStructPublisherTest.java @@ -0,0 +1,189 @@ +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 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.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"; + + private final FakeClock fakeClock = new FakeClock(); + + 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(NetworkTableInstance ntInstance) { + // Arrange + var topic = getTopic(ntInstance); + + try (StructSubscriber subscriber = topic.subscribe(DEFAULT_VALUE)) { + // Act + createPublisher(ntInstance); + + // Assert + List> publishedValues = + TimestampedValue.readQueue(subscriber); + TimestampedValue expectedValue = + TimestampedValue.withFpgaTimestampMicros(1, Translation2d.kZero); + assertThat(publishedValues).containsExactly(expectedValue); + } + } + + @Test + public void publish_withOneValue(NetworkTableInstance ntInstance) { + // Arrange + var topic = getTopic(ntInstance); + + try (StructSubscriber subscriber = topic.subscribe(DEFAULT_VALUE)) { + TimestampedStructPublisher publisher = createPublisher(ntInstance); + 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.readQueue(subscriber); + var expectedValue = + TimestampedValue.withFpgaTimestampMicros(firstFpgaTimestampMillis * 1_000, value); + assertThat(publishedValues).containsExactly(expectedValue); + } + } + + @Test + public void publish_withManyValues(NetworkTableInstance ntInstance) { + // Arrange + var topic = getTopic(ntInstance); + + try (StructSubscriber subscriber = + topic.subscribe(DEFAULT_VALUE, PubSubOption.pollStorage(5))) { + TimestampedStructPublisher publisher = createPublisher(ntInstance); + 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.readQueue(subscriber); + + assertThat(publishedValues).containsExactlyElementsIn(valuesToPublish); + } + } + + @Test + public void publish_withEmptyList_withStalePreviousValue(NetworkTableInstance ntInstance) { + // Arrange + var topic = getTopic(ntInstance); + + try (StructSubscriber subscriber = + topic.subscribe(DEFAULT_VALUE, PubSubOption.pollStorage(5))) { + TimestampedStructPublisher publisher = createPublisher(ntInstance); + 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(DEFAULT_PUBLISHED_VALUE_VALID_MICROS + 1); + + // Act + publisher.publish(List.of()); + + // Assert + List> publishedValues = + TimestampedValue.readQueue(subscriber); + TimestampedValue expectedValue = + TimestampedValue.withFpgaTimestampMicros( + firstFpgaTimestampMicros + EXPECTED_UPDATE_FREQUENCY_MICROS, Translation2d.kZero); + assertThat(publishedValues).containsExactly(expectedValue); + } + } + + @Test + public void publish_withEmptyList_withNonStalePreviousValue(NetworkTableInstance ntInstance) { + // Arrange + var topic = getTopic(ntInstance); + + try (StructSubscriber subscriber = + topic.subscribe(DEFAULT_VALUE, PubSubOption.pollStorage(5))) { + TimestampedStructPublisher publisher = createPublisher(ntInstance); + 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(DEFAULT_PUBLISHED_VALUE_VALID_MICROS - 1); + + // Act + publisher.publish(List.of()); + + // Assert + List> publishedValues = + TimestampedValue.readQueue(subscriber); + assertThat(publishedValues).isEmpty(); + } + } + + private StructTopic getTopic(NetworkTableInstance ntInstance) { + NetworkTable table = ntInstance.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; + } + } +}