Profiling framework

src/main/java/com/team766/framework/ContextImpl.java

@@ -416,7 +416,7 @@ public void end(boolean interrupted) {
     @Override
     public void execute() {
         ReservingCommand.enterCommand(this);
-        try {
+        try (var profileScope = Profiling.scope("Procedures/" + m_procedure.getName())) {
             if (m_state == State.DONE) {
                 return;
             }

src/main/java/com/team766/framework/Mechanism.java

@@ -120,7 +120,7 @@ public final Set<? extends MechanismSubsystem> getReservableSubsystems() {
     }
 
     /* package */ final void periodicInternal() {
-        try {
+        try (var profileScope = Profiling.scope("Mechanisms/" + getName())) {
             publishStatus();
 
             isRunningPeriodic = true;

src/main/java/com/team766/framework/MultiFacetedMechanism.java

@@ -81,20 +81,22 @@ public Category getLoggerCategory() {
     }
 
     /* package */ final void periodicInternal() {
-        for (var m : facets) {
-            m.periodicInternal();
-        }
-
-        try {
-            publishStatus();
-
-            isRunningPeriodic = true;
-            run();
-        } catch (Exception ex) {
-            ex.printStackTrace();
-            LoggerExceptionUtils.logException(ex);
-        } finally {
-            isRunningPeriodic = false;
+        try (var profileScope = Profiling.scope("Mechanisms/" + getName())) {
+            for (var m : facets) {
+                m.periodicInternal();
+            }
+
+            try {
+                publishStatus();
+
+                isRunningPeriodic = true;
+                run();
+            } catch (Exception ex) {
+                ex.printStackTrace();
+                LoggerExceptionUtils.logException(ex);
+            } finally {
+                isRunningPeriodic = false;
+            }
         }
     }
 

src/main/java/com/team766/framework/Profiling.java

@@ -0,0 +1,84 @@
+package com.team766.framework;
+
+import com.team766.config.ConfigFileReader;
+import com.team766.library.RateLimiter;
+import com.team766.library.ValueProvider;
+import com.team766.logging.Category;
+import com.team766.logging.Severity;
+import org.littletonrobotics.junction.Logger;
+
+public class Profiling {
+    private static double getTime() {
+        return System.currentTimeMillis();
+    }
+
+    public static class Scope implements AutoCloseable {
+        private double start;
+        private int index;
+        private String name;
+
+        private void start(String name, int index) {
+            this.name = name;
+            this.index = index;
+            this.start = getTime();
+        }
+
+        @Override
+        public void close() {
+            if (index != scopeIndex - 1) {
+                throw new IllegalStateException(
+                        "Ended scope "
+                                + name
+                                + " but expected end of scope "
+                                + scopes[scopeIndex - 1]);
+            }
+            --scopeIndex;
+            addSample(name, getTime() - start);
+            name = null;
+        }
+    }
+
+    private static final int PROFILING_SCOPE_STACK_DEPTH = 20;
+
+    private static final ValueProvider<Boolean> profilingEnabled =
+            ConfigFileReader.instance.getBoolean("profiling.enabled");
+    private static final Scope[] scopes = new Scope[PROFILING_SCOPE_STACK_DEPTH];
+    private static int scopeIndex = 0;
+    private static RateLimiter logWarningLimiter = new RateLimiter(1.0);
+
+    static {
+        for (int i = 0; i < scopes.length; ++i) {
+            scopes[i] = new Scope();
+        }
+    }
+
+    public static Scope scope(String name) {
+        if (!profilingEnabled.valueOr(false)) {
+            return null;
+        }
+        if (scopeIndex >= scopes.length) {
+            if (logWarningLimiter.next()) {
+                com.team766.logging.Logger.get(Category.FRAMEWORK)
+                        .logRaw(
+                                Severity.ERROR,
+                                "Exceeded the maximum stack depth of profiling scopes. Increase "
+                                        + "PROFILING_SCOPE_STACK_DEPTH or reduce the number of "
+                                        + "profiling scopes.");
+            }
+            return null;
+        }
+        final var scope = scopes[scopeIndex];
+        scope.start(name, scopeIndex);
+        ++scopeIndex;
+        return scope;
+    }
+
+    public static void addSample(String name, double duration) {
+        if (!profilingEnabled.valueOr(false)) {
+            return;
+        }
+        Logger.recordOutput("Profiling/" + name, duration);
+    }
+
+    private Profiling() {}
+}

src/main/java/com/team766/framework/RuleEngine.java

@@ -82,102 +82,104 @@ private void sealRules() {
     }
 
     public final void run() {
-        if (!sealed) {
-            sealRules();
-            sealed = true;
-        }
-
-        Set<Subsystem> subsystemsToUse = new HashSet<>();
-
-        // TODO(MF3): when creating a Procedure, check that the reservations are the same as
-        // what the Rule pre-computed.
+        try (var engineProfilingScope = Profiling.scope(getName())) {
+            if (!sealed) {
+                sealRules();
+                sealed = true;
+            }
 
-        // evaluate each rule
-        ruleLoop:
-        for (Rule rule : rules.values()) {
-            try {
-                rule.evaluate();
-
-                // see if the rule is triggering
-                final Rule.TriggerState triggerState = rule.getCurrentTriggerState();
-                if (triggerState != Rule.TriggerState.NONE) {
-                    int priority = getPriorityForRule(rule);
-
-                    // see if there are mechanisms a potential procedure would want to reserve
-                    Set<Subsystem> reservations = rule.getSubsystemsToReserve();
-                    for (Subsystem subsystem : reservations) {
-                        // see if any of the mechanisms higher priority rules will use would also be
-                        // used by this lower priority rule's procedure.
-                        if (subsystemsToUse.contains(subsystem)) {
-                            rule.reset(ResetReason.IGNORED);
-                            continue ruleLoop;
-                        }
-                        // see if a previously triggered rule is still using the mechanism
-                        Command existingCommand =
-                                CommandScheduler.getInstance().requiring(subsystem);
-                        if (existingCommand != null) {
-                            // look up the rule
-                            Rule existingRule = getRuleForTriggeredProcedure(existingCommand);
-                            if (existingRule != null) {
-                                // look up the priority
-                                int existingPriority = getPriorityForRule(existingRule);
-                                if (existingPriority < priority /* less is more */) {
-                                    // existing rule takes priority.
-                                    // don't proceed with this new rule.
-                                    rule.reset(ResetReason.IGNORED);
-                                    continue ruleLoop;
-                                } else if (rule != existingRule) {
-                                    // new rule takes priority
-                                    // reset existing rule
-                                    existingRule.reset(ResetReason.PREEMPTED);
+            Set<Subsystem> subsystemsToUse = new HashSet<>();
+
+            // TODO(MF3): when creating a Procedure, check that the reservations are the same as
+            // what the Rule pre-computed.
+
+            // evaluate each rule
+            ruleLoop:
+            for (Rule rule : rules.values()) {
+                try (var rulleProfilingScope = Profiling.scope("Rules/" + rule.getName())) {
+                    rule.evaluate();
+
+                    // see if the rule is triggering
+                    final Rule.TriggerState triggerState = rule.getCurrentTriggerState();
+                    if (triggerState != Rule.TriggerState.NONE) {
+                        int priority = getPriorityForRule(rule);
+
+                        // see if there are mechanisms a potential procedure would want to reserve
+                        Set<Subsystem> reservations = rule.getSubsystemsToReserve();
+                        for (Subsystem subsystem : reservations) {
+                            // see if any of the mechanisms higher priority rules would also be
+                            // used by this lower priority rule's procedure.
+                            if (subsystemsToUse.contains(subsystem)) {
+                                rule.reset(ResetReason.IGNORED);
+                                continue ruleLoop;
+                            }
+                            // see if a previously triggered rule is still using the mechanism
+                            Command existingCommand =
+                                    CommandScheduler.getInstance().requiring(subsystem);
+                            if (existingCommand != null) {
+                                // look up the rule
+                                Rule existingRule = getRuleForTriggeredProcedure(existingCommand);
+                                if (existingRule != null) {
+                                    // look up the priority
+                                    int existingPriority = getPriorityForRule(existingRule);
+                                    if (existingPriority < priority /* less is more */) {
+                                        // existing rule takes priority.
+                                        // don't proceed with this new rule.
+                                        rule.reset(ResetReason.IGNORED);
+                                        continue ruleLoop;
+                                    } else if (rule != existingRule) {
+                                        // new rule takes priority
+                                        // reset existing rule
+                                        existingRule.reset(ResetReason.PREEMPTED);
+                                    }
                                 }
                             }
                         }
-                    }
 
-                    // we're good to proceed
-                    if (triggerState == Rule.TriggerState.FINISHED
-                            && rule.getCancellationOnFinish()
-                                    == Rule.Cancellation.CANCEL_NEWLY_ACTION) {
-                        var newlyCommand =
-                                ruleMap.inverse()
-                                        .get(new RuleAction(rule, Rule.TriggerState.NEWLY));
-                        if (newlyCommand != null) {
-                            newlyCommand.cancel();
+                        // we're good to proceed
+                        if (triggerState == Rule.TriggerState.FINISHED
+                                && rule.getCancellationOnFinish()
+                                        == Rule.Cancellation.CANCEL_NEWLY_ACTION) {
+                            var newlyCommand =
+                                    ruleMap.inverse()
+                                            .get(new RuleAction(rule, Rule.TriggerState.NEWLY));
+                            if (newlyCommand != null) {
+                                newlyCommand.cancel();
+                            }
                         }
-                    }
 
-                    Procedure procedure = rule.getProcedureToRun();
-                    if (procedure == null) {
-                        continue;
+                        Procedure procedure = rule.getProcedureToRun();
+                        if (procedure == null) {
+                            continue;
+                        }
+                        log(
+                                Severity.INFO,
+                                "Rule "
+                                        + rule.getName()
+                                        + " triggered ("
+                                        + rule.getCurrentTriggerState()
+                                        + ").  Running Procedure "
+                                        + procedure.getName()
+                                        + " with reservations "
+                                        + reservations);
+
+                        // TODO(MF3): check that the reservations have not changed
+                        Command command = procedure.createCommandToRunProcedure();
+                        subsystemsToUse.addAll(reservations);
+                        ruleMap.forcePut(command, new RuleAction(rule, triggerState));
+                        command.schedule();
                     }
+                } catch (Exception ex) {
                     log(
-                            Severity.INFO,
-                            "Rule "
-                                    + rule.getName()
-                                    + " triggered ("
-                                    + rule.getCurrentTriggerState()
-                                    + ").  Running Procedure "
-                                    + procedure.getName()
-                                    + " with reservations "
-                                    + reservations);
-
-                    // TODO(MF3): check that the reservations have not changed
-                    Command command = procedure.createCommandToRunProcedure();
-                    subsystemsToUse.addAll(reservations);
-                    ruleMap.forcePut(command, new RuleAction(rule, triggerState));
-                    command.schedule();
+                            Severity.ERROR,
+                            "Exception caught while trying to run(): "
+                                    + LoggerExceptionUtils.exceptionToString(ex));
                 }
-            } catch (Exception ex) {
-                log(
-                        Severity.ERROR,
-                        "Exception caught while trying to run(): "
-                                + LoggerExceptionUtils.exceptionToString(ex));
             }
-        }
 
-        for (Rule rule : rules.values()) {
-            rule.flushLog();
+            for (Rule rule : rules.values()) {
+                rule.flushLog();
+            }
         }
     }
 }

src/main/java/com/team766/hal/wpilib/RobotMain.java

@@ -2,6 +2,7 @@
 
 import com.team766.BuildConstants;
 import com.team766.config.ConfigFileReader;
+import com.team766.framework.Profiling;
 import com.team766.hal.CanivPoller;
 import com.team766.hal.GenericRobotMain;
 import com.team766.hal.RobotProvider;
@@ -142,6 +143,16 @@ public void robotInit() {
         }
     }
 
+    private Profiling.Scope robotLoopScope = null;
+
+    @Override
+    public void robotPeriodic() {
+        if (robotLoopScope != null) {
+            robotLoopScope.close();
+        }
+        robotLoopScope = Profiling.scope("RobotMain");
+    }
+
     @Override
     public void disabledInit() {
         try {