Use std::chrono::nanoseconds in clock and AdvanceableRunner

CHANGELOG.md

@@ -5,6 +5,7 @@ All notable changes to this project are documented in this file.
 ### Added
 
 ### Changed
+- Use `std::chorno::nanoseconds` in `clock` and `AdvanceableRunner` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/702)
 
 ### Fixed
 

bindings/python/System/include/BipedalLocomotion/bindings/System/IClock.h

@@ -26,17 +26,17 @@ class IClockTrampoline : public IClockBase
 public:
     using IClockBase::IClockBase;
 
-    std::chrono::duration<double> now() override
+    std::chrono::nanoseconds now() override
     {
-        PYBIND11_OVERLOAD_PURE(std::chrono::duration<double>, IClockBase, now);
+        PYBIND11_OVERLOAD_PURE(std::chrono::nanoseconds, IClockBase, now);
     }
 
-    void sleepFor(const std::chrono::duration<double>& sleepDuration) override
+    void sleepFor(const std::chrono::nanoseconds& sleepDuration) override
     {
         PYBIND11_OVERLOAD_PURE_NAME(void, IClockBase, "sleep_for", sleepFor);
     }
 
-    void sleepUntil(const std::chrono::duration<double>& time) override
+    void sleepUntil(const std::chrono::nanoseconds& time) override
     {
         PYBIND11_OVERLOAD_PURE_NAME(void, IClockBase, "sleep_until", seepUntil);
     }

devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp

