Create sendable Tracer for profiling pipelines

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

@@ -0,0 +1,47 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.util;
+
+import edu.wpi.first.networktables.IntegerArrayPublisher;
+import edu.wpi.first.networktables.StringArrayPublisher;
+import org.photonvision.common.dataflow.networktables.NetworkTablesManager;
+
+public class EpochPublisher {
+    private IntegerArrayPublisher timePub;
+    private StringArrayPublisher namePub;
+
+    public EpochPublisher(String nickname) {
+        this.timePub =
+                NetworkTablesManager.getInstance()
+                        .kRootTable
+                        .getSubTable(nickname)
+                        .getIntegerArrayTopic("tracer/timesUs")
+                        .publish();
+        this.namePub =
+                NetworkTablesManager.getInstance()
+                        .kRootTable
+                        .getSubTable(nickname)
+                        .getStringArrayTopic("tracer/epochNames")
+                        .publish();
+    }
+
+    public void consume(OrderedTracer tracer) {
+        timePub.set(tracer.getEpochTimes());
+        namePub.set(tracer.getEpochNames());
+    }
+}

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

@@ -0,0 +1,111 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.common.util;
+
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
+import java.util.LinkedHashMap;
+import java.util.Map;
+import java.util.function.Consumer;
+
+// Copy of wpilib's Tracer, with some patches
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+public class OrderedTracer {
+    private static final long kMinPrintPeriod = 1000000; // microseconds
+
+    private static volatile long m_lastEpochsPrintTime; // microseconds
+
+    private long m_startTime; // microseconds
+
+    private final Map<String, Long> m_epochs = new LinkedHashMap<>(); // microseconds
+
+    /** Tracer constructor. */
+    public OrderedTracer() {
+        resetTimer();
+    }
+
+    /** Clears all epochs. */
+    public void clearEpochs() {
+        m_epochs.clear();
+        resetTimer();
+    }
+
+    /** Restarts the epoch timer. */
+    public final void resetTimer() {
+        m_startTime = RobotController.getFPGATime();
+    }
+
+    /**
+     * Adds time since last epoch to the list printed by printEpochs().
+     *
+     * <p>Epochs are a way to partition the time elapsed so that when overruns occur, one can
+     * determine which parts of an operation consumed the most time.
+     *
+     * <p>This should be called immediately after execution has finished, with a call to this method
+     * or {@link #resetTimer()} before execution.
+     *
+     * @param epochName The name to associate with the epoch.
+     */
+    public void addEpoch(String epochName) {
+        long currentTime = RobotController.getFPGATime();
+        m_epochs.put(epochName, currentTime - m_startTime);
+        m_startTime = currentTime;
+    }
+
+    /** Prints list of epochs added so far and their times to the DriverStation. */
+    public void printEpochs() {
+        printEpochs(out -> DriverStation.reportWarning(out, false));
+    }
+
+    /**
+     * Prints list of epochs added so far and their times to the entered String consumer.
+     *
+     * <p>This overload can be useful for logging to a file, etc.
+     *
+     * @param output the stream that the output is sent to
+     */
+    public void printEpochs(Consumer<String> output) {
+        long now = RobotController.getFPGATime();
+        if (now - m_lastEpochsPrintTime > kMinPrintPeriod) {
+            StringBuilder sb = new StringBuilder();
+            m_lastEpochsPrintTime = now;
+            m_epochs.forEach(
+                    (key, value) -> sb.append(String.format("\t%s: %.6fs\n", key, value / 1.0e6)));
+            if (sb.length() > 0) {
+                output.accept(sb.toString());
+            }
+        }
+    }
+
+    public long[] getEpochTimes() {
+        var ret = m_epochs.values();
+        var arr = new long[ret.size()];
+        int i = 0;
+        for (var e : ret) {
+            arr[i] = e;
+            i++;
+        }
+        return arr;
+    }
+
+    public String[] getEpochNames() {
+        return m_epochs.keySet().toArray(new String[0]);
+    }
+}

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

@@ -17,6 +17,7 @@
 
 package org.photonvision.vision.frame;
 
+import org.photonvision.common.util.OrderedTracer;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.opencv.CVMat;
 import org.photonvision.vision.opencv.Releasable;
