Allow loop epochs to be logged to DataLog and published to NT

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

@@ -9,6 +9,7 @@
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.util.datalog.DataLog;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
@@ -124,6 +125,31 @@ public void close() {
     LiveWindow.setDisabledListener(null);
   }
 
+  /**
+   * Starts publishing loop timings to NetworkTables. Subsequent calls will do nothing.
+   *
+   * <p>This will publish how long it takes for methods in the command-based framework to execute;
+   * periodic methods in subsystems, execute methods in commands, etc.
+   *
+   * @param topicName The NetworkTables topic to publish to
+   */
+  public void publishLoopTimingsToNetworkTables(String topicName) {
+    m_watchdog.publishToNetworkTables(topicName);
+  }
+
+  /**
+   * Starts logging loop timings to the data log. Subsequent calls will do nothing.
+   *
+   * <p>This will log how long it takes for methods in the command-based framework to execute;
+   * periodic methods in subsystems, execute methods in commands, etc.
+   *
+   * @param dataLog The data log to log loop timings to
+   * @param entry The name of the entry to log to
+   */
+  public void startLoopTimingsDataLog(DataLog dataLog, String entry) {
+    m_watchdog.startDataLog(dataLog, entry);
+  }
+
   /**
    * Get the default button poll.
    *

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

@@ -96,6 +96,16 @@ void CommandScheduler::SetPeriod(units::second_t period) {
   m_watchdog.SetTimeout(period);
 }
 
+void CommandScheduler::PublishLoopTimingsToNetworkTables(
+    std::string_view topicName) {
+  m_watchdog.PublishToNetworkTables(topicName);
+}
+
+void CommandScheduler::StartLoopTimingsDataLog(wpi::log::DataLog& dataLog,
+                                               std::string_view entry) {
+  m_watchdog.StartDataLog(dataLog, entry);
+}
+
 frc::EventLoop* CommandScheduler::GetActiveButtonLoop() const {
   return m_impl->activeButtonLoop;
 }

wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h

@@ -16,6 +16,7 @@
 #include <frc/Watchdog.h>
 #include <frc/event/EventLoop.h>
 #include <units/time.h>
+#include <wpi/DataLog.h>
 #include <wpi/FunctionExtras.h>
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
@@ -58,6 +59,30 @@ class CommandScheduler final : public wpi::Sendable,
    */
   void SetPeriod(units::second_t period);
 
