Add timeout for interface connection

robotology · Sep 28, 2023 · cf36f51 · cf36f51
1 parent 7ae0be3
commit cf36f51
Show file tree

Hide file tree

Showing 11 changed files with 45 additions and 48 deletions.
diff --git a/CHANGELOG.md b/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)

diff --git a/devices/HumanControlBoard/HumanControlBoard.cpp b/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;

diff --git a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp b/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";

diff --git a/devices/HumanLogger/HumanLogger.cpp b/devices/HumanLogger/HumanLogger.cpp
@@ -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,10 +372,10 @@ 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;

diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/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;

diff --git a/devices/RobotPositionController/RobotPositionController.cpp b/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

diff --git a/publishers/HumanDynamicsPublisher/HumanDynamicsPublisher.cpp b/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);
     }
 
     // ====

diff --git a/publishers/HumanStatePublisher/HumanStatePublisher.cpp b/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);
     }
 
     // ====

diff --git a/remappers/HumanStateRemapper/HumanStateRemapper.cpp b/remappers/HumanStateRemapper/HumanStateRemapper.cpp
@@ -12,7 +12,7 @@
 #include <mutex>
 
 const std::string RemapperName = "HumanStateRemapper";
-const std::string logPrefix = RemapperName + " :";
+const std::string LogPrefix = RemapperName + " :";
 
 using namespace hde::devices;
 
@@ -62,7 +62,7 @@ bool HumanStateRemapper::open(yarp::os::Searchable& config)
 
     // Data ports
     if (!(config.check("humanStateDataPort") && config.find("humanStateDataPort").isString())) {
-        yError() << logPrefix << "humanStateData option does not exist or it is not a list";
+        yError() << LogPrefix << "humanStateData option does not exist or it is not a list";
         return false;
     }
 
@@ -76,38 +76,38 @@ bool HumanStateRemapper::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" << humanStateDataPortName;
+        yError() << LogPrefix << "Failed to open port" << humanStateDataPortName;
         return false;
     }
 
     // ================
     // OPEN INPUT PORTS
     // ================
-    yDebug() << logPrefix << "Opening input ports";
+    yDebug() << LogPrefix << "Opening input ports";
 
 
     if (!yarp::os::Network::connect(humanStateDataPortName,
                                     pImpl->inputPort.getName())) {
-        yError() << logPrefix << "Failed to connect " << humanStateDataPortName
+        yError() << LogPrefix << "Failed to connect " << humanStateDataPortName
                  << " 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;
 }
 

diff --git a/wrappers/HumanDynamicsWrapper/HumanDynamicsWrapper.cpp b/wrappers/HumanDynamicsWrapper/HumanDynamicsWrapper.cpp
@@ -119,11 +119,11 @@ bool HumanDynamicsWrapper::attach(yarp::dev::PolyDriver* poly)
     // CHECK THE INTERFACE
     // ===================
 
-    if (pImpl->humanDynamics->getNumberOfJoints() == 0
+    while (pImpl->humanDynamics->getNumberOfJoints() == 0
         || pImpl->humanDynamics->getNumberOfJoints()
                != pImpl->humanDynamics->getJointNames().size()) {
-        yError() << "The IHumanDynamics interface might not be ready";
-        return false;
+        yInfo() << LogPrefix << "IHumanDynamics interface waiting for first data. Waiting...";
+        yarp::os::Time::delay(5);
     }
 
     yDebug() << LogPrefix << "Read" << pImpl->humanDynamics->getNumberOfJoints() << "joints";

diff --git a/wrappers/HumanStateWrapper/HumanStateWrapper.cpp b/wrappers/HumanStateWrapper/HumanStateWrapper.cpp
@@ -178,10 +178,10 @@ bool HumanStateWrapper::attach(yarp::dev::PolyDriver* poly)
         yInfo() << "Joint name (" << i << "): " << pImpl->iHumanState->getJointNames()[i];
     }
 
-    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);
     }
 
     yDebug() << LogPrefix << "Read" << pImpl->iHumanState->getNumberOfJoints() << "joints";