Use wpi::Now for image capture timestamp (#232)

This uses the same time source as CSCore does for image captures, and will make latency measurements more accurate.

Co-authored-by: Banks T <btrout.dhrs@gmail.com>

build.gradle

@@ -12,6 +12,9 @@ allprojects {
         maven {
             url = 'https://frcmaven.wpi.edu:443/artifactory/development'
         }
+        maven {
+            url = 'https://frcmaven.wpi.edu:443/artifactory/release'
+        }
     }
 }
 

common.gradle

@@ -6,7 +6,7 @@ sourceCompatibility = 11
 
 // Common version config
 ext {
-    wpilibVersion = '2020.3.2-99-g9f4de91'
+    wpilibVersion = '2021.1.2'
     joglVersion = '2.4.0-rc-20200307'
     openCVVersion = '3.4.7-2'
 }
@@ -90,4 +90,4 @@ jacocoTestReport {
             )
         }))
     }
-}
+}

photon-client/src/views/PipelineView.vue

@@ -34,7 +34,7 @@
                 :text-color="fpsTooLow ? 'white' : 'grey'"
               >
                 <span class="pr-1">{{ Math.round($store.state.pipelineResults.fps) }}&nbsp;FPS &ndash;</span>
-                <span v-if="!fpsTooLow">{{ Math.round($store.state.pipelineResults.latency) }} ms latency</span>
+                <span v-if="!fpsTooLow">{{ Math.min(Math.round($store.state.pipelineResults.latency), 100) }} ms latency</span>
                 <span v-else-if="!$store.getters.currentPipelineSettings.inputShouldShow">HSV thresholds are too broad; narrow them for better performance</span>
                 <span v-else>stop viewing the color stream for better performance</span>
               </v-chip>

photon-core/build.gradle

