Skip to content

Commit

Permalink
Add timeout for interface connection
Browse files Browse the repository at this point in the history
  • Loading branch information
ifeel committed Sep 28, 2023
1 parent 7ae0be3 commit 38e84f6
Show file tree
Hide file tree
Showing 10 changed files with 44 additions and 48 deletions.
13 changes: 6 additions & 7 deletions devices/HumanControlBoard/HumanControlBoard.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
Expand Down
13 changes: 6 additions & 7 deletions devices/HumanDynamicsEstimator/HumanDynamicsEstimator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand All @@ -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";
Expand Down
12 changes: 6 additions & 6 deletions devices/HumanLogger/HumanLogger.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"<<deviceName<<"might not be ready";
return false;
yInfo() << LogPrefix << "IHumanState interface waiting for first data. Waiting...";
yarp::os::Time::delay(5);
}

pImpl->iHumanState = tmpIHumanState;
Expand All @@ -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"<<deviceName<<"might not be ready";
return false;
yInfo() << LogPrefix << "IHumanDynamics interface waiting for first data. Waiting...";
yarp::os::Time::delay(5);
}

pImpl->iHumanDynamics = tmpIHumanDynamics;
Expand Down
6 changes: 3 additions & 3 deletions devices/HumanWrenchProvider/HumanWrenchProvider.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
6 changes: 3 additions & 3 deletions devices/RobotPositionController/RobotPositionController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
7 changes: 3 additions & 4 deletions publishers/HumanDynamicsPublisher/HumanDynamicsPublisher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

// ====
Expand Down
7 changes: 3 additions & 4 deletions publishers/HumanStatePublisher/HumanStatePublisher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

// ====
Expand Down
16 changes: 8 additions & 8 deletions remappers/HumanStateRemapper/HumanStateRemapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
#include <mutex>

const std::string RemapperName = "HumanStateRemapper";
const std::string logPrefix = RemapperName + " :";
const std::string LogPrefix = RemapperName + " :";

using namespace hde::devices;

Expand Down Expand Up @@ -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;
}

Expand All @@ -76,38 +76,38 @@ 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;
}

// We use callbacks on the input ports, the loop is a no-op
start();

yDebug() << logPrefix << "Opened correctly";
yDebug() << LogPrefix << "Opened correctly";
return true;
}

Expand Down
6 changes: 3 additions & 3 deletions wrappers/HumanDynamicsWrapper/HumanDynamicsWrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down
6 changes: 3 additions & 3 deletions wrappers/HumanStateWrapper/HumanStateWrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down

0 comments on commit 38e84f6

Please sign in to comment.