Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Move the timeout check from RemoteControlBoard to the stateExtendedReader #1847

Merged
merged 3 commits into from
Apr 11, 2019
Merged

Move the timeout check from RemoteControlBoard to the stateExtendedReader #1847

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9a57eb3
YARP_dev: move the logic of the timeout check from `RemoteControlBoar…
Nicogene Sep 4, 2018
76be59b
Update release notes.
Nicogene Sep 4, 2018
ae54352
RemoteControlBoard: cleanup and add timeout parameter from command line
Nicogene Apr 11, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions doc/release/v3_1_1.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ Bug Fixes
(#1875).
* Fixed header inclusion in `ImplementControlLimits2.h`.
* Fixed interface pointer checks in ControlBoardWrapper.
* Added timeout check in all data streamed by `*\stateExt:o` ports (#1833).

#### `YARP_companion`

Expand Down Expand Up @@ -86,6 +87,10 @@ Bug Fixes

### Devices

#### `RemoteControlBoard`

* Added `timeout` parameter.

#### `realsense2`

* Fixed `set/getDepthAccuracy` methods (#1877).
Expand Down
29 changes: 4 additions & 25 deletions src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,6 @@ namespace yarp{
#ifndef DOXYGEN_SHOULD_SKIP_THIS

const double DIAGNOSTIC_THREAD_PERIOD=1.000;
const double TIMEOUT=0.5;

inline bool getTimeStamp(Bottle &bot, Stamp &st)
{
Expand Down Expand Up @@ -986,6 +985,10 @@ class yarp::dev::RemoteControlBoard :
remote = config.find("remote").asString();
local = config.find("local").asString();

if (config.check("timeout"))
{
extendedIntputStatePort.setTimeout(config.find("timeout").asFloat64());
}
// check the Qos perefernces if available (local and remote)
yarp::os::QosStyle localQos;
if (config.check("local_qos")) {
Expand Down Expand Up @@ -1478,10 +1481,6 @@ class yarp::dev::RemoteControlBoard :
extendedPortMutex.lock();
bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_ENCODER, v, lastStamp, localArrivalTime);
extendedPortMutex.unlock();

if (ret && ( (Time::now()-localArrivalTime) > TIMEOUT) )
ret = false;

return ret;
}

Expand All @@ -1499,10 +1498,6 @@ class yarp::dev::RemoteControlBoard :
bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_ENCODER, v, lastStamp, localArrivalTime);
*t=lastStamp.getTime();
extendedPortMutex.unlock();

if (ret && ( (Time::now()-localArrivalTime) > TIMEOUT) )
ret = false;

return ret;
}

Expand Down Expand Up @@ -1538,10 +1533,6 @@ class yarp::dev::RemoteControlBoard :
bool ret = extendedIntputStatePort.getLastVector(VOCAB_ENCODERS, encs, lastStamp, localArrivalTime);
std::fill_n(ts, nj, lastStamp.getTime());
extendedPortMutex.unlock();

if ( (Time::now()-localArrivalTime) > TIMEOUT)
ret=false;

return ret;
}
/**
Expand Down Expand Up @@ -1750,10 +1741,6 @@ class yarp::dev::RemoteControlBoard :
extendedPortMutex.lock();
bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_MOTOR_ENCODER, v, lastStamp, localArrivalTime);
extendedPortMutex.unlock();

if(ret && ((Time::now()-localArrivalTime) > TIMEOUT) )
ret=false;

return ret;
}

Expand All @@ -1771,10 +1758,6 @@ class yarp::dev::RemoteControlBoard :
bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_MOTOR_ENCODER, v, lastStamp, localArrivalTime);
*t=lastStamp.getTime();
extendedPortMutex.unlock();

if(ret && ((Time::now()-localArrivalTime) > TIMEOUT) )
ret=false;

return ret;
}

Expand Down Expand Up @@ -1812,10 +1795,6 @@ class yarp::dev::RemoteControlBoard :
bool ret = extendedIntputStatePort.getLastVector(VOCAB_MOTOR_ENCODERS, encs, lastStamp, localArrivalTime);
std::fill_n(ts, nj, lastStamp.getTime());
extendedPortMutex.unlock();

if(ret && ((Time::now()-localArrivalTime) > TIMEOUT) )
ret=false;

return ret;
}
/**
Expand Down
24 changes: 21 additions & 3 deletions src/libYARP_dev/src/devices/RemoteControlBoard/stateExtendedReader.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,10 +43,15 @@ void StateExtendedInputPort::resetStat()
mutex.unlock();
}

StateExtendedInputPort::StateExtendedInputPort()
StateExtendedInputPort::StateExtendedInputPort() : deltaT{0},
deltaTMax{0},
deltaTMin{1e22},
now{Time::now()},
prev{now},
timeout{0.5},
valid{false},
count{0}
{
valid=false;
resetStat();
}

void StateExtendedInputPort::init(int numberOfJoints)
Expand Down Expand Up @@ -91,6 +96,10 @@ void StateExtendedInputPort::onRead(jointData &v)
mutex.unlock();
}

void StateExtendedInputPort::setTimeout(const double& timeout) {
this->timeout = timeout;
}

bool StateExtendedInputPort::getLastSingle(int j, int field, double *data, Stamp &stamp, double &localArrivalTime)
{
mutex.lock();
Expand Down Expand Up @@ -151,6 +160,8 @@ bool StateExtendedInputPort::getLastSingle(int j, int field, double *data, Stamp

localArrivalTime=now;
stamp = lastStamp;
if (ret && ( (Time::now()-localArrivalTime) > timeout) )
ret = false;
}
mutex.unlock();

Expand Down Expand Up @@ -181,6 +192,9 @@ bool StateExtendedInputPort::getLastSingle(int j, int field, int *data, Stamp &s
}
localArrivalTime=now;
stamp = lastStamp;
if (ret && ( (Time::now()-localArrivalTime) > timeout) )
ret = false;

}
mutex.unlock();
return ret;
Expand Down Expand Up @@ -246,6 +260,8 @@ bool StateExtendedInputPort::getLastVector(int field, double* data, Stamp& stamp

localArrivalTime=now;
stamp = lastStamp;
if (ret && ( (Time::now()-localArrivalTime) > timeout) )
ret = false;
}
mutex.unlock();

Expand Down Expand Up @@ -276,6 +292,8 @@ bool StateExtendedInputPort::getLastVector(int field, int* data, Stamp& stamp, d
}
localArrivalTime=now;
stamp = lastStamp;
if (ret && ( (Time::now()-localArrivalTime) > timeout) )
ret = false;
}
mutex.unlock();
return ret;
Expand Down
10 changes: 8 additions & 2 deletions src/libYARP_dev/src/devices/RemoteControlBoard/stateExtendedReader.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,6 @@

// encoders should arrive at least every 0.5s to be considered valide
// getEncoders will return false otherwise.
const double TIMEOUT_EXT=0.5;

using namespace yarp::os;
using namespace yarp::dev;
Expand All @@ -49,8 +48,9 @@ class StateExtendedInputPort:public yarp::os::BufferedPort<jointData>
double deltaT;
double deltaTMax;
double deltaTMin;
double prev;
double now;
double prev;
double timeout;

bool valid;
int count;
Expand All @@ -64,6 +64,12 @@ class StateExtendedInputPort:public yarp::os::BufferedPort<jointData>
using yarp::os::BufferedPort<jointData>::onRead;
void onRead(jointData &v) override;

/**
* @brief setTimeout, set the timeout for retrieving data
* @param timeout in seconds
*/
void setTimeout(const double& timeout);

// use vocab to identify the data to be read
// get a value for a single joint
bool getLastSingle(int j, int field, double *data, Stamp &stamp, double &localArrivalTime);
Expand Down