@@ -32,28 +33,42 @@ public class Frame implements Releasable {
 
     public final FrameStaticProperties frameStaticProperties;
 
+    // A Frame owns the tracer from frame capture from the underlying device until publish/GC. This
+    // does mean we get extra GC pressure, but w/e
+    public final OrderedTracer m_tracer;
+
     public Frame(
             long sequenceID,
             CVMat color,
             CVMat processed,
             FrameThresholdType type,
             long timestampNanos,
-            FrameStaticProperties frameStaticProperties) {
+            FrameStaticProperties frameStaticProperties,
+            OrderedTracer tracer) {
         this.sequenceID = sequenceID;
         this.colorImage = color;
         this.processedImage = processed;
         this.type = type;
         this.timestampNanos = timestampNanos;
         this.frameStaticProperties = frameStaticProperties;
+        this.m_tracer = tracer;
     }
 
     public Frame(
             long sequenceID,
             CVMat color,
             CVMat processed,
             FrameThresholdType processType,
-            FrameStaticProperties frameStaticProperties) {
-        this(sequenceID, color, processed, processType, MathUtils.wpiNanoTime(), frameStaticProperties);
+            FrameStaticProperties frameStaticProperties,
+            OrderedTracer tracer) {
+        this(
+                sequenceID,
+                color,
+                processed,
+                processType,
+                MathUtils.wpiNanoTime(),
+                frameStaticProperties,
+                tracer);
     }
 
     public Frame() {
@@ -63,7 +78,8 @@ public Frame() {
                 new CVMat(),
                 FrameThresholdType.NONE,
                 MathUtils.wpiNanoTime(),
-                new FrameStaticProperties(0, 0, 0, null));
+                new FrameStaticProperties(0, 0, 0, null),
+                new OrderedTracer());
     }
 
     public void copyTo(Frame destFrame) {

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

@@ -17,6 +17,7 @@
 
 package org.photonvision.vision.frame.provider;
 
+import org.photonvision.common.util.OrderedTracer;
 import org.photonvision.common.util.numbers.IntegerCouple;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameProvider;
@@ -31,15 +32,20 @@
 
 public abstract class CpuImageProcessor extends FrameProvider {
     protected static class CapturedFrame {
-        CVMat colorImage;
-        FrameStaticProperties staticProps;
-        long captureTimestamp;
+        public final CVMat colorImage;
+        public final FrameStaticProperties staticProps;
+        public final long captureTimestamp;
+        public final OrderedTracer tracer;
 
         public CapturedFrame(
-                CVMat colorImage, FrameStaticProperties staticProps, long captureTimestampNanos) {
+                CVMat colorImage,
+                FrameStaticProperties staticProps,
+                long captureTimestampNanos,
+                OrderedTracer tracer) {
             this.colorImage = colorImage;
             this.staticProps = staticProps;
             this.captureTimestamp = captureTimestampNanos;
+            this.tracer = tracer;
         }
     }
 
@@ -72,19 +78,23 @@ public final Frame get() {
         {
             CVPipeResult<Void> out = m_rImagePipe.run(input.colorImage.getMat());
             sumNanos += out.nanosElapsed;
+            input.tracer.addEpoch("CpuImageProcessor::rotate");
         }
 
         if (!input.colorImage.getMat().empty()) {
             if (m_processType == FrameThresholdType.HSV) {
                 var hsvResult = m_hsvPipe.run(input.colorImage.getMat());
                 outputMat = new CVMat(hsvResult.output);
                 sumNanos += hsvResult.nanosElapsed;
+                input.tracer.addEpoch("CpuImageProcessor::processHSV");
             } else if (m_processType == FrameThresholdType.GREYSCALE) {
                 var result = m_grayPipe.run(input.colorImage.getMat());
                 outputMat = new CVMat(result.output);
                 sumNanos += result.nanosElapsed;
+                input.tracer.addEpoch("CpuImageProcessor::processGray");
             } else {
                 outputMat = new CVMat();
+                input.tracer.addEpoch("CpuImageProcessor::processNone");
             }
 
             ++sequenceID;
@@ -99,7 +109,8 @@ public final Frame get() {
                 outputMat,
                 m_processType,
                 input.captureTimestamp,
-                input.staticProps.rotate(m_rImagePipe.getParams().rotation));
+                input.staticProps.rotate(m_rImagePipe.getParams().rotation),
+                input.tracer);
     }
 
     @Override

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

@@ -22,6 +22,7 @@
 import java.nio.file.Paths;
 import org.opencv.core.Mat;
 import org.opencv.imgcodecs.Imgcodecs;
+import org.photonvision.common.util.OrderedTracer;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.frame.FrameProvider;
@@ -98,6 +99,8 @@ public FileFrameProvider(Path path, double fov) {
 
     @Override
     public CapturedFrame getInputMat() {
+        var tracer = new OrderedTracer();
+
         var out = new CVMat();
         out.copyFrom(originalFrame);
 
@@ -111,7 +114,10 @@ public CapturedFrame getInputMat() {
         }
 
         lastGetMillis = System.currentTimeMillis();
-        return new CapturedFrame(out, properties, MathUtils.wpiNanoTime());
+
+        tracer.addEpoch("FileFrame::fakeDelay");
+
+        return new CapturedFrame(out, properties, MathUtils.wpiNanoTime(), tracer);
     }
 
     @Override

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

@@ -20,6 +20,7 @@
 import org.opencv.core.Mat;
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
+import org.photonvision.common.util.OrderedTracer;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.raspi.LibCameraJNI;
 import org.photonvision.vision.camera.LibcameraGpuSettables;
@@ -51,11 +52,15 @@ public String getName() {
 
     @Override
     public Frame get() {
+        var tracer = new OrderedTracer();
+
         // We need to make sure that other threads don't try to change video modes while
         // we're waiting for a frame
         // System.out.println("GET!");
         synchronized (settables.CAMERA_LOCK) {
+            tracer.addEpoch("Libcam::lockAcquired");
             var p_ptr = LibCameraJNI.awaitNewFrame(settables.r_ptr);
+            tracer.addEpoch("Libcam::gotNewFrame");
 
             if (p_ptr == 0) {
                 logger.error("No new frame from " + settables.getConfiguration().nickname);
@@ -74,6 +79,7 @@ public Frame get() {
 
             var colorMat = new CVMat(new Mat(LibCameraJNI.takeColorFrame(p_ptr)));
             var processedMat = new CVMat(new Mat(LibCameraJNI.takeProcessedFrame(p_ptr)));
+            tracer.addEpoch("Libcam::takeFrames");
 
             // System.out.println("Color mat: " + colorMat.getMat().size());
 
@@ -91,6 +97,7 @@ public Frame get() {
             var latency = (now - capture);
 
             LibCameraJNI.releasePair(p_ptr);
+            tracer.addEpoch("Libcam::releaseFrames");
 
             // Know frame is good -- increment sequence
             ++sequenceID;
@@ -101,7 +108,8 @@ public Frame get() {
                     processedMat,
                     type,
                     MathUtils.wpiNanoTime() - latency,
-                    settables.getFrameStaticProperties().rotate(settables.getRotation()));
+                    settables.getFrameStaticProperties().rotate(settables.getRotation()),
+                    tracer);
         }
     }