+  /**
+   * Starts publishing loop timings to NetworkTables. Subsequent calls will do
+   * nothing.
+   *
+   * This will publish how long it takes for methods in the command-based
+   * framework to execute; periodic methods in subsystems, execute methods in
+   * commands, etc.
+   * @param topicName The NetworkTables topic to publish to
+   */
+  void PublishLoopTimingsToNetworkTables(std::string_view topicName);
+
+  /**
+   * Starts logging loop timings to the data log. Subsequent calls will do
+   * nothing.
+   *
+   * This will log how long it takes for methods in the command-based
+   * framework to execute; periodic methods in subsystems, execute methods in
+   * commands, etc.
+   * @param dataLog The data log to log epochs to
+   * @param entry The name of the entry to log to
+   */
+  void StartLoopTimingsDataLog(wpi::log::DataLog& dataLog,
+                               std::string_view entry);
+
   /**
    * Get the active button poll.
    *

wpilibc/src/main/native/cpp/IterativeRobotBase.cpp

@@ -93,6 +93,16 @@ void IterativeRobotBase::TeleopExit() {}
 
 void IterativeRobotBase::TestExit() {}
 
+void IterativeRobotBase::PublishLoopTimingsToNetworkTables(
+    std::string_view topicName) {
+  m_watchdog.PublishToNetworkTables(topicName);
+}
+
+void IterativeRobotBase::StartLoopTimingsDataLog(wpi::log::DataLog& dataLog,
+                                                 std::string_view entry) {
+  m_watchdog.StartDataLog(dataLog, entry);
+}
+
 void IterativeRobotBase::SetNetworkTablesFlushEnabled(bool enabled) {
   m_ntFlushEnabled = enabled;
 }

wpilibc/src/main/native/cpp/Tracer.cpp

@@ -16,6 +16,26 @@ Tracer::Tracer() {
   ResetTimer();
 }
 
+void Tracer::PublishToNetworkTables(std::string_view topicName) {
+  if (m_publishNT) {
+    return;
+  }
+  m_publishNT = true;
+  m_inst = nt::NetworkTableInstance::GetDefault();
+  m_ntTopic = topicName;
+  m_publisherCache = wpi::StringMap<std::shared_ptr<nt::IntegerPublisher>>(10);
+}
+
+void Tracer::StartDataLog(wpi::log::DataLog& dataLog, std::string_view entry) {
+  if (m_dataLogEnabled) {
+    return;
+  }
+  m_dataLogEnabled = true;
+  m_dataLog = &dataLog;
+  m_dataLogEntry = entry;
+  m_entryCache = wpi::StringMap<int>(10);
+}
+
 void Tracer::ResetTimer() {
   m_startTime = hal::fpga_clock::now();
 }
@@ -27,7 +47,32 @@ void Tracer::ClearEpochs() {
 
 void Tracer::AddEpoch(std::string_view epochName) {
   auto currentTime = hal::fpga_clock::now();
-  m_epochs[epochName] = currentTime - m_startTime;
+  auto epoch = currentTime - m_startTime;
+  m_epochs[epochName] = epoch;
+  if (m_publishNT) {
+    // Topics are cached with the epoch name as the key, and the publisher
+    // object as the value
+    if (m_publisherCache.count(epochName) == 0) {
+      // Create and prep the epoch publisher
+      auto topic =
+          m_inst.GetIntegerTopic(fmt::format("{}/{}", m_ntTopic, epochName));
+      auto pub = std::make_shared<nt::IntegerPublisher>(topic.Publish());
+      m_publisherCache.insert(std::pair(epochName, pub));
+    }
+    m_publisherCache.lookup(epochName)->Set(epoch.count());
+  }
+  if (m_dataLogEnabled) {
+    // Epochs are cached with the epoch name as the key, and the entry index as
+    // the value
+    if (m_entryCache.count(epochName) == 0) {
+      // Start a data log entry
+      int entryIndex = m_dataLog->Start(
+          fmt::format("{}/{}", m_dataLogEntry, epochName), "int64");
+      // Cache the entry index with the epoch name as the key
+      m_entryCache.insert(std::pair(epochName, entryIndex));
+    }
+    m_dataLog->AppendInteger(m_entryCache.lookup(epochName), epoch.count(), 0);
+  }
   m_startTime = currentTime;
 }
 

wpilibc/src/main/native/cpp/Watchdog.cpp

@@ -164,6 +164,15 @@ Watchdog& Watchdog::operator=(Watchdog&& rhs) {
   return *this;
 }
 
+void Watchdog::PublishToNetworkTables(std::string_view topicName) {
+  m_tracer.PublishToNetworkTables(topicName);
+}
+
+void Watchdog::StartDataLog(wpi::log::DataLog& dataLog,
+                            std::string_view entry) {
+  m_tracer.StartDataLog(dataLog, entry);
+}
+
 units::second_t Watchdog::GetTime() const {
   return Timer::GetFPGATimestamp() - m_startTime;
 }

wpilibc/src/main/native/include/frc/IterativeRobotBase.h

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <units/time.h>
+#include <wpi/DataLog.h>
 
 #include "frc/RobotBase.h"
 #include "frc/Watchdog.h"
@@ -204,6 +205,34 @@ class IterativeRobotBase : public RobotBase {
    */
   virtual void TestExit();
 
+  /**
+   * Starts publishing loop timings to NetworkTables. Subsequent calls will do
+   * nothing.
+   *
+   * This will publish the execution time of method calls in the robot loop;
+   * methods like robot periodic and init methods, various NT
+   * updates (SmartDashboard.update(), Shuffleboard.updateValues(),
+   * LiveWindow.updateValues()) etc.
+   *
+   * @param topicName The NetworkTables topic to publish to
+   */
+  void PublishLoopTimingsToNetworkTables(std::string_view topicName);
+
+  /**
+   * Starts logging loop timings to the data log. Subsequent calls will do
+   * nothing.
+   *
+   * This will publish the execution time of method calls in the robot loop;
+   * methods like robot periodic and init methods, various NT
+   * updates (SmartDashboard.update(), Shuffleboard.updateValues(),
+   * LiveWindow.updateValues()) etc.
+   *
+   * @param dataLog The data log to log epochs to
+   * @param entry The name of the entry to log to
+   */
+  void StartLoopTimingsDataLog(wpi::log::DataLog& dataLog,
+                               std::string_view entry);
+
   /**
    * Enables or disables flushing NetworkTables every loop iteration.
    * By default, this is enabled.

wpilibc/src/main/native/include/frc/Tracer.h

@@ -5,9 +5,13 @@
 #pragma once
 
 #include <chrono>
+#include <memory>
 #include <string_view>
 
 #include <hal/cpp/fpga_clock.h>
+#include <networktables/IntegerTopic.h>
+#include <networktables/NetworkTableInstance.h>
+#include <wpi/DataLog.h>
 #include <wpi/StringMap.h>
 
 namespace wpi {
@@ -30,6 +34,23 @@ class Tracer {
    */
   Tracer();
 
