Add maximum admissible time step parameter to YarpRobotLoggerDevice

CHANGELOG.md

@@ -26,8 +26,9 @@ All notable changes to this project are documented in this file.
 
 ### Fixed
 - Fix the barrier logic for threads synchronization (https://github.com/ami-iit/bipedal-locomotion-framework/pull/811)
-- InstallBasicPackageFiles: Fix bug of OVERRIDE_MODULE_PATH that corrupt CMAKE_MODULE_PATH values set by blf transitive dependencies (https://github.com/ami-iit/bipedal-locomotion-framework/pull/827)
+- InstallBasicPackageFiles: Fix bug of OVERRIDE_MODULE_PATH that corrupt `CMAKE_MODULE_PATH` values set by blf transitive dependencies (https://github.com/ami-iit/bipedal-locomotion-framework/pull/827)
 - InstallBasicPackageFiles: Fix compatibility with CMake 3.29.1 (https://github.com/ami-iit/bipedal-locomotion-framework/pull/835)
+- Fix `YARPRobotLoggerDevice` excessively long time horizon for signals logged with `YARP_CLOCK` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/839)
 
 ### Removed
 

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml

@@ -10,6 +10,7 @@ BSD-3-Clause license. -->
   <param name="video_codec_code">mp4v</param>
   <param name="port_prefix">/yarp-robot-logger</param>
   <param name="text_logging_subnames">("ergoCubGazeboV1/yarprobotinterface")</param>
+  <param name="maximum_admissible_time_step">1.0</param>
 
   <group name="Telemetry">
     <param name="save_period">600.0</param>

devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml

@@ -10,6 +10,7 @@ BSD-3-Clause license. -->
   <param name="video_codec_code">mp4v</param>
   <param name="port_prefix">/yarp-robot-logger</param>
   <param name="text_logging_subnames">("ergoCubGazeboV1/yarprobotinterface")</param>
+  <param name="maximum_admissible_time_step">1.0</param>
 
   <group name="Telemetry">
     <param name="save_period">600.0</param>

devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml

@@ -19,6 +19,7 @@ BSD-3-Clause license. -->
   <param name="port_prefix">/yarp-robot-logger</param>
   <param name="text_logging_subnames">("ergocub-torso/yarprobotinterface")</param>
   <param name="code_status_cmd_prefixes">("ssh [email protected]" "ssh [email protected]")</param>
+  <param name="maximum_admissible_time_step">1.0</param>
 
   <group name="Telemetry">
     <param name="save_period">1800.0</param>

devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml

@@ -19,6 +19,7 @@ BSD-3-Clause license. -->
   <param name="port_prefix">/yarp-robot-logger</param>
   <param name="text_logging_subnames">("ergoCubSN001/yarprobotinterface")</param>
   <param name="code_status_cmd_prefixes">("ssh [email protected]" "ssh [email protected]")</param>
+  <param name="maximum_admissible_time_step">1.0</param>
 
   <group name="Telemetry">
     <param name="save_period">1800.0</param>

devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml

@@ -7,6 +7,7 @@ BSD-3-Clause license. -->
   <param name="robot">icubSim</param>
   <param name="sampling_period_in_s">0.01</param>
   <param name="port_prefix">/yarp-robot-logger</param>
+  <param name="maximum_admissible_time_step">1.0</param>
 
   <group name="Telemetry">
     <param name="save_period">600.0</param>

devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml

@@ -11,6 +11,7 @@ BSD-3-Clause license. -->
   <param name="port_prefix">/yarp-robot-logger</param>
   <param name="text_logging_subnames">("icub-head/yarprobotinterface")</param>
   <param name="code_status_cmd_prefixes">("ssh [email protected]" "ssh [email protected]")</param>
+  <param name="maximum_admissible_time_step">1.0</param>
 
   <group name="Telemetry">
     <param name="save_period">600.0</param>

devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h

@@ -53,6 +53,10 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
     virtual void run() final;
 
 private:
+    std::chrono::nanoseconds m_previousTimestamp;
+    std::chrono::nanoseconds m_acceptableStep{std::chrono::nanoseconds::max()};
+    bool m_firstRun{true};
+
     using ft_t = Eigen::Matrix<double, 6, 1>;
     using gyro_t = Eigen::Matrix<double, 3, 1>;
     using accelerometer_t = Eigen::Matrix<double, 3, 1>;

devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp

@@ -141,8 +141,7 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config)
         std::string remote;
         rtParameters->getParameter("remote", remote);
         log()->info("{} Activating Real Time Logging on yarp port: {}", logPrefix, remote);
-    }
-    else
+    } else
     {
         log()->info("{} Real time logging not activated", logPrefix);
     }
