Add vision project and PhotonVisionPosePublisher (from Robot2025)

build.gradle

@@ -13,5 +13,9 @@ allprojects {
             // REV Robotics
             url 'https://maven.revrobotics.com/'
         }
+        maven {
+            name 'photonvisionRepositoryRepository'
+            url 'https://maven.photonvision.org/releases'
+        }
     }
 }

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/"
     }
 }

settings.gradle

@@ -10,4 +10,5 @@
 rootProject.name = 'lib2813'
 include('lib')
 include('limelight')
+include('vision')
 include('testing')

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'
+}

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.
+ *
+ * <p>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<Pose3d> robotPosePublisher;
+  private final TimestampedStructPublisher<Pose3d> 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<Double> fpgaTimestampSupplier) {
+    this.aprilTagFieldLayout = aprilTagFieldLayout;
+    NetworkTable table = getTableForCamera(camera);
+    StructTopic<Pose3d> 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.
+   *
+   * <p>This should be called in a <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/convenience-features/scheduling-functions.html">periodic
+   * method</a> 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<EstimatedRobotPose> poseEstimates) {
+    // Publish all the estimated robot positions.
+    List<TimestampedValue<Pose3d>> robotPoses =
+        poseEstimates.stream().map(this::getRobotPoseFromEstimatedRobotPose).toList();
+    robotPosePublisher.publish(robotPoses);
+
+    // Publish the location of the AprilTags used for the above estimated positions.
+    List<TimestampedValue<Pose3d>> aprilTagPoses =
+        poseEstimates.stream()
+            .map(this::getBestVisibleAprilTag)
+            .flatMap(Optional::stream) // Convert Stream<Optional<V>> -> Stream<V>
+            .toList();
+    aprilTagPosePublisher.publish(aprilTagPoses);
+  }
+
+  /** Gets the robot pose from the EstimatedRobotPose and converts it to a timestamped value. */
+  private TimestampedValue<Pose3d> 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<TimestampedValue<Pose3d>> getBestVisibleAprilTag(
+      EstimatedRobotPose estimatedRobotPose) {
+    List<PhotonTrackedTarget> 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<Pose3d> poseOfBestAprilTag =
+        aprilTagFieldLayout.getTagPose(visibleAprilTags.get(0).fiducialId);
+    return poseOfBestAprilTag.map(
+        aprilTagPose ->
+            TimestampedValue.withFpgaTimestamp(
+                estimatedRobotPose.timestampSeconds, Units.Seconds, aprilTagPose));
+  }
+}

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.
+ *
+ * <p>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<S> {
+  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<S> publisher;
+  private final Supplier<Double> 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<S> topic, S zeroValue, Supplier<Double> 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.
+   *
+   * <p>This should be called in a <a
+   * href="https://docs.wpilib.org/en/stable/docs/software/convenience-features/scheduling-functions.html">periodic
+   * method</a> once per loop, even if no data is currently available.
+   */
+  public void publish(List<TimestampedValue<S>> 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);
+  }
+}