[wpilib] Tracer Overhaul

epilogue-processor/src/main/java/edu/wpi/first/epilogue/processor/EpilogueGenerator.java

@@ -182,7 +182,9 @@ public static boolean shouldLog(Logged.Importance importance) {
             out.println("    robot.addPeriodic(() -> {");
             out.println("      update(robot);");
             out.println(
-                "    }, config.loggingPeriod.in(Seconds), config.loggingPeriodOffset.in(Seconds));");
+                "    },");
+            out.println("    \"EpiloguePeriodic\",");
+            out.println("    config.loggingPeriod.in(Seconds), config.loggingPeriodOffset.in(Seconds));");
             out.println("  }");
           }
         }

epilogue-processor/src/test/java/edu/wpi/first/epilogue/processor/EpilogueGeneratorTest.java

@@ -176,7 +176,9 @@ public static void bind(edu.wpi.first.epilogue.Example robot) {
 
             robot.addPeriodic(() -> {
               update(robot);
-            }, config.loggingPeriod.in(Seconds), config.loggingPeriodOffset.in(Seconds));
+            },
+            "EpiloguePeriodic",
+            config.loggingPeriod.in(Seconds), config.loggingPeriodOffset.in(Seconds));
           }
         }
         """;
@@ -256,7 +258,9 @@ public static void bind(edu.wpi.first.epilogue.AlphaBot robot) {
 
             robot.addPeriodic(() -> {
               update(robot);
-            }, config.loggingPeriod.in(Seconds), config.loggingPeriodOffset.in(Seconds));
+            },
+            "EpiloguePeriodic",
+            config.loggingPeriod.in(Seconds), config.loggingPeriodOffset.in(Seconds));
           }
 
           /**
@@ -288,7 +292,9 @@ public static void bind(edu.wpi.first.epilogue.BetaBot robot) {
 
             robot.addPeriodic(() -> {
               update(robot);
-            }, config.loggingPeriod.in(Seconds), config.loggingPeriodOffset.in(Seconds));
+            },
+            "EpiloguePeriodic",
+            config.loggingPeriod.in(Seconds), config.loggingPeriodOffset.in(Seconds));
           }
         }
         """;

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.Tracer;
 import java.util.Collection;
 import java.util.HashSet;
 import java.util.Set;
@@ -581,6 +582,29 @@ public WrapperCommand withName(String name) {
     return wrapper;
   }
 
+  /**
+   * Decorates this Command so that it's `Execute` method is implicitly timed using the {@link
+   * Tracer} class. This decorator also names the command similarly to the {@link #withName(String)}
+   * method.
+   *
+   * @param name name
+   * @return the decorated Command
+   */
+  public WrapperCommand traced(String name) {
+    WrapperCommand wrapper =
+        new WrapperCommand(Command.this) {
+          @Override
+          public void execute() {
+            // Avoid `Trace.traceFunc` to keep it out of the stack trace
+            Tracer.startTrace(name);
+            super.execute();
+            Tracer.endTrace();
+          }
+        };
+    wrapper.setName(name);
+    return wrapper;
+  }
+
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Command");

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java

@@ -16,6 +16,7 @@
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.RobotState;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Tracer;
 import edu.wpi.first.wpilibj.Watchdog;
 import edu.wpi.first.wpilibj.event.EventLoop;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
@@ -97,7 +98,13 @@ public static synchronized CommandScheduler getInstance() {
   private final List<Optional<Command>> m_toCancelInterruptors = new ArrayList<>();
   private final Set<Command> m_endingCommands = new LinkedHashSet<>();
 
-  private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {});
+  private final Watchdog m_watchdog =
+      new Watchdog(
+          TimedRobot.kDefaultPeriod,
+          () -> {
+            System.out.println(
+                "CommandScheduler loop overrun, check NetworkTables for timing info");
+          });
 
   CommandScheduler() {
     HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand2_Scheduler);
@@ -170,8 +177,6 @@ private void initCommand(Command command, Set<Subsystem> requirements) {
     for (Consumer<Command> action : m_initActions) {
       action.accept(command);
     }
-
-    m_watchdog.addEpoch(command.getName() + ".initialize()");
   }
 
   /**
@@ -251,29 +256,32 @@ public void schedule(Command... commands) {
    * have their end methods called and are removed.
    *
    * <p>Any subsystems not being used as requirements have their default methods started.
+   *
+   * <p>This method is implicitly timed using the {@link Tracer} class.
    */
   public void run() {
     if (m_disabled) {
       return;
     }
+
+    Tracer.startTrace("CommandScheduler");
     m_watchdog.reset();
 
     // Run the periodic method of all registered subsystems.
     for (Subsystem subsystem : m_subsystems.keySet()) {
-      subsystem.periodic();
+      Tracer.traceFunc(subsystem.getName() + "Periodic", subsystem::periodic);
       if (RobotBase.isSimulation()) {
-        subsystem.simulationPeriodic();
+        Tracer.traceFunc(subsystem.getName() + "SimulationPeriodic", subsystem::simulationPeriodic);
       }
-      m_watchdog.addEpoch(subsystem.getName() + ".periodic()");
     }
 
     // Cache the active instance to avoid concurrency problems if setActiveLoop() is called from
     // inside the button bindings.
     EventLoop loopCache = m_activeButtonLoop;
     // Poll buttons for new commands to add.
-    loopCache.poll();
-    m_watchdog.addEpoch("buttons.run()");
+    Tracer.traceFunc("PollButtons", loopCache::poll);
 
+    Tracer.startTrace("Commands");
     m_inRunLoop = true;
     boolean isDisabled = RobotState.isDisabled();
     // Run scheduled commands, remove finished commands.
@@ -289,7 +297,6 @@ public void run() {
       for (Consumer<Command> action : m_executeActions) {
         action.accept(command);
       }
-      m_watchdog.addEpoch(command.getName() + ".execute()");
       if (command.isFinished()) {
         m_endingCommands.add(command);
         command.end(false);
@@ -300,7 +307,6 @@ public void run() {
         iterator.remove();
 
         m_requirements.keySet().removeAll(command.getRequirements());
-        m_watchdog.addEpoch(command.getName() + ".end(false)");
       }
     }
     m_inRunLoop = false;
@@ -325,12 +331,9 @@ public void run() {
         schedule(subsystemCommand.getValue());
       }
     }
+    Tracer.endTrace(); // Commands
 
     m_watchdog.disable();
-    if (m_watchdog.isExpired()) {
-      System.out.println("CommandScheduler loop overrun");
-      m_watchdog.printEpochs();
-    }
   }
 
   /**
@@ -487,7 +490,6 @@ private void cancel(Command command, Optional<Command> interruptor) {
     m_endingCommands.remove(command);
     m_scheduledCommands.remove(command);
     m_requirements.keySet().removeAll(command.getRequirements());
-    m_watchdog.addEpoch(command.getName() + ".end(true)");
   }
 
   /** Cancels all commands that are currently scheduled. */
@@ -530,11 +532,6 @@ public void enable() {
     m_disabled = false;
   }
 
-  /** Prints list of epochs added so far and their times. */
-  public void printWatchdogEpochs() {
-    m_watchdog.printEpochs();
-  }
-
   /**
    * Adds an action to perform on the initialization of any command by the scheduler.
    *

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java

@@ -30,13 +30,17 @@ public interface Subsystem {
    * subsystem-specific state that you don't want to offload to a {@link Command}. Teams should try
    * to be consistent within their own codebases about which responsibilities will be handled by
    * Commands, and which will be handled here.
+   *
+   * <p>This method is implicitly timed using the {@link edu.wpi.first.wpilibj.Tracer}.
    */
   default void periodic() {}
 
   /**
    * This method is called periodically by the {@link CommandScheduler}. Useful for updating
    * subsystem-specific state that needs to be maintained for simulations, such as for updating
    * {@link edu.wpi.first.wpilibj.simulation} classes and setting simulated sensor readings.
+   *
+   * <p>This method is implicitly timed using the {@link edu.wpi.first.wpilibj.Tracer}.
    */
   default void simulationPeriodic() {}
 

wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp

@@ -149,6 +149,10 @@ CommandPtr Command::WithName(std::string_view name) && {
   return std::move(*this).ToPtr().WithName(name);
 }
 
+CommandPtr Command::Traced(std::string_view name) && {
+  return std::move(*this).ToPtr().Traced(name);
+}
+
 void Command::Schedule() {
   CommandScheduler::GetInstance().Schedule(this);
 }

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandPtr.cpp

@@ -5,10 +5,12 @@
 #include "frc2/command/CommandPtr.h"
 
 #include <memory>
+#include <string>
 #include <utility>
 #include <vector>
 
 #include <frc/Errors.h>
+#include <frc/Tracer.h>
 
 #include "frc2/command/CommandScheduler.h"
 #include "frc2/command/ConditionalCommand.h"
@@ -250,6 +252,30 @@ CommandPtr CommandPtr::WithName(std::string_view name) && {
   return std::move(wrapper).ToPtr();
 }
 