@@ -167,6 +166,14 @@ bool YarpRobotLoggerDevice::open(yarp::os::Searchable& config)
                     logPrefix);
     }
 
+    if (!params->getParameter("maximum_admissible_time_step", m_acceptableStep))
+    {
+        log()->info("{} Unable to get the 'maximum_admissible_time_step' parameter. The time "
+                    "step will be set to the default value. Default value: {}",
+                    logPrefix,
+                    m_acceptableStep);
+    }
+
     if (!this->setupRobotSensorBridge(params->getGroup("RobotSensorBridge")))
     {
         return false;
@@ -628,7 +635,8 @@ bool YarpRobotLoggerDevice::setupRobotCameraBridge(
     return true;
 }
 
-bool YarpRobotLoggerDevice::initMetadata(const std::string& nameKey, const std::vector<std::string>& metadataNames)
+bool YarpRobotLoggerDevice::initMetadata(const std::string& nameKey,
+                                         const std::vector<std::string>& metadataNames)
 {
     bool ok = m_bufferManager.addChannel({nameKey, {metadataNames.size(), 1}, metadataNames});
     std::string rtNameKey = robotRtRootName + treeDelim + nameKey;
@@ -639,7 +647,6 @@ bool YarpRobotLoggerDevice::initMetadata(const std::string& nameKey, const std::
     return ok;
 }
 
-
 bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly)
 {
     constexpr auto logPrefix = "[YarpRobotLoggerDevice::attachAll]";
@@ -686,16 +693,13 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly)
     if (m_sendDataRT)
     {
         char* tmp = std::getenv("YARP_ROBOT_NAME");
-        std::string metadataName
-            = robotRtRootName + treeDelim + robotName;
+        std::string metadataName = robotRtRootName + treeDelim + robotName;
         m_vectorCollectionRTDataServer.populateMetadata(metadataName, {std::string(tmp)});
 
-        metadataName
-            = robotRtRootName + treeDelim + timestampsName;
+        metadataName = robotRtRootName + treeDelim + timestampsName;
         m_vectorCollectionRTDataServer.populateMetadata(metadataName, {timestampsName});
 
-        std::string rtMetadataName = robotRtRootName + treeDelim
-                                    + robotDescriptionList;
+        std::string rtMetadataName = robotRtRootName + treeDelim + robotDescriptionList;
         m_vectorCollectionRTDataServer.populateMetadata(rtMetadataName, joints);
     }
 
@@ -729,8 +733,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly)
     {
         for (const auto& sensorName : m_robotSensorBridge->getSixAxisForceTorqueSensorsList())
         {
-            std::string fullFTSensorName
-                = ftsName + treeDelim + sensorName;
+            std::string fullFTSensorName = ftsName + treeDelim + sensorName;
             ok = ok && initMetadata(fullFTSensorName, ftElementNames);
         }
     }