@@ -12,6 +12,12 @@ dependencies {
     implementation 'org.msgpack:msgpack-core:0.8.20'
     implementation 'org.msgpack:jackson-dataformat-msgpack:0.8.20'
 
+    // Wpiutil
+    compile "edu.wpi.first.wpiutil:wpiutil-jni:$wpilibVersion:linuxaarch64bionic"
+    compile "edu.wpi.first.wpiutil:wpiutil-jni:$wpilibVersion:linuxraspbian"
+    compile "edu.wpi.first.wpiutil:wpiutil-jni:$wpilibVersion:linuxx86-64"
+    compile "edu.wpi.first.wpiutil:wpiutil-jni:$wpilibVersion:windowsx86-64"
+
     // JOGL stuff (currently we only distribute for aarch64, which is Pi 4)
     implementation "org.jogamp.gluegen:gluegen-rt:$joglVersion"
     implementation "org.jogamp.jogl:jogl-all:$joglVersion"

photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java

@@ -38,16 +38,17 @@ public class UIDataPublisher implements CVPipelineResultConsumer {
 
     @Override
     public void accept(CVPipelineResult result) {
-        var now = System.currentTimeMillis();
+        long now = System.currentTimeMillis();
+
+        var dataMap = new HashMap<String, Object>();
+        dataMap.put("latency", result.getLatencyMillis());
 
         // only update the UI at 15hz
         if (lastUIResultUpdateTime + 1000.0 / 10.0 > now) return;
 
         var uiMap = new HashMap<Integer, HashMap<String, Object>>();
-        var dataMap = new HashMap<String, Object>();
 
         dataMap.put("fps", result.fps);
-        dataMap.put("latency", result.getLatencyMillis());
 
         var targets = result.targets;
 

photon-core/src/main/java/org/photonvision/common/util/math/MathUtils.java

@@ -17,6 +17,7 @@
 
 package org.photonvision.common.util.math;
 
+import edu.wpi.first.wpiutil.WPIUtilJNI;
 import org.apache.commons.math3.util.FastMath;
 
 public class MathUtils {
@@ -72,4 +73,8 @@ public class MathUtils {
     public static double lerp(double startValue, double endValue, double t) {
         return startValue + (endValue - startValue) * t;
     }
+
+    public static long wpiNanoTime() {
+        return microsToNanos(WPIUtilJNI.now());
+    }
 }

photon-core/src/main/java/org/photonvision/vision/frame/Frame.java

@@ -18,6 +18,7 @@
 package org.photonvision.vision.frame;
 
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.opencv.Releasable;
 
@@ -33,13 +34,14 @@ public class Frame implements Releasable {
     }
 
     public Frame(CVMat image, FrameStaticProperties frameStaticProperties) {
-        this(image, System.nanoTime(), frameStaticProperties);
+        this(image, MathUtils.wpiNanoTime(), frameStaticProperties);
     }
 
     public Frame() {
-        timestampNanos = 0;
-        image = new CVMat();
-        frameStaticProperties = new FrameStaticProperties(0, 0, 0, new Rotation2d(), null);
+        this(
+                new CVMat(),
+                MathUtils.wpiNanoTime(),
+                new FrameStaticProperties(0, 0, 0, new Rotation2d(), null));
     }
 
     public void copyTo(Frame destFrame) {

photon-core/src/main/java/org/photonvision/vision/frame/provider/AcceleratedPicamFrameProvider.java

@@ -18,6 +18,7 @@
 package org.photonvision.vision.frame.provider;
 
 import org.opencv.core.Mat;
+import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.raspi.PicamJNI;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameProvider;
@@ -47,6 +48,8 @@ public class AcceleratedPicamFrameProvider implements FrameProvider {
         long matHandle = PicamJNI.grabFrame(false);
         mat = new CVMat(new Mat(matHandle));
         return new Frame(
-                mat, System.nanoTime() - PicamJNI.getFrameLatency(), settables.getFrameStaticProperties());
+                mat,
+                MathUtils.wpiNanoTime() - PicamJNI.getFrameLatency(),
+                settables.getFrameStaticProperties());
     }
 }

photon-core/src/main/java/org/photonvision/vision/frame/provider/USBFrameProvider.java

@@ -25,9 +25,6 @@ import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.processes.VisionSourceSettables;
 
 public class USBFrameProvider implements FrameProvider {
-    private static final long unixEpochToNanoEpoch =
-            System.nanoTime()
-                    - MathUtils.millisToNanos(System.currentTimeMillis()); // Units are nanoseconds
     private final CvSink cvSink;
 
     @SuppressWarnings("SpellCheckingInspection")
@@ -43,13 +40,16 @@ public class USBFrameProvider implements FrameProvider {
     @Override
     public Frame get() {
         var mat = new CVMat(); // We do this so that we don't fill a Mat in use by another thread
+        // This is from wpi::Now, or WPIUtilJNI.now()
         long time =
                 cvSink.grabFrame(
                         mat.getMat()); // Units are microseconds, epoch is the same as the Unix epoch
-        return new Frame(
-                mat,
-                MathUtils.microsToNanos(time) + unixEpochToNanoEpoch,
-                settables.getFrameStaticProperties());
+
+        // Sometimes CSCore gives us a zero frametime.
+        if (time <= 1e-6) {
+            time = MathUtils.wpiNanoTime();
+        }
+        return new Frame(mat, MathUtils.microsToNanos(time), settables.getFrameStaticProperties());
     }
 
     @Override

photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipeline.java

@@ -18,7 +18,6 @@
 package org.photonvision.vision.pipeline;
 
 import java.util.List;
-import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.camera.QuirkyCamera;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameStaticProperties;
@@ -62,9 +61,7 @@ public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelin
         }
         R result = process(frame, settings);
 
-        // Important! This assumes that the frame timestamp has the same epoch as System.nanoTime (which
-        // itself has an arbitrary epoch)
-        result.setLatencyMillis(MathUtils.nanosToMillis(System.nanoTime() - frame.timestampNanos));
+        result.setImageCaptureTimestampNanos(frame.timestampNanos);
 
         return result;
     }

photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java

@@ -34,7 +34,6 @@ import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.SerializationUtils;
 import org.photonvision.common.util.file.FileUtils;
-import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.raspi.PicamJNI;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.camera.CameraQuirk;
@@ -147,7 +146,7 @@ public class Calibrate3dPipeline
 
         // Return the drawn chessboard if corners are found, if not, then return the input image.
         return new CVPipelineResult(
-                MathUtils.nanosToMillis(sumPipeNanosElapsed),
+                sumPipeNanosElapsed,
                 fps, // Unused but here in case
                 Collections.emptyList(),
                 new Frame(outputColorCVMat, frame.frameStaticProperties));

photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java

@@ -23,7 +23,6 @@ import java.util.stream.Collectors;
 import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.core.Mat;
 import org.opencv.core.Point;
-import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.opencv.*;
 import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
@@ -298,7 +297,7 @@ public class ColoredShapePipeline
         var fps = fpsResult.output;
 
         return new CVPipelineResult(
-                MathUtils.nanosToMillis(sumPipeNanosElapsed),
+                sumPipeNanosElapsed,
                 fps,
                 targetList,
                 new Frame(new CVMat(hsvPipeResult.output), frame.frameStaticProperties),

photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java

@@ -19,7 +19,6 @@ package org.photonvision.vision.pipeline;
 
 import java.util.List;
 import org.apache.commons.lang3.tuple.Pair;
-import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.CVMat;
@@ -131,7 +130,7 @@ public class OutputStreamPipeline {
         var fps = fpsResult.output;
 
         return new CVPipelineResult(
-                MathUtils.nanosToMillis(sumPipeNanosElapsed),
+                sumPipeNanosElapsed,
                 fps, // Unused but here just in case
                 targetsToDraw,
                 new Frame(new CVMat(outMat), outputFrame.frameStaticProperties),

photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java

@@ -172,7 +172,7 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
             // the GPU
             hsvPipeResult = new CVPipeResult<>();
             hsvPipeResult.output = frame.image.getMat();
-            hsvPipeResult.nanosElapsed = System.nanoTime() - frame.timestampNanos;
+            hsvPipeResult.nanosElapsed = MathUtils.wpiNanoTime() - frame.timestampNanos;
 
             sumPipeNanosElapsed = pipeProfileNanos[1] = hsvPipeResult.nanosElapsed;
         }
@@ -223,7 +223,7 @@ public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectiveP
         PipelineProfiler.printReflectiveProfile(pipeProfileNanos);
 
         return new CVPipelineResult(
-                MathUtils.nanosToMillis(sumPipeNanosElapsed),
+                sumPipeNanosElapsed,
                 fps,
                 targetList,
                 new Frame(new CVMat(hsvPipeResult.output), frame.frameStaticProperties),

photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java

@@ -19,12 +19,13 @@ package org.photonvision.vision.pipeline.result;
 
 import java.util.Collections;
 import java.util.List;
+import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.opencv.Releasable;
 import org.photonvision.vision.target.TrackedTarget;
 
 public class CVPipelineResult implements Releasable {
-    private double latencyMillis;
+    private long imageCaptureTimestampNanos;
     public final double processingMillis;
     public final double fps;
     public final List<TrackedTarget> targets;
@@ -46,8 +47,8 @@ public class CVPipelineResult implements Releasable {
     }
 
     public CVPipelineResult(
-            double processingMillis, double fps, List<TrackedTarget> targets, Frame outputFrame) {
-        this(processingMillis, fps, targets, outputFrame, null);
+            double processingNanos, double fps, List<TrackedTarget> targets, Frame outputFrame) {
+        this(processingNanos, fps, targets, outputFrame, null);
     }
 
     public boolean hasTargets() {
@@ -62,11 +63,21 @@ public class CVPipelineResult implements Releasable {
         if (inputFrame != null) inputFrame.release();
     }
 
+    /**
+    * Get the latency between now (wpi::Now) and the time at which the image was captured. FOOTGUN:
+    * the latency is relative to the time at which this method is called. Waiting to call this method
+    * will change the latency this method returns.
+    */
     public double getLatencyMillis() {
-        return latencyMillis;
+        var now = MathUtils.wpiNanoTime();
+        return MathUtils.nanosToMillis(now - imageCaptureTimestampNanos);
     }
 
-    public void setLatencyMillis(double latencyMillis) {
-        this.latencyMillis = latencyMillis;
+    public long getImageCaptureTimestampNanos() {
+        return imageCaptureTimestampNanos;
+    }
+
+    public void setImageCaptureTimestampNanos(long imageCaptureTimestampNanos) {
+        this.imageCaptureTimestampNanos = imageCaptureTimestampNanos;
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java

@@ -21,7 +21,7 @@ import java.util.List;
 import org.photonvision.vision.frame.Frame;
 
 public class DriverModePipelineResult extends CVPipelineResult {
-    public DriverModePipelineResult(double latencyMillis, double fps, Frame outputFrame) {
-        super(latencyMillis, fps, List.of(), outputFrame);
+    public DriverModePipelineResult(double latencyNanos, double fps, Frame outputFrame) {
+        super(latencyNanos, fps, List.of(), outputFrame);
     }
 }

photon-core/src/test/java/org/photonvision/common/ShapeBenchmarkTest.java

@@ -174,7 +174,7 @@ public class ShapeBenchmarkTest {
             pipelineResult.release();
             processingTimes.add(pipelineResult.processingMillis);
             latencyTimes.add(pipelineResult.getLatencyMillis());
-        } while (System.currentTimeMillis() - benchmarkStartMillis < secondsToRun * 1000);
+        } while (System.currentTimeMillis() - benchmarkStartMillis < secondsToRun * 1000L);
         System.out.println("Benchmark complete.");
 
         var processingMin = Collections.min(processingTimes);