Use timeout for checking iHumanState interface

robotology · Sep 2, 2022 · 0b8881b · 0b8881b
1 parent c8c681a
commit 0b8881b
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 7 deletions.
diff --git a/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp b/devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp
@@ -43,6 +43,9 @@ const std::string DeviceName = "HumanDynamicsEstimator";
 const std::string LogPrefix = DeviceName + " :";
 constexpr double DefaultPeriod = 0.01;
 
+// Timeout for checking that the iHumanState interface is providing consistent data
+constexpr static double INTERFACE_CHECK_TIMEOUT_S = 10;
+
 using namespace hde::devices;
 
 static bool parseYarpValueToStdVector(const yarp::os::Value& option, std::vector<double>& output)
@@ -1235,12 +1238,17 @@ bool HumanDynamicsEstimator::attach(yarp::dev::PolyDriver* poly)
             return false;
         }
 
-        //TODO Check the interface
+        // Check the iHumanState interface
+        double interfaceCheckStart = yarp::os::Time::now();
         int count = 1000000;
         while (pImpl->iHumanState->getNumberOfJoints() == 0
                 || pImpl->iHumanState->getNumberOfJoints() != pImpl->iHumanState->getJointNames().size()) {
-            yError() << LogPrefix<<"The IHumanState interface might not be ready";
-            if(count--==0) return false;
+
+            if(yarp::os::Time::now()-interfaceCheckStart>INTERFACE_CHECK_TIMEOUT_S)
+            {
+                yError() << LogPrefix<<"The iHumanState interface has been providing inconsistent data for"<<INTERFACE_CHECK_TIMEOUT_S<<"seconds!";
+                return false;
+            }
         }
 
         yInfo() << LogPrefix << deviceName << "attach() successful";

diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp
@@ -34,6 +34,10 @@
 
 const std::string DeviceName = "HumanWrenchProvider";
 const std::string LogPrefix = DeviceName + " :";
+
+// Timeout in seconds for checking that the iHumanState interface is providing consistent data
+constexpr double INTERFACE_CHECK_TIMEOUT_S = 10;
+
 constexpr double DefaultPeriod = 0.01;
 
 using namespace hde::devices;
@@ -1274,13 +1278,17 @@ bool HumanWrenchProvider::attach(yarp::dev::PolyDriver* poly)
             return false;
         }
 
-        //TODO Check the interface
+        // Check the iHumanState interface
+        double interfaceCheckStart = yarp::os::Time::now();
         int count = 1000000;
         while (pImpl->iHumanState->getNumberOfJoints() == 0
                 || pImpl->iHumanState->getNumberOfJoints() != pImpl->iHumanState->getJointNames().size()) {
-            yError() << LogPrefix<<"The IHumanState interface might not be ready";
-            count--;
-            if(count==0) return false;
+
+            if(yarp::os::Time::now()-interfaceCheckStart>INTERFACE_CHECK_TIMEOUT_S)
+            {
+                yError() << LogPrefix<<"The iHumanState interface has been providing inconsistent data for"<<INTERFACE_CHECK_TIMEOUT_S<<"seconds!";
+                return false;
+            }
         }
 
         yInfo() << LogPrefix << deviceName << "attach() successful";