@@ -739,41 +742,34 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly)
     {
         for (const auto& sensorName : m_robotSensorBridge->getGyroscopesList())
         {
-            std::string fullGyroSensorName
-                = gyrosName + treeDelim + sensorName;
+            std::string fullGyroSensorName = gyrosName + treeDelim + sensorName;
             ok = ok && initMetadata(fullGyroSensorName, gyroElementNames);
         }
 
         for (const auto& sensorName : m_robotSensorBridge->getLinearAccelerometersList())
         {
-            std::string fullAccelerometerSensorName
-                = accelerometersName + treeDelim + sensorName;
+            std::string fullAccelerometerSensorName = accelerometersName + treeDelim + sensorName;
             ok = ok && initMetadata(fullAccelerometerSensorName, AccelerometerElementNames);
         }
 
         for (const auto& sensorName : m_robotSensorBridge->getOrientationSensorsList())
         {
-            std::string fullOrientationsSensorName
-                = orientationsName + treeDelim + sensorName;
+            std::string fullOrientationsSensorName = orientationsName + treeDelim + sensorName;
             ok = ok && initMetadata(fullOrientationsSensorName, orientationElementNames);
         }
 
         for (const auto& sensorName : m_robotSensorBridge->getMagnetometersList())
         {
-            std::string fullMagnetometersSensorName
-                = magnetometersName + treeDelim + sensorName;
+            std::string fullMagnetometersSensorName = magnetometersName + treeDelim + sensorName;
             ok = ok && initMetadata(fullMagnetometersSensorName, magnetometerElementNames);
         }
 
         // an IMU contains a gyro accelerometer and an orientation sensor
         for (const auto& sensorName : m_robotSensorBridge->getIMUsList())
         {
-            std::string fullAccelerometerSensorName
-                = accelerometersName + treeDelim + sensorName;
-            std::string fullGyroSensorName
-                = gyrosName + treeDelim + sensorName;
-            std::string fullOrientationsSensorName
-                = orientationsName + treeDelim + sensorName;
+            std::string fullAccelerometerSensorName = accelerometersName + treeDelim + sensorName;
+            std::string fullGyroSensorName = gyrosName + treeDelim + sensorName;
+            std::string fullOrientationsSensorName = orientationsName + treeDelim + sensorName;
             ok = ok && initMetadata(fullAccelerometerSensorName, AccelerometerElementNames);
             ok = ok && initMetadata(fullGyroSensorName, gyroElementNames);
             ok = ok && initMetadata(fullOrientationsSensorName, orientationElementNames);
@@ -784,8 +780,8 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly)
     {
         for (const auto& cartesianWrenchName : m_robotSensorBridge->getCartesianWrenchesList())
         {
-            std::string fullCartesianWrenchName = cartesianWrenchesName
-                                                  + treeDelim + cartesianWrenchName;
+            std::string fullCartesianWrenchName
+                = cartesianWrenchesName + treeDelim + cartesianWrenchName;
             ok = ok && initMetadata(fullCartesianWrenchName, cartesianWrenchNames);
         }
     }
@@ -794,8 +790,7 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly)
     {
         for (const auto& sensorName : m_robotSensorBridge->getTemperatureSensorsList())
         {
-            std::string fullTemperatureSensorName
-                = temperatureName + treeDelim + sensorName;
+            std::string fullTemperatureSensorName = temperatureName + treeDelim + sensorName;
             ok = ok && initMetadata(fullTemperatureSensorName, temperatureNames);
         }
     }
@@ -1204,7 +1199,7 @@ void YarpRobotLoggerDevice::lookForNewLogs()
                     yarp::os::Network::connect(port.name, m_textLoggingPortName, "udp");
                 }
             }
-        }   // end of scope for lock guarding text logging port
+        } // end of scope for lock guarding text logging port
 
         // release the CPU
         BipedalLocomotion::clock().yield();
