diff --git a/CHANGELOG.md b/CHANGELOG.md index 7a5f7d96c..a74bd1b8c 100644 --- 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 index 34ce54153..e32b92aff 100644 --- 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 index 697763abb..76f29f7a9 100644 --- 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 index ab319e8b8..57eb835f5 100644 --- 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"<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"<iHumanDynamics = tmpIHumanDynamics; diff --git a/devices/HumanWrenchProvider/HumanWrenchProvider.cpp b/devices/HumanWrenchProvider/HumanWrenchProvider.cpp index f7f7e26cc..60a767dc5 100644 --- 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 index 129307eaa..0986222af 100755 --- 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 index 21dfb4fc7..d798e0031 100644 --- 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 index 235ce5413..1a42702b2 100644 --- 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 index 89afec7bb..9a5cc3c29 100644 --- a/remappers/HumanStateRemapper/HumanStateRemapper.cpp +++ b/remappers/HumanStateRemapper/HumanStateRemapper.cpp @@ -12,7 +12,7 @@ #include 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,30 +76,30 @@ 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; } @@ -107,7 +107,7 @@ bool HumanStateRemapper::open(yarp::os::Searchable& config) // 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 index adf4b204c..ea022dc4d 100644 --- 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 index fc34474c6..0c7eb881a 100644 --- 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";