From 7ae0be3fd337b7042c4d82e35640080c90665334 Mon Sep 17 00:00:00 2001 From: ifeel Date: Thu, 28 Sep 2023 11:46:09 +0200 Subject: [PATCH 1/2] Unify LogPrefix capitalization --- devices/HumanLogger/HumanLogger.cpp | 42 +++++++++---------- .../HumanDynamicsRemapper.cpp | 16 +++---- .../WearableTargetsRemapper.cpp | 16 +++---- 3 files changed, 37 insertions(+), 37 deletions(-) diff --git a/devices/HumanLogger/HumanLogger.cpp b/devices/HumanLogger/HumanLogger.cpp index 7c7a9f350..ab319e8b8 100644 --- a/devices/HumanLogger/HumanLogger.cpp +++ b/devices/HumanLogger/HumanLogger.cpp @@ -17,7 +17,7 @@ #include const std::string LoggerName = "HumanLogger"; -const std::string logPrefix = LoggerName + " :"; +const std::string LogPrefix = LoggerName + " :"; constexpr double DefaultPeriod = 0.01; namespace hde { @@ -98,7 +98,7 @@ void HumanLogger::run() if (pImpl->settings.logHumanState) { if(!pImpl->iHumanState) { - yError() << logPrefix << "The IHumanState pointer is null in the driver loop."; + yError() << LogPrefix << "The IHumanState pointer is null in the driver loop."; askToStop(); return; } @@ -131,7 +131,7 @@ void HumanLogger::run() if (pImpl->settings.logHumanDynamics) { if(!pImpl->iHumanDynamics) { - yError() << logPrefix << "The IHumanDynamics pointer is null in the driver loop."; + yError() << LogPrefix << "The IHumanDynamics pointer is null in the driver loop."; askToStop(); return; } @@ -153,7 +153,7 @@ void HumanLogger::run() bool HumanLogger::open(yarp::os::Searchable& config) { if (!config.check("period")) { - yInfo() << logPrefix << "Using default period: " << DefaultPeriod << "s"; + yInfo() << LogPrefix << "Using default period: " << DefaultPeriod << "s"; } const double period = config.check("period", yarp::os::Value(DefaultPeriod)).asFloat64(); @@ -162,7 +162,7 @@ bool HumanLogger::open(yarp::os::Searchable& config) // Load settings in the class bool ok = pImpl->loadSettingsFromConfig(config); if (!ok) { - yError() << logPrefix << "Problem in loading settings from config."; + yError() << LogPrefix << "Problem in loading settings from config."; return false; } @@ -190,14 +190,14 @@ bool HumanLogger::impl::loadSettingsFromConfig(yarp::os::Searchable& config) && config.find("LoggerType").isString() && setLoggerType(config.find("LoggerType").asString()) ) ) { - yError() << logPrefix << "LoggerType not found or not valid"; + yError() << LogPrefix << "LoggerType not found or not valid"; return false; } // Display the current logger level switch (this->loggerType) { case LoggerType::MATLAB: { - yInfo() << logPrefix << "LoggerType set to MATLAB"; + yInfo() << LogPrefix << "LoggerType set to MATLAB"; break; } default: @@ -217,7 +217,7 @@ bool HumanLogger::impl::loadSettingsFromConfig(yarp::os::Searchable& config) bufferConfig.filename = config.find(experimentName.c_str()).asString(); } else { - yError() << logPrefix << " missing parameter: " << experimentName; + yError() << LogPrefix << " missing parameter: " << experimentName; return false; } @@ -231,7 +231,7 @@ bool HumanLogger::impl::loadSettingsFromConfig(yarp::os::Searchable& config) bufferConfig.n_samples = config.find(n_samples.c_str()).asInt32(); } else { - yError() << logPrefix << " missing parameter: " << n_samples; + yError() << LogPrefix << " missing parameter: " << n_samples; return false; } @@ -246,7 +246,7 @@ bool HumanLogger::impl::loadSettingsFromConfig(yarp::os::Searchable& config) bufferConfig.save_period = config.find(save_period.c_str()).asFloat64(); } else { - yError() << logPrefix << " missing parameter: " << save_period; + yError() << LogPrefix << " missing parameter: " << save_period; return false; } @@ -263,7 +263,7 @@ bool HumanLogger::impl::loadSettingsFromConfig(yarp::os::Searchable& config) if (!(bufferConfig.auto_save || bufferConfig.save_periodically)) { yError() - << logPrefix + << LogPrefix << " both auto_save and save_periodically are set to false, nothing will be saved."; return false; } @@ -323,7 +323,7 @@ bool HumanLogger::impl::configureBufferManager() ok = ok && bufferManager.configure(bufferConfig); if (ok) { - yDebug() << logPrefix << " buffer manager configured successfully."; + yDebug() << LogPrefix << " buffer manager configured successfully."; } return ok; @@ -338,7 +338,7 @@ void HumanLogger::threadRelease() {} bool HumanLogger::attachAll(const yarp::dev::PolyDriverList& driverList) { if (driverList.size() > 2) { - yError() << logPrefix << "This wrapper accepts maximum two attached PolyDriver."; + yError() << LogPrefix << "This wrapper accepts maximum two attached PolyDriver."; return false; } @@ -346,7 +346,7 @@ bool HumanLogger::attachAll(const yarp::dev::PolyDriverList& driverList) yarp::dev::PolyDriver* poly = driverList[i]->poly; if (!poly) { - yError() << logPrefix << "Passed PolyDriver is nullptr."; + yError() << LogPrefix << "Passed PolyDriver is nullptr."; return false; } @@ -383,41 +383,41 @@ bool HumanLogger::attachAll(const yarp::dev::PolyDriverList& driverList) if(!tmpIHumanState && !tmpIHumanDynamics) { - yError()<settings.logHumanState && !pImpl->iHumanState) { - yError()<settings.logHumanDynamics && !pImpl->iHumanDynamics) { - yError()<configureBufferManager()) { - yError() << logPrefix << "Failed to configure buffer manager for the logger."; + yError() << LogPrefix << "Failed to configure buffer manager for the logger."; return false; } // Start the periodic thread if(!start()) { - yError() << logPrefix << "Failed to start the loop."; + yError() << LogPrefix << "Failed to start the loop."; return false; } - yInfo() << logPrefix << "Successfully attached all the required devices"; + yInfo() << LogPrefix << "Successfully attached all the required devices"; return true; } diff --git a/remappers/HumanDynamicsRemapper/HumanDynamicsRemapper.cpp b/remappers/HumanDynamicsRemapper/HumanDynamicsRemapper.cpp index 4ecb1f870..7d188ef02 100644 --- a/remappers/HumanDynamicsRemapper/HumanDynamicsRemapper.cpp +++ b/remappers/HumanDynamicsRemapper/HumanDynamicsRemapper.cpp @@ -12,7 +12,7 @@ #include const std::string RemapperName = "HumanDynamicsRemapper"; -const std::string logPrefix = RemapperName + " :"; +const std::string LogPrefix = RemapperName + " :"; using namespace hde::devices; @@ -54,7 +54,7 @@ bool HumanDynamicsRemapper::open(yarp::os::Searchable& config) // Data ports // TODO: where to check this port? if (!(config.check("humanDynamicsDataPort") && config.find("humanDynamicsDataPort").isString())) { - yError() << logPrefix << "humanDynamicsData option does not exist or it is not a list"; + yError() << LogPrefix << "humanDynamicsData option does not exist or it is not a list"; return false; } @@ -68,30 +68,30 @@ bool HumanDynamicsRemapper::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" << humanDynamicsDataPortName; + yError() << LogPrefix << "Failed to open port" << humanDynamicsDataPortName; return false; } // ================ // OPEN INPUT PORTS // ================ - yDebug() << logPrefix << "Opening input ports"; + yDebug() << LogPrefix << "Opening input ports"; if (!yarp::os::Network::connect(humanDynamicsDataPortName, pImpl->inputPort.getName())) { - yError() << logPrefix << "Failed to connect " << humanDynamicsDataPortName + yError() << LogPrefix << "Failed to connect " << humanDynamicsDataPortName << " with " << pImpl->inputPort.getName(); return false; } @@ -99,7 +99,7 @@ bool HumanDynamicsRemapper::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/remappers/WearableTargetsRemapper/WearableTargetsRemapper.cpp b/remappers/WearableTargetsRemapper/WearableTargetsRemapper.cpp index 9db7f3f42..400043430 100644 --- a/remappers/WearableTargetsRemapper/WearableTargetsRemapper.cpp +++ b/remappers/WearableTargetsRemapper/WearableTargetsRemapper.cpp @@ -12,7 +12,7 @@ #include const std::string RemapperName = "WearableTargetsRemapper"; -const std::string logPrefix = RemapperName + " :"; +const std::string LogPrefix = RemapperName + " :"; using namespace hde::devices; @@ -88,7 +88,7 @@ bool WearableTargetsRemapper::open(yarp::os::Searchable& config) // Data ports if (!(config.check("wearableTargetsDataPort") && config.find("wearableTargetsDataPort").isString())) { - yError() << logPrefix << "wearableTargetsDataPort option does not exist or it is not a list"; + yError() << LogPrefix << "wearableTargetsDataPort option does not exist or it is not a list"; return false; } @@ -102,30 +102,30 @@ bool WearableTargetsRemapper::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" << wearableTargetsDataPortName; + yError() << LogPrefix << "Failed to open port" << wearableTargetsDataPortName; return false; } // ================ // OPEN INPUT PORTS // ================ - yDebug() << logPrefix << "Opening input ports"; + yDebug() << LogPrefix << "Opening input ports"; if (!yarp::os::Network::connect(wearableTargetsDataPortName, pImpl->inputPort.getName())) { - yError() << logPrefix << "Failed to connect " << wearableTargetsDataPortName + yError() << LogPrefix << "Failed to connect " << wearableTargetsDataPortName << " with " << pImpl->inputPort.getName(); return false; } @@ -133,7 +133,7 @@ bool WearableTargetsRemapper::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; } From cf36f5163345801ded04678dd672b494fe5fbf7d Mon Sep 17 00:00:00 2001 From: ifeel Date: Thu, 28 Sep 2023 11:46:31 +0200 Subject: [PATCH 2/2] Add timeout for interface connection --- CHANGELOG.md | 1 + devices/HumanControlBoard/HumanControlBoard.cpp | 13 ++++++------- .../HumanDynamicsEstimator.cpp | 13 ++++++------- devices/HumanLogger/HumanLogger.cpp | 12 ++++++------ .../HumanWrenchProvider/HumanWrenchProvider.cpp | 6 +++--- .../RobotPositionController.cpp | 6 +++--- .../HumanDynamicsPublisher.cpp | 7 +++---- .../HumanStatePublisher/HumanStatePublisher.cpp | 7 +++---- .../HumanStateRemapper/HumanStateRemapper.cpp | 16 ++++++++-------- .../HumanDynamicsWrapper.cpp | 6 +++--- wrappers/HumanStateWrapper/HumanStateWrapper.cpp | 6 +++--- 11 files changed, 45 insertions(+), 48 deletions(-) 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";