@@ -1343,9 +1338,7 @@ void YarpRobotLoggerDevice::recordVideo(const std::string& cameraName, VideoWrit
 
 void YarpRobotLoggerDevice::run()
 {
-    auto logData = [this](const std::string& name,
-                      const auto& data,
-                      const double time) {
+    auto logData = [this](const std::string& name, const auto& data, const double time) {
         m_bufferManager.push_back(data, time, name);
         std::string rtName = robotRtRootName + treeDelim + name;
         if (m_sendDataRT)
@@ -1355,7 +1348,23 @@ void YarpRobotLoggerDevice::run()
     };
 
     constexpr auto logPrefix = "[YarpRobotLoggerDevice::run]";
-    const double time = std::chrono::duration<double>(BipedalLocomotion::clock().now()).count();
+    const std::chrono::nanoseconds t = BipedalLocomotion::clock().now();
+
+    if (!m_firstRun)
+    {
+        if (t - m_previousTimestamp > m_acceptableStep)
+        {
+            log()->warn("{} The time step is too big. The previous timestamp is {} and the current "
+                        "timestamp is {}. The time step is {}.",
+                        logPrefix,
+                        std::chrono::duration<double>(m_previousTimestamp),
+                        std::chrono::duration<double>(t),
+                        std::chrono::duration<double>(t - m_previousTimestamp));
+            return;
+        }
+    }
+
+    const double time = std::chrono::duration<double>(t).count();
     std::string signalFullName = "";
     std::string rtSignalFullName = "";
 
@@ -1366,8 +1375,7 @@ void YarpRobotLoggerDevice::run()
         m_vectorCollectionRTDataServer.clearData();
         Eigen::Matrix<double, 1, 1> timeData;
         timeData << time;
-        rtSignalFullName
-            = robotRtRootName + treeDelim + timestampsName;
+        rtSignalFullName = robotRtRootName + treeDelim + timestampsName;
         m_vectorCollectionRTDataServer.populateData(rtSignalFullName, timeData);
     }
 
@@ -1477,8 +1485,7 @@ void YarpRobotLoggerDevice::run()
             if (m_robotSensorBridge->getLinearAccelerometerMeasurement(sensorName,
                                                                        m_acceloremeterBuffer))
             {
-                signalFullName
-                    = accelerometersName + treeDelim + sensorName;
+                signalFullName = accelerometersName + treeDelim + sensorName;
                 logData(signalFullName, m_acceloremeterBuffer, time);
             }
         }
@@ -1532,8 +1539,7 @@ void YarpRobotLoggerDevice::run()
         {
             if (m_robotSensorBridge->getCartesianWrench(cartesianWrenchName, m_ftBuffer))
             {
-                signalFullName = cartesianWrenchesName + treeDelim
-                             + cartesianWrenchName;
+                signalFullName = cartesianWrenchesName + treeDelim + cartesianWrenchName;
                 logData(signalFullName, m_ftBuffer, time);
             }
         }
@@ -1566,15 +1572,13 @@ void YarpRobotLoggerDevice::run()
                     }
                 }
                 signal.dataArrived = true;
-            }
-            else
+            } else
             {
                 for (const auto& [key, vector] : collection->vectors)
                 {
                     signalFullName = signal.signalName + treeDelim + key;
                     logData(signalFullName, vector, time);
                 }
-
             }
         }
     }
@@ -1604,18 +1608,20 @@ 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;
+                    signalFullName = msg.portSystem + "::" + msg.portPrefix + "::" + msg.processName
+                                     + "::p" + msg.processPID;
 
                     // matlab does not support the character - as a key of a struct
                     findAndReplaceAll(signalFullName, "-", "_");
 
                     // if it is the first time this signal is seen by the device the channel is
                     // added
-                    if (m_textLogsStoredInManager.find(signalFullName) == m_textLogsStoredInManager.end())
+                    if (m_textLogsStoredInManager.find(signalFullName)
+                        == m_textLogsStoredInManager.end())
                     {
                         m_bufferManager.addChannel({signalFullName, {1, 1}});
                         m_textLogsStoredInManager.insert(signalFullName);
@@ -1627,12 +1633,15 @@ void YarpRobotLoggerDevice::run()
                 break;
             }
         }
-    }   // end of lock guard scope for text logging port
+    } // end of lock guard scope for text logging port
 
     if (m_sendDataRT)
     {
         m_vectorCollectionRTDataServer.sendData();
     }
+
+    m_previousTimestamp = t;
+    m_firstRun = false;
 }
 
 bool YarpRobotLoggerDevice::saveCallback(const std::string& fileName,