+  /**
+   * Starts publishing added epochs to NetworkTables. Subsequent calls will do
+   * nothing.
+   *
+   * @param topicName The NetworkTables topic to publish to
+   */
+  void PublishToNetworkTables(std::string_view topicName);
+
+  /**
+   * Starts logging added epochs to the data log. Subsequent calls will do
+   * nothing.
+   *
+   * @param dataLog The data log to log epochs to
+   * @param entry The name of the entry to log to
+   */
+  void StartDataLog(wpi::log::DataLog& dataLog, std::string_view entry);
+
   /**
    * Restarts the epoch timer.
    */
@@ -67,6 +88,14 @@ class Tracer {
 
   hal::fpga_clock::time_point m_startTime;
   hal::fpga_clock::time_point m_lastEpochsPrintTime = hal::fpga_clock::epoch();
+  bool m_publishNT = false;
+  nt::NetworkTableInstance m_inst;
+  std::string_view m_ntTopic;
+  wpi::StringMap<std::shared_ptr<nt::IntegerPublisher>> m_publisherCache;
+  bool m_dataLogEnabled = false;
+  wpi::log::DataLog* m_dataLog;
+  std::string_view m_dataLogEntry;
+  wpi::StringMap<int> m_entryCache;
 
   wpi::StringMap<std::chrono::nanoseconds> m_epochs;
 };

wpilibc/src/main/native/include/frc/Watchdog.h

@@ -45,6 +45,23 @@ class Watchdog {
   Watchdog(Watchdog&& rhs);
   Watchdog& operator=(Watchdog&& rhs);
 
+  /**
+   * Starts publishing added epochs to NetworkTables. Subsequent calls will do
+   * nothing.
+   *
+   * @param topicName The NetworkTables topic to publish to
+   */
+  void PublishToNetworkTables(std::string_view topicName);
+
+  /**
+   * Starts logging added epochs to the data log. Subsequent calls will do
+   * nothing.
+   *
+   * @param dataLog The data log to log epochs to
+   * @param entry The name of the entry to log to
+   */
+  void StartDataLog(wpi::log::DataLog& dataLog, std::string_view entry);
+
   /**
    * Returns the time since the watchdog was last fed.
    */

wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.util.datalog.DataLog;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -250,6 +251,33 @@ public void teleopExit() {}
    */
   public void testExit() {}
 
+  /**
+   * Starts publishing loop timings to NetworkTables. Subsequent calls will do nothing.
+   *
+   * <p>This will publish the execution time of method calls in the robot loop; methods like robot
+   * periodic and init methods, various NT updates (SmartDashboard.update(),
+   * Shuffleboard.updateValues(), LiveWindow.updateValues()) etc.
+   *
+   * @param topicName The NetworkTables topic to publish to
+   */
+  public void publishLoopTimingsToNetworkTables(String topicName) {
+    m_watchdog.publishToNetworkTables(topicName);
+  }
+
+  /**
+   * Starts logging loop timings to the data log. Subsequent calls will do nothing.
+   *
+   * <p>This will log the execution time of method calls in the robot loop; methods like robot
+   * periodic and init methods, various NT updates (SmartDashboard.update(),
+   * Shuffleboard.updateValues(), LiveWindow.updateValues()) etc.
+   *
+   * @param dataLog The data log to log epochs to
+   * @param entry The name of the entry to log to
+   */
+  public void startLoopTimingsDataLog(DataLog dataLog, String entry) {
+    m_watchdog.startDataLog(dataLog, entry);
+  }
+
   /**
    * Enables or disables flushing NetworkTables every loop iteration. By default, this is enabled.
    *