Improve sensor check at startup logic

robotology · Jul 11, 2024 · a67d287 · a67d287
1 parent 871e59d
commit a67d287
Showing 1 changed file with 7 additions and 3 deletions.
diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp
@@ -1850,13 +1850,17 @@ bool WholeBodyDynamicsDevice::attachAllFTs(const PolyDriverList& p)
         while( (timeSpentTryngToReadSensors < wholeBodyDynamics_sensorTimeoutInSeconds) && !readSuccessfull )
         {
             readSuccessfull = readFTSensors(verbose);
-            timeSpentTryngToReadSensors = (yarp::os::Time::now() - tic);
-            yarp::os::Time::delay(0.001);
+            // Only update the timeSpentTryngToReadSensors and wait if the read was not successful
+            if (!readSuccessfull)
+            {
+                timeSpentTryngToReadSensors = (yarp::os::Time::now() - tic);
+                yarp::os::Time::delay(0.001);
+            }
         }
 
         if( !readSuccessfull )
         {
-            yError() << "WholeBodyDynamicsDevice was unable to correctly read from the FT sensors";
+            yError() << "WholeBodyDynamicsDevice was unable to correctly read from the FT sensors after " << timeSpentTryngToReadSensors << " seconds.";
             return false;
         }
     }