Add Periodic Thread class and reduce YarpLoggerDevice latency

CHANGELOG.md

@@ -8,6 +8,7 @@ All notable changes to this project are documented in this file.
 - Implement low-pass filter for estimated friction torques in `JointTorqueControlDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/892)
 - Add `blf-motor-current-tracking.py` application (https://github.com/ami-iit/bipedal-locomotion-framework/pull/894)
 - Add the possibility to initialize the base position and the feet pose in the `unicycleTrajectoryGenerator` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/887)
+- Add a `Periodic Thread Class` and add options to reduce `YarpLoggerDevice` latency (https://github.com/ami-iit/bipedal-locomotion-framework/pull/889)
 
 ### Changed
 

devices/YarpRobotLoggerDevice/README.md

@@ -138,3 +138,28 @@ robot-log-visualizer
 Then, you can open the mat file generated by the logger and explore the logged data as in the following video:
 
 [robot-log-visualizer.webm](https://github.com/ami-iit/robot-log-visualizer/assets/16744101/3fd5c516-da17-4efa-b83b-392b5ce1383b)
+
+## How to reduce latency
+If you are experiencing some latency in logged data, you can try to enable different real time scheduling strategies through the `real_time_scheduling_strategy` in the [yarp-robot-logger.xml](https://github.com/ami-iit/bipedal-locomotion-framework/blob/master/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml). For example:
+
+```xml
+<?xml version="1.0" encoding="UTF-8" ?>
+<device  xmlns:xi="http://www.w3.org/2001/XInclude" name="yarp-robot-logger" type="YarpRobotLoggerDevice">
+  <param name="robot">ergocub</param>
+  <param name="sampling_period_in_s">0.001</param>
+  <param name="real_time_scheduling_strategy">early_wakeup_and_fifo</param>
+```
+
+The available strategies are `none` (which is the default and means no strategy will be applied), `early_wakeup`, `fifo`, and `early_wakeup_and_fifo`.
+The `early_wakeup` strategy makes the YarpLoggerDevice run thread wake up earlier and then busy wait until it is time to resume.
+The `fifo` strategy increases the YarpLoggerDevice run thread priority and changes its scheduling policy to SCHED-FIFO.
+The `early_wakeup_and_fifo` combines the `early_wakeup` and `fifo` strategies.
+
+Note that the `fifo` and `early_wakeup_and_fifo` strategies are only available for Linux. Moreover, you should run the `YarpLoggerDevice` with elevated privileges when deploying them.
+For example:
+
+```console
+sudo -E <yarprobotinterface_directory>/yarprobotinterface --config launch-yarp-robot-logger.xml
+```
+
+with `<yarprobotinterface_directory>` being the directory containing `yarprobotinterface`, that you can determine with the terminal command `which yarprobotinterface`.

devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h

@@ -21,13 +21,13 @@
 #include <yarp/dev/IMultipleWrapper.h>
 #include <yarp/os/Bottle.h>
 #include <yarp/os/BufferedPort.h>
-#include <yarp/os/PeriodicThread.h>
 #include <yarp/sig/Vector.h>
 
 #include <robometry/BufferManager.h>
 
 #include <BipedalLocomotion/RobotInterface/YarpCameraBridge.h>
 #include <BipedalLocomotion/RobotInterface/YarpSensorBridge.h>
+#include <BipedalLocomotion/System/PeriodicThread.h>
 #include <BipedalLocomotion/YarpUtilities/VectorsCollection.h>
 #include <BipedalLocomotion/YarpUtilities/VectorsCollectionClient.h>
 #include <BipedalLocomotion/YarpUtilities/VectorsCollectionServer.h>
@@ -37,20 +37,17 @@ namespace BipedalLocomotion
 
 class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
                               public yarp::dev::IMultipleWrapper,
-                              public yarp::os::PeriodicThread
+                              public BipedalLocomotion::System::PeriodicThread
 {
 public:
-    YarpRobotLoggerDevice(double period,
-                          yarp::os::ShouldUseSystemClock useSystemClock
-                          = yarp::os::ShouldUseSystemClock::No);
-    YarpRobotLoggerDevice();
+    YarpRobotLoggerDevice(double period = 0.01);
     ~YarpRobotLoggerDevice();
 
     virtual bool open(yarp::os::Searchable& config) final;
     virtual bool close() final;
     virtual bool attachAll(const yarp::dev::PolyDriverList& poly) final;
     virtual bool detachAll() final;
-    virtual void run() final;
+    virtual bool run() final;
 
 private:
     std::chrono::nanoseconds m_previousTimestamp;
@@ -177,6 +174,15 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
     std::mutex m_bufferManagerMutex;
     robometry::BufferManager m_bufferManager;
 
+    enum class RealTimeSchedulingStrategy
+    {
+        None,
+        EarlyWakeUp,
+        FIFO,
+        EarlyWakeUpAndFIFO,
+    };
+    RealTimeSchedulingStrategy m_RealTimeSchedulingStrategy{RealTimeSchedulingStrategy::None};
+
     void lookForNewLogs();
     void lookForExogenousSignals();
 

devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp

@@ -15,6 +15,9 @@
 #include <sstream>
 #include <string>
 #include <tuple>
+#ifdef __linux__
+#include <sched.h>
+#endif
 
 #include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
 #include <BipedalLocomotion/ParametersHandler/YarpImplementation.h>
@@ -93,23 +96,9 @@ void YarpRobotLoggerDevice::VectorsCollectionSignal::disconnect()
     }
 }
 
-YarpRobotLoggerDevice::YarpRobotLoggerDevice(double period,
-                                             yarp::os::ShouldUseSystemClock useSystemClock)
-    : yarp::os::PeriodicThread(period, useSystemClock)
-{
-    // Use the yarp clock in blf
-    BipedalLocomotion::System::ClockBuilder::setFactory(
-        std::make_shared<BipedalLocomotion::System::YarpClockFactory>());
-
-    // the logging message are streamed using yarp
-    BipedalLocomotion::TextLogging::LoggerBuilder::setFactory(
-        std::make_shared<BipedalLocomotion::TextLogging::YarpLoggerFactory>());
-
-    m_sendDataRT = false;
-}
-
-YarpRobotLoggerDevice::YarpRobotLoggerDevice()
-    : yarp::os::PeriodicThread(0.01, yarp::os::ShouldUseSystemClock::No)
+YarpRobotLoggerDevice::YarpRobotLoggerDevice(double period)
+    : BipedalLocomotion::System::PeriodicThread(
+          std::chrono::nanoseconds(static_cast<int64_t>(period * 1e9)))
 {
     // Use the yarp clock in blf
     BipedalLocomotion::System::ClockBuilder::setFactory(
@@ -149,7 +138,79 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config)
     double devicePeriod{0.01};
     if (params->getParameter("sampling_period_in_s", devicePeriod))
     {
-        this->setPeriod(devicePeriod);
+        this->setPeriod(std::chrono::nanoseconds(static_cast<int64_t>(devicePeriod)));
+    }
+
+    std::string realTimeSchedulingStrategy{"none"};
+    if (!params->getParameter("real_time_scheduling_strategy", realTimeSchedulingStrategy))
+    {
+        log()->info("{} The 'real_time_scheduling_strategy' parameter is not found. "
+                    "YarpLoggerDevice will run without any real time strategy.",
+                    logPrefix);
+    }
+    if (realTimeSchedulingStrategy == "none")
+    {
+        m_RealTimeSchedulingStrategy = RealTimeSchedulingStrategy::None;
+    } else if (realTimeSchedulingStrategy == "early_wakeup")
+    {
+        m_RealTimeSchedulingStrategy = RealTimeSchedulingStrategy::EarlyWakeUp;
+    } else if (realTimeSchedulingStrategy == "fifo")
+    {
+        m_RealTimeSchedulingStrategy = RealTimeSchedulingStrategy::FIFO;
+    } else if (realTimeSchedulingStrategy == "early_wakeup_and_fifo")
+    {
+        m_RealTimeSchedulingStrategy = RealTimeSchedulingStrategy::EarlyWakeUpAndFIFO;
+    } else
+    {
+        log()->error("{} The 'real_time_scheduling_strategy' parameter is not valid. Available "
+                     "options are 'none', 'early_wakeup', 'fifo', 'early_wakeup_and_fifo'.",
+                     logPrefix);
+        return false;
+    }
+
+    switch (m_RealTimeSchedulingStrategy)
+    {
+
+    case RealTimeSchedulingStrategy::None:
+        break;
+
+    case RealTimeSchedulingStrategy::EarlyWakeUp:
+        if (!this->enableEarlyWakeUp())
+        {
+            log()->error("{} Failed to enable the early wake up.", logPrefix);
+            return false;
+        }
+        break;
+#ifdef __linux__
+    case RealTimeSchedulingStrategy::FIFO:
+        if (!this->setPolicy(SCHED_FIFO, 80))
+        {
+            log()->error("{} Failed to set the policy to SCHED_FIFO.", logPrefix);
+            return false;
+        }
+        log()->info("{} The FIFO scheduling policy is set.", logPrefix);
+        break;
+    case RealTimeSchedulingStrategy::EarlyWakeUpAndFIFO:
+        if (!this->enableEarlyWakeUp())
+        {
+            log()->error("{} Failed to enable the early wake up.", logPrefix);
+            return false;
+        }
+        if (!this->setPolicy(SCHED_FIFO, 80))
+        {
+            log()->error("{} Failed to set the policy to SCHED_FIFO.", logPrefix);
+            return false;
+        }
+        break;
+#else
+    case RealTimeSchedulingStrategy::FIFO:
+        log()->error("{} The FIFO scheduling policy is not supported on this OS.", logPrefix);
+        return false;
+    case RealTimeSchedulingStrategy::EarlyWakeUpAndFIFO:
+        log()->error("{} The EarlyWakeUpAndFIFO scheduling policy is not supported on this OS.",
+                     logPrefix);
+        return false;
+#endif
     }
 
     if (!params->getParameter("text_logging_subnames", m_textLoggingSubnames))
@@ -1189,22 +1250,21 @@ void YarpRobotLoggerDevice::lookForExogenousSignals()
                         continue;
                     }
 
-                    log()->info("[YarpRobotLoggerDevice::lookForExogenousSignals] Attempt to get the "
-                                "metadata for the vectors collection signal named: {}",
+                    log()->info("[YarpRobotLoggerDevice::lookForExogenousSignals] Attempt to get "
+                                "the metadata for the vectors collection signal named: {}",
                                 name);
 
                     if (!signal.client.getMetadata(signal.metadata))
                     {
-                        log()->warn("[YarpRobotLoggerDevice::lookForExogenousSignals] Unable to get "
-                                    "the metadata for the signal named: {}. The exogenous signal will "
-                                    "not contain the metadata.",
+                        log()->warn("[YarpRobotLoggerDevice::lookForExogenousSignals] Unable to "
+                                    "get the metadata for the signal named: {}. The exogenous "
+                                    "signal will not contain the metadata.",
                                     name);
                     }
                 }
             }
 
             signal.connected = connectionDone;
-
         }
     };
 
@@ -1429,7 +1489,7 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit
     }
 }
 
-void YarpRobotLoggerDevice::run()
+bool YarpRobotLoggerDevice::run()
 {
     auto logData = [this](const std::string& name, const auto& data, const double time) {
         m_bufferManager.push_back(data, time, name);
@@ -1453,7 +1513,7 @@ void YarpRobotLoggerDevice::run()
                         std::chrono::duration<double>(m_previousTimestamp),
                         std::chrono::duration<double>(t),
                         std::chrono::duration<double>(t - m_previousTimestamp));
-            return;
+            return true;
         }
     }
 
@@ -1718,12 +1778,11 @@ void YarpRobotLoggerDevice::run()
         yarp::os::Bottle* b = m_textLoggingPort.read(false);
         if (b != nullptr)
         {
-            msg = BipedalLocomotion::TextLoggingEntry::deserializeMessage(*b,
-                                                                            std::to_string(time));
+            msg = BipedalLocomotion::TextLoggingEntry::deserializeMessage(*b, std::to_string(time));
             if (msg.isValid)
             {
                 signalFullName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName
-                                    + "::p" + msg.processPID;
+                                 + "::p" + msg.processPID;
 
                 // matlab does not support the character - as a key of a struct
                 findAndReplaceAll(signalFullName, "-", "_");
@@ -1736,7 +1795,7 @@ void YarpRobotLoggerDevice::run()
                     m_bufferManager.addChannel({signalFullName, {1, 1}});
                     m_textLogsStoredInManager.insert(signalFullName);
                 }
-                //Not using logData here because we don't want to stream the data to RT
+                // Not using logData here because we don't want to stream the data to RT
                 m_bufferManager.push_back(msg, time, signalFullName);
             }
             bufferportSize = m_textLoggingPort.getPendingReads();
@@ -1753,6 +1812,8 @@ void YarpRobotLoggerDevice::run()
 
     m_previousTimestamp = t;
     m_firstRun = false;
+
+    return true;
 }
 
 bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName,

examples/CMakeLists.txt

@@ -1 +1,2 @@
 add_subdirectory(UnicycleTrajectoryGenerator)
+add_subdirectory(PeriodicThread)

examples/PeriodicThread/CMakeLists.txt

@@ -0,0 +1,5 @@
+add_executable(PeriodicThreadExample main.cpp)
+
+target_link_libraries(PeriodicThreadExample
+BipedalLocomotion::System
+BipedalLocomotion::TextLogging)

examples/PeriodicThread/main.cpp

@@ -0,0 +1,71 @@
+#include <chrono>
+#include <cstdlib>
+#include <string>
+
+#include <BipedalLocomotion/System/Barrier.h>
+#include <BipedalLocomotion/System/Clock.h>
+#include <BipedalLocomotion/System/PeriodicThread.h>
+#include <BipedalLocomotion/TextLogging/Logger.h>
+
+class Thread : public BipedalLocomotion::System::PeriodicThread
+{
+public:
+    std::string name = "Thread";
+    Thread(std::string name);
+    ~Thread();
+    bool run() override;
+
+    bool threadInit() override;
+};
+
+bool Thread::run()
+{
+    BipedalLocomotion::log()->info("[Thread::run] {} is running.", name);
+    return true;
+}
+
+Thread::Thread(std::string name)
+    : name(name)
+    , BipedalLocomotion::System::PeriodicThread(std::chrono::milliseconds(1000)){};
+
+Thread::~Thread()
+{
+    BipedalLocomotion::log()->info("[Thread::~Thread] {} is destroyed.", name);
+};
+
+bool Thread::threadInit()
+{
+    BipedalLocomotion::log()->info("[Thread::threadInit] {} is initialized.", name);
+    return true;
+}
+
+int main()
+{
+
+    auto barrier = BipedalLocomotion::System::Barrier::create(2);
+
+    // Thread thread;
+    auto thread1 = Thread("Thread 1");
+    thread1.start(barrier);
+
+    BipedalLocomotion::clock().sleepFor(std::chrono::milliseconds(2000));
+
+    auto thread2 = Thread("Thread 2");
+    thread2.setPeriod(std::chrono::seconds(2));
+    thread2.start(barrier);
+
+    while (thread1.isRunning() || thread2.isRunning())
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(4000));
+        thread1.stop();
+        BipedalLocomotion::log()->info("[main] Thread 1 is asked to stop.");
+
+        if (!thread1.isRunning())
+        {
+            thread2.stop();
+            BipedalLocomotion::log()->info("[main] Thread 2 is asked to stop.");
+        }
+    }
+    BipedalLocomotion::log()->info("[main] About to exit the application.");
+    return EXIT_SUCCESS;
+}

src/System/CMakeLists.txt

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