From 427d3c1e6554fa70b1d8dc641b92d8c5459eb313 Mon Sep 17 00:00:00 2001 From: oh-yes-0-fps Date: Thu, 19 Sep 2024 01:09:41 -0400 Subject: [PATCH] rework tracer --- .../wpi/first/wpilibj/IterativeRobotBase.java | 37 +- .../edu/wpi/first/wpilibj/TimedRobot.java | 4 + .../java/edu/wpi/first/wpilibj/Tracer.java | 328 ++++++++++++++---- .../java/edu/wpi/first/wpilibj/Watchdog.java | 24 -- .../edu/wpi/first/wpilibj/WatchdogTest.java | 30 -- 5 files changed, 270 insertions(+), 153 deletions(-) diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java index b0c46604283..d1207fc5ba1 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java @@ -299,6 +299,8 @@ public double getPeriod() { /** Loop function. */ protected void loopFunc() { + Tracer.startTrace("RobotLoop"); + DriverStation.refreshData(); m_watchdog.reset(); @@ -344,15 +346,12 @@ protected void loopFunc() { switch (mode) { case kDisabled -> { disabledInit(); - m_watchdog.addEpoch("disabledInit()"); } case kAutonomous -> { autonomousInit(); - m_watchdog.addEpoch("autonomousInit()"); } case kTeleop -> { teleopInit(); - m_watchdog.addEpoch("teleopInit()"); } case kTest -> { if (m_lwEnabledInTest) { @@ -360,7 +359,6 @@ protected void loopFunc() { Shuffleboard.enableActuatorWidgets(); } testInit(); - m_watchdog.addEpoch("testInit()"); } default -> { // NOP @@ -374,44 +372,35 @@ protected void loopFunc() { switch (mode) { case kDisabled -> { DriverStationJNI.observeUserProgramDisabled(); - disabledPeriodic(); - m_watchdog.addEpoch("disabledPeriodic()"); + Tracer.traceFunc("DisabledPeriodic", this::disabledPeriodic); } case kAutonomous -> { DriverStationJNI.observeUserProgramAutonomous(); - autonomousPeriodic(); - m_watchdog.addEpoch("autonomousPeriodic()"); + Tracer.traceFunc("AutonomousPeriodic", this::autonomousPeriodic); } case kTeleop -> { DriverStationJNI.observeUserProgramTeleop(); - teleopPeriodic(); - m_watchdog.addEpoch("teleopPeriodic()"); + Tracer.traceFunc("TeleopPeriodic", this::teleopPeriodic); } case kTest -> { DriverStationJNI.observeUserProgramTest(); - testPeriodic(); - m_watchdog.addEpoch("testPeriodic()"); + Tracer.traceFunc("TestPeriodic", this::testPeriodic); } default -> { // NOP } } - robotPeriodic(); - m_watchdog.addEpoch("robotPeriodic()"); + Tracer.traceFunc("RobotPeriodic", this::robotPeriodic); SmartDashboard.updateValues(); - m_watchdog.addEpoch("SmartDashboard.updateValues()"); LiveWindow.updateValues(); - m_watchdog.addEpoch("LiveWindow.updateValues()"); Shuffleboard.update(); - m_watchdog.addEpoch("Shuffleboard.update()"); if (isSimulation()) { HAL.simPeriodicBefore(); - simulationPeriodic(); + Tracer.traceFunc("SimulationPeriodic", this::simulationPeriodic); HAL.simPeriodicAfter(); - m_watchdog.addEpoch("simulationPeriodic()"); } m_watchdog.disable(); @@ -421,15 +410,7 @@ protected void loopFunc() { NetworkTableInstance.getDefault().flushLocal(); } - // Warn on loop time overruns - if (m_watchdog.isExpired()) { - m_watchdog.printEpochs(); - } - } - - /** Prints list of epochs added so far and their times. */ - public void printWatchdogEpochs() { - m_watchdog.printEpochs(); + Tracer.endTrace(); } private void printLoopOverrunMessage() { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java index 7b59f528a6b..f514dd67957 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -128,6 +128,8 @@ public void startCompetition() { break; } + Tracer.startTrace("TimedRobot"); + callback.func.run(); // Increment the expiration time by the number of full periods it's behind @@ -150,6 +152,8 @@ public void startCompetition() { + (currentTime - callback.expirationTime) / callback.period * callback.period; m_callbacks.add(callback); } + + Tracer.endTrace(); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java index 15ca7fff81f..4723d2445b2 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java @@ -4,81 +4,267 @@ package edu.wpi.first.wpilibj; +import java.lang.management.GarbageCollectorMXBean; +import java.lang.management.ManagementFactory; +import java.util.ArrayList; import java.util.HashMap; -import java.util.Map; -import java.util.function.Consumer; +import java.util.function.Supplier; +import java.util.concurrent.atomic.AtomicBoolean; + +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; /** - * A class for keeping track of how much time it takes for different parts of code to execute. This - * is done with epochs, that are added by calls to {@link #addEpoch(String)}, and can be printed - * with a call to {@link #printEpochs()}. - * - *

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. + * A Utility class for tracing code execution time. + * Will put info to NetworkTables under the "Tracer" table. + * + *

Example inside {@code Robot.java} + * 


+ * @Override
+ * public void robotPeriodic() {
+ *   Tracer.traceFunc("CommandScheduler", scheduler::run);
+ *   Tracer.traceFunc("MyVendorDep", MyVendorDep::updateAll);
+ * }
+ *
+ * + *

Example inside a {@code Drive Subsystem} + * 


+ * public void periodic() {
+ *   Tracer.startTrace("DrivePeriodic");
+ *   for (var module : modules) {
+ *     Tracer.traceFunc("Module" + module.getName(), module::update);
+ *   }
+ *   Tracer.traceFunc("Gyro", gyro::update);
+ *   Tracer.endTrace();
+ * }
+ *
*/ public class Tracer { - private static final long kMinPrintPeriod = 1000000; // microseconds - - private long m_lastEpochsPrintTime; // microseconds - private long m_startTime; // microseconds - - private final Map m_epochs = new HashMap<>(); // microseconds - - /** Tracer constructor. */ - public Tracer() { - 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(). - * - *

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. - * - *

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. - * - *

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 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()); - } + private static final class TraceStartData { + private double startTime = 0.0; + private double startGCTotalTime = 0.0; + + private void set(double startTime, double startGCTotalTime) { + this.startTime = startTime; + this.startGCTotalTime = startGCTotalTime; + } + } + + /** + * All of the tracers persistent state in a single object to be stored in a + * {@link ThreadLocal}. + */ + private static final class TracerState { + private final NetworkTable rootTable; + + // the stack of traces, every startTrace will add to this stack + // and every endTrace will remove from this stack + private final ArrayList traceStack = new ArrayList<>(); + // ideally we only need `traceStack` but in the interest of memory optimization + // and string concatenation speed we store the history of the stack to reuse the stack names + private final ArrayList traceStackHistory = new ArrayList<>(); + // the time of each trace, the key is the trace name, modified every endTrace + private final HashMap traceTimes = new HashMap<>(); + // the start time of each trace and the gc time at the start of the trace, + // the key is the trace name, modified every startTrace and endTrace. + private final HashMap traceStartTimes = new HashMap<>(); + // the publishers for each trace, the key is the trace name, modified every endCycle + private final HashMap publisherHeap = new HashMap<>(); + + // the garbage collector beans + private final ArrayList gcs = new ArrayList<>(ManagementFactory.getGarbageCollectorMXBeans()); + private final DoublePublisher gcTimeEntry; + private double gcTimeThisCycle = 0.0; + + private TracerState(String threadName) { + if (threadName == null) { + this.rootTable = NetworkTableInstance.getDefault().getTable("Tracer"); + } else { + this.rootTable = NetworkTableInstance.getDefault().getTable("Tracer").getSubTable(threadName); + } + this.gcTimeEntry = rootTable.getDoubleTopic("GCTime").publishEx( + "double", + "{ \"cached\": false}" + ); + } + + private String appendTraceStack(String trace) { + traceStack.add(trace); + StringBuilder sb = new StringBuilder(); + for (int i = 0; i < traceStack.size(); i++) { + sb.append(traceStack.get(i)); + if (i < traceStack.size() - 1) { + sb.append("/"); + } + } + String str = sb.toString(); + traceStackHistory.add(str); + return str; + } + + private String popTraceStack() { + traceStack.remove(traceStack.size() - 1); + return traceStackHistory.remove(traceStackHistory.size() - 1); + } + + private double totalGCTime() { + double gcTime = 0; + for (GarbageCollectorMXBean gc : gcs) { + gcTime += gc.getCollectionTime(); + } + return gcTime; + } + + private void endCycle() { + // update times for all already existing publishers + for (var publisher : publisherHeap.entrySet()) { + Double time = traceTimes.remove(publisher.getKey()); + if (time == null) time = 0.0; + publisher.getValue().set(time); + } + // create publishers for all new entries + for (var traceTime : traceTimes.entrySet()) { + DoublePublisher publisher = rootTable.getDoubleTopic(traceTime.getKey()).publishEx( + "double", + "{ \"cached\": false}" + ); + publisher.set(traceTime.getValue()); + publisherHeap.put(traceTime.getKey(), publisher); + } + + // log gc time + if (gcs.size() > 0) gcTimeEntry.set(gcTimeThisCycle); + gcTimeThisCycle = 0.0; + + // clean up state + traceTimes.clear(); + traceStackHistory.clear(); + } + } + + private static final AtomicBoolean singleThreadedMode = new AtomicBoolean(false); + private static final ThreadLocal threadLocalState = ThreadLocal.withInitial(() -> { + if (singleThreadedMode.get()) { + throw new IllegalStateException("Single threaded mode is enabled, cannot create new TracerState"); + } + return new TracerState(Thread.currentThread().getName()); + }); + + private static void startTraceInner(final String name, final TracerState state) { + String stack = state.appendTraceStack(name); + TraceStartData data = state.traceStartTimes.get(stack); + if (data == null) { + data = new TraceStartData(); + state.traceStartTimes.put(stack, data); + } + data.set(Timer.getFPGATimestamp() * 1_000.0, state.totalGCTime()); + } + + private static void endTraceInner(final TracerState state) { + try { + String stack = state.popTraceStack(); + var startData = state.traceStartTimes.get(stack); + double gcTimeSinceStart = state.totalGCTime() - startData.startGCTotalTime; + state.gcTimeThisCycle += gcTimeSinceStart; + state.traceTimes.put( + stack, + Timer.getFPGATimestamp() * 1_000.0 + - startData.startTime + - gcTimeSinceStart + ); + if (state.traceStack.size() == 0) { + state.endCycle(); + } + } catch (Exception e) { + DriverStation.reportError("[Tracer] An end trace was called with no opening trace " + e, true); + for (var trace : state.traceStack) { + DriverStation.reportError("[Tracer] Open trace: " + trace, false); + } + state.endCycle(); + } + } + + /** + * Starts a trace, + * should be called at the beginning of a function thats not being called by + * user code. + * Should be paired with {@link Tracer#endTrace()} at the end of the function. + * + * Best used in periodic functions in Subsystems and Robot.java. + * + * @param name the name of the trace, should be unique to the function. + */ + public static void startTrace(String name) { + startTraceInner(name, threadLocalState.get()); + } + + /** + * Ends a trace, should only be called at the end of a function thats not being + * called by user code. + * If a {@link Tracer#startTrace(String)} is not paired with a + * {@link Tracer#endTrace()} there could be a crash. + */ + public static void endTrace() { + endTraceInner(threadLocalState.get()); + } + + /** + * Disables garbage collection logging for the current thread. + * This can help performance in some cases. + */ + public static void disableGcLoggingForCurrentThread() { + TracerState state = threadLocalState.get(); + state.gcTimeEntry.close(); + state.gcs.clear(); + } + + /** + * Enables single threaded mode for the Tracer. + * This will cause traces on different threads to throw an exception. + * This will shorten the path of traced data in NetworkTables by not including the thread name. + * + *

Warning: This will throw an exception if called after any traces have been started. + */ + public static void enableSingleThreadedMode() { + threadLocalState.set(new TracerState(null)); + singleThreadedMode.set(true); + } + + /** + * Traces a function, should be used in place of + * {@link Tracer#startTrace(String)} and {@link Tracer#endTrace()} + * for functions called by user code like {@code CommandScheduler.run()} and + * other expensive functions. + * + * @param name the name of the trace, should be unique to the function. + * @param runnable the function to trace. + * + * @apiNote If you want to return a value then use + * {@link Tracer#traceFunc(String, Supplier)}. + */ + public static void traceFunc(String name, Runnable runnable) { + final TracerState state = threadLocalState.get(); + startTraceInner(name, state); + runnable.run(); + endTraceInner(state); + } + + /** + * Traces a function, should be used in place of + * {@link Tracer#startTrace(String)} and {@link Tracer#endTrace()} + * for functions called by user code like {@code CommandScheduler.run()} and + * other expensive functions. + * + * @param name the name of the trace, should be unique to the function. + * @param supplier the function to trace. + */ + public static R traceFunc(String name, Supplier supplier) { + final TracerState state = threadLocalState.get(); + startTraceInner(name, state); + R ret = supplier.get(); + endTraceInner(state); + return ret; } - } } + diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java index 6d00d6402bb..ad3c49d3446 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java @@ -31,8 +31,6 @@ public class Watchdog implements Closeable, Comparable { boolean m_suppressTimeoutMessage; - private final Tracer m_tracer; - private static final PriorityQueue m_watchdogs = new PriorityQueue<>(); private static ReentrantLock m_queueMutex = new ReentrantLock(); private static int m_notifier; @@ -52,7 +50,6 @@ public class Watchdog implements Closeable, Comparable { public Watchdog(double timeoutSeconds, Runnable callback) { m_timeoutSeconds = timeoutSeconds; m_callback = callback; - m_tracer = new Tracer(); } @Override @@ -94,7 +91,6 @@ public double getTime() { */ public void setTimeout(double timeoutSeconds) { m_startTimeSeconds = Timer.getFPGATimestamp(); - m_tracer.clearEpochs(); m_queueMutex.lock(); try { @@ -138,25 +134,6 @@ public boolean isExpired() { } } - /** - * Adds time since last epoch to the list printed by printEpochs(). - * - * @see Tracer#addEpoch(String) - * @param epochName The name to associate with the epoch. - */ - public void addEpoch(String epochName) { - m_tracer.addEpoch(epochName); - } - - /** - * Prints list of epochs added so far and their times. - * - * @see Tracer#printEpochs() - */ - public void printEpochs() { - m_tracer.printEpochs(); - } - /** * Resets the watchdog timer. * @@ -169,7 +146,6 @@ public void reset() { /** Enables the watchdog timer. */ public void enable() { m_startTimeSeconds = Timer.getFPGATimestamp(); - m_tracer.clearEpochs(); m_queueMutex.lock(); try { diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java index 5e9e1229418..4356903a340 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java @@ -117,36 +117,6 @@ void isExpiredTest() { } } - @Test - @ResourceLock("timing") - void epochsTest() { - final AtomicInteger watchdogCounter = new AtomicInteger(0); - - try (Watchdog watchdog = new Watchdog(0.4, () -> watchdogCounter.addAndGet(1))) { - // Run 1 - watchdog.enable(); - watchdog.addEpoch("Epoch 1"); - SimHooks.stepTiming(0.1); - watchdog.addEpoch("Epoch 2"); - SimHooks.stepTiming(0.1); - watchdog.addEpoch("Epoch 3"); - watchdog.disable(); - - assertEquals(0, watchdogCounter.get(), "Watchdog triggered early"); - - // Run 2 - watchdog.enable(); - watchdog.addEpoch("Epoch 1"); - SimHooks.stepTiming(0.2); - watchdog.reset(); - SimHooks.stepTiming(0.2); - watchdog.addEpoch("Epoch 2"); - watchdog.disable(); - - assertEquals(0, watchdogCounter.get(), "Watchdog triggered early"); - } - } - @Test @ResourceLock("timing") void multiWatchdogTest() {