@@ -1018,13 +1018,14 @@ bool YarpRobotLoggerDevice::hasSubstring(const std::string& str,
 
 void YarpRobotLoggerDevice::lookForNewLogs()
 {
+    using namespace std::chrono_literals;
     yarp::profiler::NetworkProfiler::ports_name_set yarpPorts;
     constexpr auto textLoggingPortPrefix = "/log/";
 
     auto time = BipedalLocomotion::clock().now();
     auto oldTime = time;
     auto wakeUpTime = time;
-    const auto lookForNewLogsPeriod = std::chrono::duration<double>(2);
+    const auto lookForNewLogsPeriod = 2s;
     m_lookForNewLogsIsRunning = true;
 
     while (m_lookForNewLogsIsRunning)
@@ -1072,7 +1073,9 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit
     auto oldTime = time;
     auto wakeUpTime = time;
     writer.recordVideoIsRunning = true;
-    const auto recordVideoPeriod = std::chrono::duration<double>(1 / double(writer.fps));
+    const auto recordVideoPeriod = std::chrono::duration_cast<std::chrono::nanoseconds>(
+        std::chrono::duration<double>(1.0 / double(writer.fps)));
+
     unsigned int imageIndex = 0;
 
     while (writer.recordVideoIsRunning)

src/System/RosImplementation/include/BipedalLocomotion/System/RosClock.h

@@ -58,19 +58,19 @@ class RosClock final : public IClock
      * @note `BipedalLocomotion::clock().now().count()` returns a double containing the seconds
      * since epoch.
      */
-    std::chrono::duration<double> now() final;
+    std::chrono::nanoseconds now() final;
 
     /**
      * Blocks the execution of the current thread for at least the specified sleepDuration.
      * @param time duration to sleep
      */
-    void sleepFor(const std::chrono::duration<double>& sleepDuration) final;
+    void sleepFor(const std::chrono::nanoseconds& sleepDuration) final;
 
     /**
      * Blocks the execution of the current thread until specified sleepTime has been reached.
      * @param time to block until
      */
-    void sleepUntil(const std::chrono::duration<double>& sleepTime) final;
+    void sleepUntil(const std::chrono::nanoseconds& sleepTime) final;
 
     /**
      * Provides a hint to the implementation to reschedule the execution of threads, allowing other

src/System/RosImplementation/src/RosClock.cpp

@@ -14,18 +14,18 @@
 
 using namespace BipedalLocomotion::System;
 
-std::chrono::duration<double> RosClock::now()
+std::chrono::nanoseconds RosClock::now()
 {
-    return std::chrono::duration<double>(m_clock.now().seconds());
+    return std::chrono::nanoseconds(m_clock.now().nanoseconds());
 }
 
-void RosClock::sleepFor(const std::chrono::duration<double>& sleepDuration)
+void RosClock::sleepFor(const std::chrono::nanoseconds& sleepDuration)
 {
     // std::chrono::duration store the time in second
     m_clock.sleep_for(sleepDuration);
 }
 
-void RosClock::sleepUntil(const std::chrono::duration<double>& sleepTime)
+void RosClock::sleepUntil(const std::chrono::nanoseconds& sleepTime)
 {
     m_clock.sleep_for(sleepTime - this->now());
 }

src/System/YarpImplementation/include/BipedalLocomotion/System/YarpClock.h

@@ -49,19 +49,19 @@ class YarpClock final : public IClock
      * @note `BipedalLocomotion::clock().now().count()` returns a double containing the seconds
      * since epoch.
      */
-    std::chrono::duration<double> now() final;
+    std::chrono::nanoseconds now() final;
 
     /**
      * Blocks the execution of the current thread for at least the specified sleepDuration.
      * @param time duration to sleep
      */
-    void sleepFor(const std::chrono::duration<double>& sleepDuration) final;
+    void sleepFor(const std::chrono::nanoseconds& sleepDuration) final;
 
     /**
      * Blocks the execution of the current thread until specified sleepTime has been reached.
      * @param time to block until
      */
-    void sleepUntil(const std::chrono::duration<double>& sleepTime) final;
+    void sleepUntil(const std::chrono::nanoseconds& sleepTime) final;
 
     /**
      * Provides a hint to the implementation to reschedule the execution of threads, allowing other

src/System/YarpImplementation/src/YarpClock.cpp

@@ -11,22 +11,24 @@
 
 using namespace BipedalLocomotion::System;
 
-std::chrono::duration<double> YarpClock::now()
+std::chrono::nanoseconds YarpClock::now()
 {
-    // The yarp now function  returns the time in seconds
-    return std::chrono::duration<double>(yarp::os::Time::now());
+    // The yarp now function returns the time in seconds
+    return std::chrono::duration_cast<std::chrono::nanoseconds>(
+        std::chrono::duration<double>(yarp::os::Time::now()));
 }
 
-void YarpClock::sleepFor(const std::chrono::duration<double>& sleepDuration)
+void YarpClock::sleepFor(const std::chrono::nanoseconds& sleepDuration)
 {
     // std::chrono::duration store the time in second
-    yarp::os::Time::delay(sleepDuration.count());
+    yarp::os::Time::delay(std::chrono::duration<double>(sleepDuration).count());
 }
 
-void YarpClock::sleepUntil(const std::chrono::duration<double>& sleepTime)
+void YarpClock::sleepUntil(const std::chrono::nanoseconds& sleepTime)
 {
-    yarp::os::Time::delay(
-        (sleepTime - std::chrono::duration<double>(yarp::os::Time::now())).count());
+    yarp::os::Time::delay((std::chrono::duration<double>(sleepTime)
+                           - std::chrono::duration<double>(yarp::os::Time::now()))
+                              .count());
 }
 
 void YarpClock::yield()

src/System/include/BipedalLocomotion/System/AdvanceableRunner.h

@@ -50,9 +50,7 @@ template <class _Advanceable> class AdvanceableRunner
 
 private:
     bool m_isInitialized{false}; /**< True if the runner is initialized */
-    std::chrono::duration<double> m_dT{std::chrono::duration_values<double>::zero()}; /**< Period of
-                                                                                         the runner
-                                                                                       */
+    std::chrono::nanoseconds m_dT{std::chrono::nanoseconds::zero()}; /**< Period of the runner */
     std::atomic<bool> m_isRunning{false}; /**> If True the runner is running */
 
     std::unique_ptr<_Advanceable> m_advanceable; /**< Advanceable contained in the runner */
@@ -63,25 +61,26 @@ template <class _Advanceable> class AdvanceableRunner
     struct Info
     {
         unsigned int deadlineMiss{0}; /**< Number of deadline miss */
-        double dT{-1.0}; /**< Period of the runner */
+        std::chrono::nanoseconds dT{std::chrono::nanoseconds::zero()}; /**< Period of the runner */
         std::string name{}; /**< Name associated to the runner */
     };
     std::mutex m_infoMutex; /**< Mutex used to protect the information struct */
     Info m_info; /**< Information struct */
 
 public:
+    // clang-format off
     /**
      * Initialize the AdvanceableRunner class
      * @param handler pointer to a parameter handler
      * @note The following parameters are required
-     * |   Parameter Name   |   Type   |                              Description | Mandatory |
+     * |   Parameter Name   |   Type   |                              Description                              | Mandatory |
      * |:------------------:|:--------:|:---------------------------------------------------------------------:|:---------:|
-     * |   `sampling_time`  | `double` |   Strictly positive number rapresenting the sampling time
-     * in seconds  |    Yes    | | `enable_telemetry` |  `bool`  | If True some additional
-     * information are stored. Default value `false` |     No    |
+     * |   `sampling_time`  | `double` |   Strictly positive number representing the sampling time in seconds  |    Yes    |
+     * | `enable_telemetry` |  `bool`  | If True some additional information are stored. Default value `false` |     No    |
      * @return true in case of success, false otherwise.
      */
     bool initialize(std::weak_ptr<const ParametersHandler::IParametersHandler> handler);
+    // clang-format on
 
     /**
      * Set the advanceable inside the runner.
@@ -164,13 +163,7 @@ bool AdvanceableRunner<_Advanceable>::initialize(
         return false;
     }
 
-    if (m_info.dT <= 0)
-    {
-        log()->error("{} The sampling time must be a strictly positive number.", errorPrefix);
-        return false;
-    }
-
-    m_dT = std::chrono::duration<double>(m_info.dT);
+    m_dT = m_info.dT;
 
     if (!ptr->getParameter("enable_telemetry", m_isTelemetryEnabled))
     {
@@ -335,7 +328,7 @@ AdvanceableRunner<_Advanceable>::run(std::optional<std::reference_wrapper<Barrie
         }
 
         log()->info("{} - {}: Closing the AdvanceableRunner.", logPrefix, m_info.name);
-        if(m_isTelemetryEnabled)
+        if (m_isTelemetryEnabled)
         {
             unsigned int deadlineMiss = 0;
 

src/System/include/BipedalLocomotion/System/IClock.h

@@ -44,19 +44,19 @@ class IClock
      * @note `BipedalLocomotion::clock().now().count()` returns a double containing the seconds
      * since epoch.
      */
-    virtual std::chrono::duration<double> now() = 0;
+    virtual std::chrono::nanoseconds now() = 0;
 
     /**
      * Blocks the execution of the current thread for at least the specified sleepDuration.
      * @param time duration to sleep
      */
-    virtual void sleepFor(const std::chrono::duration<double>& sleepDuration) = 0;
+    virtual void sleepFor(const std::chrono::nanoseconds& sleepDuration) = 0;
 
     /**
      * Blocks the execution of the current thread until specified sleepTime has been reached.
      * @param time to block until
      */
-    virtual void sleepUntil(const std::chrono::duration<double>& time) = 0;
+    virtual void sleepUntil(const std::chrono::nanoseconds& time) = 0;
 
     /**
      * Provides a hint to the implementation to reschedule the execution of threads, allowing other

src/System/include/BipedalLocomotion/System/SharedResource.h

@@ -26,7 +26,7 @@ namespace System
 template <class T> class SharedResource
 {
     T m_resource; /**< The resource */
-    std::mutex m_mutex; /**< The mutex used to protect the resource */
+    mutable std::mutex m_mutex; /**< The mutex used to protect the resource */
 
     SharedResource() = default;
 
@@ -36,13 +36,13 @@ template <class T> class SharedResource
     /**
      * Set the resource
      */
-    void set(const T& resource);
+    inline void set(const T& resource);
 
     /**
      * Get the resource.
      * @return the copy of the object inside the shared resource.
      */
-    T get();
+    inline T get() const;
 
     /**
      * Method used to create a shared resource.
@@ -53,13 +53,13 @@ template <class T> class SharedResource
 
 template <class T> void SharedResource<T>::set(const T& resource)
 {
-    const std::lock_guard<std::mutex> lock(m_mutex);
+    std::lock_guard<std::mutex> lock(m_mutex);
     m_resource = resource;
 }
 
-template <class T> T SharedResource<T>::get()
+template <class T> T SharedResource<T>::get() const
 {
-    const std::lock_guard<std::mutex> lock(m_mutex);
+    std::lock_guard<std::mutex> lock(m_mutex);
     return m_resource;
 }
 

src/System/include/BipedalLocomotion/System/StdClock.h

@@ -39,21 +39,21 @@ class StdClock final : public IClock
      * @note `BipedalLocomotion::clock().now().count()` returns a double containing the seconds
      * since epoch
      */
-    std::chrono::duration<double> now() final;
+    std::chrono::nanoseconds now() final;
 
     /**
      * Blocks the execution of the current thread for at least the specified sleepDuration.
      * @param time duration to sleep
      * @note std::this_tread::sleep_for() function is used.
      */
-    void sleepFor(const std::chrono::duration<double>& sleepDuration) final;
+    void sleepFor(const std::chrono::nanoseconds& sleepDuration) final;
 
     /**
      * Blocks the execution of the current thread until specified sleepTime has been reached.
      * @param time to block until
      * @note sleepTime is the duration since epoch
      */
-    void sleepUntil(const std::chrono::duration<double>& sleepTime) final;
+    void sleepUntil(const std::chrono::nanoseconds& sleepTime) final;
 
     /**
      * Provides a hint to the implementation to reschedule the execution of threads, allowing other

src/System/src/StdClock.cpp

@@ -12,17 +12,17 @@
 
 using namespace BipedalLocomotion::System;
 
-std::chrono::duration<double> StdClock::now()
+std::chrono::nanoseconds StdClock::now()
 {
     return std::chrono::system_clock::now().time_since_epoch();
 }
 
-void StdClock::sleepFor(const std::chrono::duration<double>& sleepDuration)
+void StdClock::sleepFor(const std::chrono::nanoseconds& sleepDuration)
 {
     std::this_thread::sleep_for(sleepDuration);
 }
 
-void StdClock::sleepUntil(const std::chrono::duration<double>& time)
+void StdClock::sleepUntil(const std::chrono::nanoseconds& time)
 {
     std::this_thread::sleep_for(time - std::chrono::system_clock::now().time_since_epoch());
 }

src/System/tests/AdvanceableRunnerTest.cpp

@@ -5,6 +5,8 @@
  * General Public License v2.1 or any later version.
  */
 
+#include <chrono>
+
 // Catch2
 #include <catch2/catch_test_macros.hpp>
 
@@ -16,6 +18,7 @@
 
 using namespace BipedalLocomotion::System;
 using namespace BipedalLocomotion::ParametersHandler;
+using namespace std::chrono_literals;
 
 class DummyBlock : public Source<bool>
 {
@@ -48,9 +51,10 @@ class DummyBlock : public Source<bool>
 
 TEST_CASE("Test Block")
 {
+    using namespace std::chrono_literals;
     std::shared_ptr param = std::make_shared<StdImplementation>();
 
-    param->setParameter("sampling_time", 0.001);
+    param->setParameter("sampling_time", 1ms);
     param->setParameter("enable_telemetry", true);
     param->setParameter("name", "Runner");
 
@@ -83,7 +87,7 @@ TEST_CASE("Test Block")
 
         while (!output0->get() || !output1->get())
         {
-            BipedalLocomotion::clock().sleepFor(std::chrono::duration<double>(0.01));
+            BipedalLocomotion::clock().sleepFor(10ms);
         }
 
         // close the runner
@@ -124,7 +128,7 @@ TEST_CASE("Test Block")
 
         while (!output0->get() || !output1->get())
         {
-            BipedalLocomotion::clock().sleepFor(std::chrono::duration<double>(0.01));
+            BipedalLocomotion::clock().sleepFor(10ms);
         }
 
         // close the runner