Implement System::TimeProfiler (#826)

The original code has been taken from https://github.com/robotology/walking-controllers/tree/edc8984998043c98182f726e17822deb2a7b1915/src/TimeProfiler
-

--------

Co-authored-by: Giulio Romualdi <[email protected]>

CHANGELOG.md

@@ -11,6 +11,7 @@ All notable changes to this project are documented in this file.
 - Added a member function to set the gravity vector of the `CentroidalDynamics` Continuous Dynamical System (https://github.com/ami-iit/bipedal-locomotion-framework/pull/821).
 - Add the possibility to set the exogenous signal for the `IK::CoMTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/824)
 - Add getJointTorques to python binding in SensorBridge (https://github.com/ami-iit/bipedal-locomotion-framework/pull/825)
+- Add `System::TimeProfiler` class (https://github.com/ami-iit/bipedal-locomotion-framework/pull/826)
 
 ### Changed
 - 🤖 [ergoCubSN001] Add logging of the wrist and fix the name of the waist imu (https://github.com/ami-iit/bipedal-locomotion-framework/pull/810)

src/System/CMakeLists.txt

@@ -17,11 +17,11 @@ if(FRAMEWORK_COMPILE_System)
                            ${H_PREFIX}/IClock.h ${H_PREFIX}/StdClock.h ${H_PREFIX}/Clock.h
                            ${H_PREFIX}/SharedResource.h ${H_PREFIX}/AdvanceableRunner.h
                            ${H_PREFIX}/QuitHandler.h
-                           ${H_PREFIX}/Barrier.h
+                           ${H_PREFIX}/Barrier.h ${H_PREFIX}/TimeProfiler.h
                            ${H_PREFIX}/WeightProvider.h ${H_PREFIX}/ConstantWeightProvider.h
     SOURCES                src/VariablesHandler.cpp src/LinearTask.cpp
                            src/StdClock.cpp src/Clock.cpp src/QuitHandler.cpp src/Barrier.cpp
-                           src/ConstantWeightProvider.cpp
+                           src/ConstantWeightProvider.cpp src/TimeProfiler.cpp
     PUBLIC_LINK_LIBRARIES  BipedalLocomotion::ParametersHandler Eigen3::Eigen
     SUBDIRECTORIES         tests YarpImplementation RosImplementation
     )

src/System/include/BipedalLocomotion/System/TimeProfiler.h

@@ -0,0 +1,103 @@
+/**
+ * @file TimeProfiler.h
+ * @authors Guglielmo Cervettini, Giulio Romualdi
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_SYSTEM_TIME_PROFILER_H
+#define BIPEDAL_LOCOMOTION_SYSTEM_TIME_PROFILER_H
+
+// std
+#include <chrono>
+#include <map>
+#include <memory>
+
+namespace BipedalLocomotion
+{
+namespace System
+{
+/**
+ * Timer is a simple class that can be used to measure the time between two events.
+ */
+class Timer
+{
+    std::chrono::time_point<std::chrono::steady_clock> m_initTime; /**< Init time. */
+    std::chrono::time_point<std::chrono::steady_clock> m_endTime; /**< End time. */
+    std::chrono::nanoseconds m_averageDuration; /**< Average duration. */
+
+public:
+    /**
+     * Reset the average duration.
+     */
+    void resetAverageDuration();
+
+    /**
+     * Set initial time.
+     */
+    void setInitTime();
+
+    /**
+     * Set final time.
+     */
+    void setEndTime();
+
+    /**
+     * Evaluate the average duration.
+     */
+    void evaluateDuration();
+
+    /**
+     * Get the average duration.
+     * @return average duration.
+     */
+    const std::chrono::nanoseconds& getAverageDuration() const;
+};
+
+/**
+ * TimeProfiler is a simple class that can be used to profile the code.
+ */
+class TimeProfiler
+{
+    int m_counter; /**< Counter useful to print the profiling quantities only every m_maxCounter
+                      times. */
+    int m_maxCounter; /**< The profiling quantities will be printed every maxCounter cycles. */
+    std::map<std::string, Timer> m_timers; /**< Dictionary that contains all the timers. */
+public:
+    /**
+     * Set the output period.
+     * @param maxCounter is the period (expressed in cycles).
+     */
+    void setPeriod(int maxCounter);
+
+    /**
+     * Add a new timer
+     * @param key is the name of the timer.
+     * @return true/false in case of success/failure.
+     */
+    bool addTimer(const std::string& key);
+
+    /**
+     * Set the init time for the timer named "key"
+     * @param key is the name of the timer.
+     * @return true/false in case of success/failure.
+     */
+    bool setInitTime(const std::string& key);
+
+    /**
+     * Set the end time for the timer named "key"
+     * @param key is the name of the timer.
+     * @return true/false in case of success/failure.
+     */
+    bool setEndTime(const std::string& key);
+
+    /**
+     * Print the profiling quantities.
+     */
+    void profiling();
+};
+
+}; // namespace System
+}; // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_SYSTEM_TIME_PROFILER_H

src/System/src/TimeProfiler.cpp

@@ -0,0 +1,114 @@
+/**
+ * @file TimeProfiler.cpp
+ * @authors Guglielmo Cervettini, Giulio Romualdi
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <chrono>
+#include <string>
+
+#include <BipedalLocomotion/System/TimeProfiler.h>
+#include <BipedalLocomotion/TextLogging/Logger.h>
+
+using namespace BipedalLocomotion::System;
+
+const std::chrono::nanoseconds& Timer::getAverageDuration() const
+{
+    return m_averageDuration;
+}
+
+void Timer::resetAverageDuration()
+{
+    m_averageDuration = std::chrono::nanoseconds(0);
+}
+
+void Timer::setInitTime()
+{
+    m_initTime = std::chrono::steady_clock::now();
+}
+
+void Timer::setEndTime()
+{
+    m_endTime = std::chrono::steady_clock::now();
+}
+
+void Timer::evaluateDuration()
+{
+    const std::chrono::nanoseconds duration = m_endTime - m_initTime;
+    m_averageDuration += duration;
+}
+
+void TimeProfiler::setPeriod(int maxCounter)
+{
+    m_maxCounter = maxCounter;
+}
+
+bool TimeProfiler::addTimer(const std::string& key)
+{
+    auto timer = m_timers.find(key);
+    if (timer != m_timers.end())
+    {
+        log()->error("[TimeProfiler::addTimer] The timer named {} already exists.", key);
+        return false;
+    }
+
+    m_timers.insert(std::make_pair(key, Timer()));
+    return true;
+}
+
+bool TimeProfiler::setInitTime(const std::string& key)
+{
+    auto timer = m_timers.find(key);
+    if (timer == m_timers.end())
+    {
+        log()->error("[TimeProfiler::setInitTime] Unable to find the timer named {}.", key);
+        return false;
+    }
+
+    timer->second.setInitTime();
+    return true;
+}
+
+bool TimeProfiler::setEndTime(const std::string& key)
+{
+    auto timer = m_timers.find(key);
+    if (timer == m_timers.end())
+    {
+        log()->error("[TimeProfiler::setEndTime] Unable to find the timer named {}.", key);
+        return false;
+    }
+
+    timer->second.setEndTime();
+    return true;
+}
+
+void TimeProfiler::profiling()
+{
+    std::string infoStream;
+    m_counter++;
+    for (auto& [key, timer] : m_timers)
+    {
+        timer.evaluateDuration();
+        if (m_counter != m_maxCounter)
+        {
+            continue;
+        }
+
+        const std::chrono::nanoseconds& durationInNs = timer.getAverageDuration();
+
+        // convert the duration in ns to ms
+        const auto durationInMs
+            = std::chrono::duration_cast<std::chrono::duration<double, std::milli>>(durationInNs)
+              / double(m_counter);
+
+        infoStream += key + ": " + std::to_string(durationInMs.count()) + " ms ";
+        timer.resetAverageDuration();
+    }
+
+    if (m_counter == m_maxCounter)
+    {
+        m_counter = 0;
+        log()->info("[TimeProfiler::profiling] {}", infoStream);
+    }
+}