Add timeout to wait for data available in interface

CHANGELOG.md

@@ -15,6 +15,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Changed
 - The license of the repo changed to [`BSD-3-Clause`](https://spdx.org/licenses/BSD-3-Clause.html) (https://github.com/robotology/human-dynamics-estimation/pull/350).
+- Devices wait for data available in IHumanState and IHumanDynamics interfaces instead of returning error (https://github.com/robotology/human-dynamics-estimation/pull/357).
 
 ### Fixed
 - [`HumanLogger`] Removed IWrapper from implemented interface to fix checks on log flags and attached interfaces (https://github.com/robotology/human-dynamics-estimation/pull/344)

devices/HumanControlBoard/HumanControlBoard.cpp

@@ -234,10 +234,10 @@ bool HumanControlBoard::impl::attach(yarp::dev::PolyDriver* poly)
     if(!iHumanState && !poly->view(tmpIHumanState)) {
 
         // Check the interface
-        if (tmpIHumanState->getNumberOfJoints() == 0
+        while (tmpIHumanState->getNumberOfJoints() == 0
                 || tmpIHumanState->getNumberOfJoints() != tmpIHumanState->getJointNames().size()) {
-            yError() << "The IHumanState interface might not be ready";
-            return false;
+            yInfo() << LogPrefix << "IHumanState interface waiting for first data. Waiting...";
+            yarp::os::Time::delay(5);
         }
 
         isHumanStateAttached = true;
@@ -249,11 +249,10 @@ bool HumanControlBoard::impl::attach(yarp::dev::PolyDriver* poly)
     if (!iHumanDynamics && poly->view(tmpIHumanDynamics)) {
 
         // Check the interface
-        if (tmpIHumanDynamics->getNumberOfJoints() != 0
+        while (tmpIHumanDynamics->getNumberOfJoints() != 0
             && (tmpIHumanDynamics->getNumberOfJoints() != tmpIHumanDynamics->getJointNames().size())) {
-            yError() << LogPrefix << "The IHumanDynamics interface is not valid."
-                     << "The number of joints should match the number of joint names.";
-            return false;
+            yInfo() << LogPrefix << "IHumanDynamics interface waiting for first data. Waiting...";
+            yarp::os::Time::delay(5);
         }
 
         isHumanDynamicsAttached = true;

devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp

@@ -1235,11 +1235,10 @@ bool HumanDynamicsEstimator::Impl::attach(yarp::dev::PolyDriver* poly)
     // Try to attach IHumanState
     if(!iHumanState && poly->view(tmpIHumanState)){
         // Check the iHumanState interface
-        if (tmpIHumanState->getNumberOfJoints() == 0
+        while (tmpIHumanState->getNumberOfJoints() == 0
             || tmpIHumanState->getNumberOfJoints() != tmpIHumanState->getJointNames().size()) {
-
-            yError() << LogPrefix<<"The IHumanState interface has been providing inconsistent data and might not be ready";
-                return false;
+            yInfo() << LogPrefix << "IHumanState interface waiting for first data. Waiting...";
+                yarp::os::Time::delay(5);
         }
 
         yInfo() << LogPrefix << "Device" << deviceName << "attached successfully as IHumanState interfaces";
@@ -1250,10 +1249,10 @@ bool HumanDynamicsEstimator::Impl::attach(yarp::dev::PolyDriver* poly)
     if(!iHumanWrench && poly->view(tmpIHumanWrench)){
         // Check the interface
         auto numberOfWrenchSources = tmpIHumanWrench->getNumberOfWrenchSources();
-        if ( numberOfWrenchSources == 0 ||
+        while ( numberOfWrenchSources == 0 ||
              numberOfWrenchSources != tmpIHumanWrench->getWrenchSourceNames().size()) {
-            yError() << LogPrefix << "The IHumanWrench interface might not be ready";
-            return false;
+            yInfo() << LogPrefix << "IHumanWrench interface waiting for first data. Waiting...";
+            yarp::os::Time::delay(5);
         }
 
         yInfo() << LogPrefix << "Device" << deviceName << "attached successfully as IHumanWrench";

devices/HumanLogger/HumanLogger.cpp

@@ -17,7 +17,7 @@
 #include <robometry/BufferManager.h>
 
 const std::string LoggerName = "HumanLogger";
-const std::string logPrefix = LoggerName + " :";
+const std::string LogPrefix = LoggerName + " :";
 constexpr double DefaultPeriod = 0.01;
 
 namespace hde {
@@ -98,7 +98,7 @@ void HumanLogger::run()
     if (pImpl->settings.logHumanState)
     {
         if(!pImpl->iHumanState) {
-            yError() << logPrefix << "The IHumanState pointer is null in the driver loop.";
+            yError() << LogPrefix << "The IHumanState pointer is null in the driver loop.";
             askToStop();
             return;
         }
@@ -131,7 +131,7 @@ void HumanLogger::run()
     if (pImpl->settings.logHumanDynamics)
     {
         if(!pImpl->iHumanDynamics) {
-            yError() << logPrefix << "The IHumanDynamics pointer is null in the driver loop.";
+            yError() << LogPrefix << "The IHumanDynamics pointer is null in the driver loop.";
             askToStop();
             return;
         }
@@ -153,7 +153,7 @@ void HumanLogger::run()
 bool HumanLogger::open(yarp::os::Searchable& config)
 {
     if (!config.check("period")) {
-        yInfo() << logPrefix << "Using default period: " << DefaultPeriod << "s";
+        yInfo() << LogPrefix << "Using default period: " << DefaultPeriod << "s";
     }
 
     const double period = config.check("period", yarp::os::Value(DefaultPeriod)).asFloat64();
@@ -162,7 +162,7 @@ bool HumanLogger::open(yarp::os::Searchable& config)
     // Load settings in the class
     bool ok = pImpl->loadSettingsFromConfig(config);
     if (!ok) {
-        yError() << logPrefix << "Problem in loading settings from config.";
+        yError() << LogPrefix << "Problem in loading settings from config.";
         return false;
     }
 
@@ -190,14 +190,14 @@ bool HumanLogger::impl::loadSettingsFromConfig(yarp::os::Searchable& config)
           && config.find("LoggerType").isString()
           && setLoggerType(config.find("LoggerType").asString()) ) ) {
 
-        yError() << logPrefix << "LoggerType not found or not valid";
+        yError() << LogPrefix << "LoggerType not found or not valid";
         return false;
     }
 
     // Display the current logger level
     switch (this->loggerType) {
         case LoggerType::MATLAB: {
-            yInfo() << logPrefix << "LoggerType set to MATLAB";
+            yInfo() << LogPrefix << "LoggerType set to MATLAB";
             break;
         }
         default:
@@ -217,7 +217,7 @@ bool HumanLogger::impl::loadSettingsFromConfig(yarp::os::Searchable& config)
         bufferConfig.filename = config.find(experimentName.c_str()).asString();
     }
     else {
-        yError() << logPrefix << " missing parameter: " << experimentName;
+        yError() << LogPrefix << " missing parameter: " << experimentName;
         return false;
     }
 
@@ -231,7 +231,7 @@ bool HumanLogger::impl::loadSettingsFromConfig(yarp::os::Searchable& config)
         bufferConfig.n_samples = config.find(n_samples.c_str()).asInt32();
     }
     else {
-        yError() << logPrefix << " missing parameter: " << n_samples;
+        yError() << LogPrefix << " missing parameter: " << n_samples;
         return false;
     }
 
@@ -246,7 +246,7 @@ bool HumanLogger::impl::loadSettingsFromConfig(yarp::os::Searchable& config)
             bufferConfig.save_period = config.find(save_period.c_str()).asFloat64();
         }
         else {
-            yError() << logPrefix << " missing parameter: " << save_period;
+            yError() << LogPrefix << " missing parameter: " << save_period;
             return false;
         }
 
@@ -263,7 +263,7 @@ bool HumanLogger::impl::loadSettingsFromConfig(yarp::os::Searchable& config)
 
     if (!(bufferConfig.auto_save || bufferConfig.save_periodically)) {
         yError()
-            << logPrefix
+            << LogPrefix
             << " both auto_save and save_periodically are set to false, nothing will be saved.";
         return false;
     }
@@ -323,7 +323,7 @@ bool HumanLogger::impl::configureBufferManager()
 
     ok = ok && bufferManager.configure(bufferConfig);
     if (ok) {
-        yDebug() << logPrefix << " buffer manager configured successfully.";
+        yDebug() << LogPrefix << " buffer manager configured successfully.";
     }
 
     return ok;
@@ -338,15 +338,15 @@ void HumanLogger::threadRelease() {}
 bool HumanLogger::attachAll(const yarp::dev::PolyDriverList& driverList)
 {
     if (driverList.size() > 2) {
-        yError() << logPrefix << "This wrapper accepts maximum two attached PolyDriver.";
+        yError() << LogPrefix << "This wrapper accepts maximum two attached PolyDriver.";
         return false;
     }
 
     for (int i = 0; i < driverList.size(); i++) {
         yarp::dev::PolyDriver* poly = driverList[i]->poly;
 
         if (!poly) {
-            yError() << logPrefix << "Passed PolyDriver is nullptr.";
+            yError() << LogPrefix << "Passed PolyDriver is nullptr.";
             return false;
         }
 
@@ -359,10 +359,10 @@ bool HumanLogger::attachAll(const yarp::dev::PolyDriverList& driverList)
         if(pImpl->settings.logHumanState && !pImpl->iHumanState && poly->view(tmpIHumanState))
         {
             // Check the interface
-            if (tmpIHumanState->getNumberOfJoints() == 0
+            while (tmpIHumanState->getNumberOfJoints() == 0
                     || tmpIHumanState->getNumberOfJoints() != tmpIHumanState->getJointNames().size()) {
-                yError() << logPrefix <<"The IHumanState interface"<<deviceName<<"might not be ready";
-                return false;
+                yInfo() << LogPrefix << "IHumanState interface waiting for first data. Waiting...";
+                yarp::os::Time::delay(5);
             }
 
             pImpl->iHumanState = tmpIHumanState;
@@ -372,52 +372,52 @@ bool HumanLogger::attachAll(const yarp::dev::PolyDriverList& driverList)
         if(pImpl->settings.logHumanDynamics && !pImpl->iHumanDynamics && poly->view(tmpIHumanDynamics))
         {
             // Check the interface
-            if (tmpIHumanDynamics->getNumberOfJoints() == 0
+            while (tmpIHumanDynamics->getNumberOfJoints() == 0
                     || tmpIHumanDynamics->getNumberOfJoints() != tmpIHumanDynamics->getJointNames().size()) {
-                yError() << logPrefix << "The IHumanDynamics interface"<<deviceName<<"might not be ready";
-                return false;
+                yInfo() << LogPrefix << "IHumanDynamics interface waiting for first data. Waiting...";
+                yarp::os::Time::delay(5);
             }
 
             pImpl->iHumanDynamics = tmpIHumanDynamics;
         }
 
         if(!tmpIHumanState && !tmpIHumanDynamics)
         {
-            yError()<<logPrefix<<"The device"<<deviceName<<"does not implement any of the attachable interfaces!";
+            yError()<<LogPrefix<<"The device"<<deviceName<<"does not implement any of the attachable interfaces!";
             return false;
         }
         else
         {
-            yInfo()<<logPrefix<<"Device"<<deviceName<<"successfully attached";
+            yInfo()<<LogPrefix<<"Device"<<deviceName<<"successfully attached";
         }
     }
 
     // Check IHumanState interface
     if (pImpl->settings.logHumanState && !pImpl->iHumanState)
     {
-        yError()<<logPrefix<<"No IHumanState interface attached, stopping";
+        yError()<<LogPrefix<<"No IHumanState interface attached, stopping";
         return false;
     }
 
     // Check IHumanDynamics interface
     if (pImpl->settings.logHumanDynamics && !pImpl->iHumanDynamics)
     {
-        yError()<<logPrefix<<"No IHumanDynamics interface attached, stopping";
+        yError()<<LogPrefix<<"No IHumanDynamics interface attached, stopping";
         return false;
     }
 
     if (!pImpl->configureBufferManager()) {
-        yError() << logPrefix << "Failed to configure buffer manager for the logger.";
+        yError() << LogPrefix << "Failed to configure buffer manager for the logger.";
         return false;
     }
 
     // Start the periodic thread
     if(!start()) {
-        yError() << logPrefix << "Failed to start the loop.";
+        yError() << LogPrefix << "Failed to start the loop.";
         return false;
     }
 
-    yInfo() << logPrefix << "Successfully attached all the required devices";
+    yInfo() << LogPrefix << "Successfully attached all the required devices";
 
     return true;
 }

devices/HumanWrenchProvider/HumanWrenchProvider.cpp

@@ -1276,11 +1276,11 @@ bool HumanWrenchProvider::Impl::attach(yarp::dev::PolyDriver* poly)
     if (!iHumanState && poly->view(tmpIHumanState)) {
 
         // Check the iHumanState interface
-        if (tmpIHumanState->getNumberOfJoints() == 0
+        while (tmpIHumanState->getNumberOfJoints() == 0
             || tmpIHumanState->getNumberOfJoints() != tmpIHumanState->getJointNames().size()) {
 
-            yError() << LogPrefix<<"The IHumanState interface has been providing inconsistent data and might not be ready";
-                return false;
+            yInfo() << LogPrefix << "IHumanState interface waiting for first data. Waiting...";
+            yarp::os::Time::delay(5);
         }
 
         iHumanState = tmpIHumanState;

devices/RobotPositionController/RobotPositionController.cpp

@@ -469,10 +469,10 @@ bool RobotPositionController::attach(yarp::dev::PolyDriver* poly)
     // CHECK THE INTERFACE
     // ===================
 
-    if (pImpl->iHumanState->getNumberOfJoints() == 0
+    while (pImpl->iHumanState->getNumberOfJoints() == 0
         || pImpl->iHumanState->getNumberOfJoints() != pImpl->iHumanState->getJointNames().size()) {
-        yError() << "The IHumanState interface might not be ready";
-        return false;
+        yInfo() << LogPrefix << "IHumanState interface waiting for first data. Waiting...";
+        yarp::os::Time::delay(5);
     }
 
     // Check the joint numbers match

publishers/HumanDynamicsPublisher/HumanDynamicsPublisher.cpp

@@ -250,11 +250,10 @@ bool HumanDynamicsPublisher::attach(yarp::dev::PolyDriver* poly)
     // ===================
 
     // If it is ready, check that is valid
-    if (pImpl->humanDynamics->getNumberOfJoints() != 0
+    while (pImpl->humanDynamics->getNumberOfJoints() != 0
         && (pImpl->humanDynamics->getNumberOfJoints() != pImpl->humanDynamics->getJointNames().size())) {
-        yError() << LogPrefix << "The IHumanDynamics interface is not valid."
-                 << "The number of joints should match the number of joint names.";
-        return false;
+        yInfo() << LogPrefix << "IHumanDynamics interface waiting for first data. Waiting...";
+        yarp::os::Time::delay(5);
     }
 
     // ====

publishers/HumanStatePublisher/HumanStatePublisher.cpp

@@ -451,11 +451,10 @@ bool HumanStatePublisher::attach(yarp::dev::PolyDriver* poly)
     // ===================
 
     // If it is ready, check that is valid
-    if (pImpl->humanState->getNumberOfJoints() != 0
+    while (pImpl->humanState->getNumberOfJoints() != 0
         && (pImpl->humanState->getNumberOfJoints() != pImpl->humanState->getJointNames().size())) {
-        yError() << LogPrefix << "The IHumanState interface is not valid."
-                 << "The number of joints should match the number of joint names.";
-        return false;
+        yInfo() << LogPrefix << "IHumanState interface waiting for first data. Waiting...";
+        yarp::os::Time::delay(5);
     }
 
     // ====

remappers/HumanDynamicsRemapper/HumanDynamicsRemapper.cpp

@@ -12,7 +12,7 @@
 #include <mutex>
 
 const std::string RemapperName = "HumanDynamicsRemapper";
-const std::string logPrefix = RemapperName + " :";
+const std::string LogPrefix = RemapperName + " :";
 
 using namespace hde::devices;
 
@@ -54,7 +54,7 @@ bool HumanDynamicsRemapper::open(yarp::os::Searchable& config)
     // Data ports
     // TODO: where to check this port?
     if (!(config.check("humanDynamicsDataPort") && config.find("humanDynamicsDataPort").isString())) {
-        yError() << logPrefix << "humanDynamicsData option does not exist or it is not a list";
+        yError() << LogPrefix << "humanDynamicsData option does not exist or it is not a list";
         return false;
     }
 
@@ -68,38 +68,38 @@ bool HumanDynamicsRemapper::open(yarp::os::Searchable& config)
     // TODO: is this required in every DeviceDriver?
     pImpl->network = yarp::os::Network();
     if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) {
-        yError() << logPrefix << "YARP server wasn't found active.";
+        yError() << LogPrefix << "YARP server wasn't found active.";
         return false;
     }
 
     // ==========================
     // CONFIGURE INPUT DATA PORTS
     // ==========================
-    yDebug() << logPrefix << "Configuring input data ports";
+    yDebug() << LogPrefix << "Configuring input data ports";
 
     pImpl->inputPort.useCallback(*this);
     if (!pImpl->inputPort.open("...")) {
-        yError() << logPrefix << "Failed to open port" << humanDynamicsDataPortName;
+        yError() << LogPrefix << "Failed to open port" << humanDynamicsDataPortName;
         return false;
     }
 
     // ================
     // OPEN INPUT PORTS
     // ================
-    yDebug() << logPrefix << "Opening input ports";
+    yDebug() << LogPrefix << "Opening input ports";
 
 
     if (!yarp::os::Network::connect(humanDynamicsDataPortName,
                                     pImpl->inputPort.getName())) {
-        yError() << logPrefix << "Failed to connect " << humanDynamicsDataPortName
+        yError() << LogPrefix << "Failed to connect " << humanDynamicsDataPortName
                  << " with " << pImpl->inputPort.getName();
         return false;
     }
 
     // We use callbacks on the input ports, the loop is a no-op
     start();
 
-    yDebug() << logPrefix << "Opened correctly";
+    yDebug() << LogPrefix << "Opened correctly";
     return true;
 }