From 11238494445c9bfa566159b8685ada3707d72e5f Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 18 Apr 2024 18:40:29 +0200 Subject: [PATCH] Add maximum admissible time step parameter to YarpRobotLoggerDevice (#839) --- CHANGELOG.md | 3 +- .../ergoCubGazeboV1/yarp-robot-logger.xml | 1 + .../ergoCubGazeboV1_1/yarp-robot-logger.xml | 1 + .../robots/ergoCubSN000/yarp-robot-logger.xml | 1 + .../robots/ergoCubSN001/yarp-robot-logger.xml | 1 + .../robots/iCubGazeboV3/yarp-robot-logger.xml | 1 + .../robots/iCubGenova09/yarp-robot-logger.xml | 1 + .../BipedalLocomotion/YarpRobotLoggerDevice.h | 4 + .../src/YarpRobotLoggerDevice.cpp | 107 ++++++++++-------- 9 files changed, 70 insertions(+), 50 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5f4cf2a853..00c5035ee3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -27,8 +27,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 diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml index c3b4a8bcb5..47905bd907 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml @@ -10,6 +10,7 @@ BSD-3-Clause license. --> mp4v /yarp-robot-logger ("ergoCubGazeboV1/yarprobotinterface") + 1.0 600.0 diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml index 0a21ffbf97..638c786b96 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml @@ -10,6 +10,7 @@ BSD-3-Clause license. --> mp4v /yarp-robot-logger ("ergoCubGazeboV1/yarprobotinterface") + 1.0 600.0 diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml index b7f282478c..3482e634bd 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml @@ -19,6 +19,7 @@ BSD-3-Clause license. --> /yarp-robot-logger ("ergocub-torso/yarprobotinterface") ("ssh ergocub@10.0.2.2" "ssh ergocub@10.0.2.3") + 1.0 1800.0 diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml index 75c846eaa8..c5e53db684 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml @@ -19,6 +19,7 @@ BSD-3-Clause license. --> /yarp-robot-logger ("ergoCubSN001/yarprobotinterface") ("ssh ergocub@10.0.2.2" "ssh ergocub@10.0.2.3") + 1.0 1800.0 diff --git a/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml index 49a5a85445..380340677f 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml @@ -7,6 +7,7 @@ BSD-3-Clause license. --> icubSim 0.01 /yarp-robot-logger + 1.0 600.0 diff --git a/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml index 19c500b63c..6ce85c31ac 100644 --- a/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml +++ b/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml @@ -11,6 +11,7 @@ BSD-3-Clause license. --> /yarp-robot-logger ("icub-head/yarprobotinterface") ("ssh icub@10.0.0.2" "ssh icub@10.0.0.150") + 1.0 600.0 diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h index a839b157ae..24e2b87eeb 100644 --- a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h +++ b/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; using gyro_t = Eigen::Matrix; using accelerometer_t = Eigen::Matrix; diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp index ded7e5a071..e163baf1cc 100644 --- a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp +++ b/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& metadataNames) +bool YarpRobotLoggerDevice::initMetadata(const std::string& nameKey, + const std::vector& 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(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(m_previousTimestamp), + std::chrono::duration(t), + std::chrono::duration(t - m_previousTimestamp)); + return; + } + } + + const double time = std::chrono::duration(t).count(); std::string signalFullName = ""; std::string rtSignalFullName = ""; @@ -1366,8 +1375,7 @@ void YarpRobotLoggerDevice::run() m_vectorCollectionRTDataServer.clearData(); Eigen::Matrix 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,