+namespace {
+class TracedCommand : public WrapperCommand {
+ public:
+  TracedCommand(std::unique_ptr<Command>&& command, std::string_view name)
+      : WrapperCommand(std::move(command)), m_name(name) {}
+
+  void Execute() override {
+    frc::Tracer::StartTrace(m_name);
+    WrapperCommand::Execute();
+    frc::Tracer::EndTrace();
+  }
+
+ private:
+  std::string m_name;
+};
+}  // namespace
+
+CommandPtr CommandPtr::Traced(std::string_view name) && {
+  AssertValid();
+  m_ptr = std::make_unique<TracedCommand>(std::move(m_ptr), name);
+  m_ptr->SetName(name);
+  return std::move(*this);
+}
+
 Command* CommandPtr::get() const& {
   AssertValid();
   return m_ptr.get();

wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp

@@ -13,6 +13,7 @@
 #include <frc/RobotBase.h>
 #include <frc/RobotState.h>
 #include <frc/TimedRobot.h>
+#include <frc/Tracer.h>
 #include <frc/livewindow/LiveWindow.h>
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
@@ -71,7 +72,9 @@ static bool ContainsKey(const TMap& map, TKey keyToCheck) {
 
 CommandScheduler::CommandScheduler()
     : m_impl(new Impl), m_watchdog(frc::TimedRobot::kDefaultPeriod, [] {
-        std::puts("CommandScheduler loop time overrun.");
+        std::puts(
+            "CommandScheduler loop overrun, check NetworkTables for timing "
+            "info");
       }) {
   HAL_Report(HALUsageReporting::kResourceType_Command,
              HALUsageReporting::kCommand2_Scheduler);
@@ -154,7 +157,6 @@ void CommandScheduler::Schedule(Command* command) {
     for (auto&& action : m_impl->initActions) {
       action(*command);
     }
-    m_watchdog.AddEpoch(command->GetName() + ".Initialize()");
   }
 }
 
@@ -179,24 +181,29 @@ void CommandScheduler::Run() {
     return;
   }
 
+  frc::Tracer::StartTrace("CommandScheduler");
   m_watchdog.Reset();
 
   // Run the periodic method of all registered subsystems.
   for (auto&& subsystem : m_impl->subsystems) {
-    subsystem.getFirst()->Periodic();
+    frc::Tracer::TraceFunc(
+        subsystem.getFirst()->GetName() + "Periodic",
+        std::bind(&Subsystem::Periodic, subsystem.getFirst()));
     if constexpr (frc::RobotBase::IsSimulation()) {
-      subsystem.getFirst()->SimulationPeriodic();
+      frc::Tracer::TraceFunc(
+          subsystem.getFirst()->GetName() + "SimulationPeriodic",
+          std::bind(&Subsystem::SimulationPeriodic, subsystem.getFirst()));
     }
-    m_watchdog.AddEpoch(subsystem.getFirst()->GetName() + ".Periodic()");
   }
 
   // Cache the active instance to avoid concurrency problems if SetActiveLoop()
   // is called from inside the button bindings.
   frc::EventLoop* loopCache = m_impl->activeButtonLoop;
   // Poll buttons for new commands to add.
-  loopCache->Poll();
-  m_watchdog.AddEpoch("buttons.Run()");
+  frc::Tracer::TraceFunc("PollButtons",
+                         std::bind(&frc::EventLoop::Poll, loopCache));
 
+  frc::Tracer::StartTrace("Commands");
   m_impl->inRunLoop = true;
   bool isDisabled = frc::RobotState::IsDisabled();
   // Run scheduled commands, remove finished commands.
@@ -210,8 +217,6 @@ void CommandScheduler::Run() {
     for (auto&& action : m_impl->executeActions) {
       action(*command);
     }
-    m_watchdog.AddEpoch(command->GetName() + ".Execute()");
-
     if (command->IsFinished()) {
       m_impl->endingCommands.insert(command);
       command->End(false);
@@ -224,8 +229,6 @@ void CommandScheduler::Run() {
       for (auto&& requirement : command->GetRequirements()) {
         m_impl->requirements.erase(requirement);
       }
-
-      m_watchdog.AddEpoch(command->GetName() + ".End(false)");
     }
   }
   m_impl->inRunLoop = false;
@@ -249,11 +252,9 @@ void CommandScheduler::Run() {
       Schedule({subsystem.getSecond().get()});
     }
   }
+  frc::Tracer::EndTrace();
 
   m_watchdog.Disable();
-  if (m_watchdog.IsExpired()) {
-    m_watchdog.PrintEpochs();
-  }
 }
 
 void CommandScheduler::RegisterSubsystem(Subsystem* subsystem) {
@@ -356,7 +357,6 @@ void CommandScheduler::Cancel(Command* command,
       m_impl->requirements.erase(requirement.first);
     }
   }
-  m_watchdog.AddEpoch(command->GetName() + ".End(true)");
 }
 
 void CommandScheduler::Cancel(Command* command) {
@@ -432,10 +432,6 @@ void CommandScheduler::Enable() {
   m_impl->disabled = false;
 }
 
-void CommandScheduler::PrintWatchdogEpochs() {
-  m_watchdog.PrintEpochs();
-}
-
 void CommandScheduler::OnCommandInitialize(Action action) {
   m_impl->initActions.emplace_back(std::move(action));
 }