YarpLoggerDevice inherits from blf PeriodicThread

ami-iit · Oct 7, 2024 · b92d1ca · b92d1ca
1 parent 1441c85
commit b92d1ca
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 37 deletions.
diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/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;

diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp
@@ -93,23 +93,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 +135,7 @@ 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)));
     }
 
     if (!params->getParameter("text_logging_subnames", m_textLoggingSubnames))
@@ -1189,22 +1175,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 +1414,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 +1438,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 +1703,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 +1720,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 +1737,8 @@ void YarpRobotLoggerDevice::run()
 
     m_previousTimestamp = t;
     m_firstRun = false;
+
+    return true;
 }
 
 bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName,