From a67d28734036156f0729c537771a20338cc38803 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Thu, 11 Jul 2024 12:42:52 +0200 Subject: [PATCH] Improve sensor check at startup logic --- devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index b1c851a..c91ccf3 100644 --- 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; } }