Skip to content

Commit

Permalink
Merge pull request #3095 from Nicogene/reduceIntRemapper
Browse files Browse the repository at this point in the history 
controlBoardRemapper: align the mandatory interfaces to controlBoard_nws_yarp
  • Loading branch information
@randaz81
randaz81 authored Apr 8, 2024
2 parents 2aa9512 + a6d5fbc commit e4762b0
Show file tree
Hide file tree
Showing 4 changed files with 422 additions and 35 deletions.
6 changes: 5 additions & 1 deletion doc/release/master.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,11 @@ New Features

### Devices

#### controlboardremapper

* Aligned to `controlBoard_nws_yarp` in terms of required interfaces. See https://github.com/robotology/yarp/pull/3095.

#### deviceBundler

* Added new device `deviceBundler` which can be useful to open two devices and attach them while using a single yarpdev command line.
See https://github.com/robotology/yarp/discussions/3078
See https://github.com/robotology/yarp/discussions/3078
155 changes: 129 additions & 26 deletions src/devices/controlBoardRemapper/ControlBoardRemapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1014,9 +1014,15 @@ bool ControlBoardRemapper::positionMove(const double *refs)
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->pos->positionMove(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
bool ok = true;
if (p->pos) {
ok = p->pos->positionMove(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
}
else {
ok = false;
}
ret = ret && ok;
}

Expand All @@ -1034,9 +1040,15 @@ bool ControlBoardRemapper::positionMove(const int n_joints, const int *joints, c
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->pos->positionMove(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
bool ok = true;
if (p->pos) {
ok = p->pos->positionMove(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
}
else {
ok = false;
}
ret = ret && ok;
}

Expand Down Expand Up @@ -1158,10 +1170,15 @@ bool ControlBoardRemapper::relativeMove(const double *deltas)
for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->pos->relativeMove(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
bool ok = true;
if (p->pos) {
ok = p->pos->relativeMove(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
}
else {
ok = false;
}
ret = ret && ok;
}

Expand All @@ -1178,10 +1195,15 @@ bool ControlBoardRemapper::relativeMove(const int n_joints, const int *joints, c
for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->pos->relativeMove(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
bool ok = true;
if (p->pos) {
ok = p->pos->relativeMove(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
}
else {
ok = false;
}
ret = ret && ok;
}

Expand Down Expand Up @@ -1306,9 +1328,15 @@ bool ControlBoardRemapper::setRefSpeeds(const double *spds)
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->pos->setRefSpeeds(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
bool ok = true;
if (p->pos) {
ok = p->pos->setRefSpeeds(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
}
else {
ok = false;
}
ret = ret && ok;
}

Expand All @@ -1326,9 +1354,15 @@ bool ControlBoardRemapper::setRefSpeeds(const int n_joints, const int *joints, c
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->pos->setRefSpeeds(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
bool ok = true;
if (p->pos) {
ok = p->pos->setRefSpeeds(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
}
else {
ok = false;
}
ret = ret && ok;
}

Expand Down Expand Up @@ -1366,9 +1400,15 @@ bool ControlBoardRemapper::setRefAccelerations(const double *accs)
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->pos->setRefAccelerations(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
bool ok = true;
if (p->pos) {
ok = p->pos->setRefAccelerations(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
}
else {
ok = false;
}
ret = ret && ok;
}

Expand All @@ -1386,9 +1426,15 @@ bool ControlBoardRemapper::setRefAccelerations(const int n_joints, const int *jo
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->pos->setRefAccelerations(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
bool ok = true;
if (p->pos) {
ok = p->pos->setRefAccelerations(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
}
else {
ok = false;
}
ret = ret && ok;
}

Expand Down Expand Up @@ -1425,7 +1471,6 @@ bool ControlBoardRemapper::getRefSpeeds(double *spds)
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = true;

if( p->pos )
{
ok = p->pos->getRefSpeeds(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
Expand Down Expand Up @@ -1542,7 +1587,6 @@ bool ControlBoardRemapper::getRefAccelerations(const int n_joints, const int *jo
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = true;

if( p->pos )
{
ok = p->pos->getRefAccelerations(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
Expand Down Expand Up @@ -1614,9 +1658,15 @@ bool ControlBoardRemapper::stop(const int n_joints, const int *joints)
for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->pos->stop(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
bool ok = true;
if (p->pos) {
ok = p->pos->stop(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data());
}
else
{
ok = false;
}
ret = ret && ok;
}

Expand Down Expand Up @@ -1677,9 +1727,15 @@ bool ControlBoardRemapper::velocityMove(const double *v)
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->vel->velocityMove(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
bool ok = true;
if (p->vel) {
ok = p->vel->velocityMove(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
}
else {
ok = false;
}
ret = ret && ok;
}

Expand Down Expand Up @@ -3575,7 +3631,16 @@ bool ControlBoardRemapper::getControlMode(int j, int *mode)
return false;
}

return p->iMode->getControlMode(off, mode);
bool ok = true;

if (p->iMode) {
ok = p->iMode->getControlMode(off, mode);
}
else {
ok = false;
}

return ok;

}

Expand Down Expand Up @@ -3657,7 +3722,14 @@ bool ControlBoardRemapper::setControlMode(const int j, const int mode)
return false;
}

ret = p->iMode->setControlMode(off, mode);
if (p->iMode)
{
ret = p->iMode->setControlMode(off, mode);
}
else
{
ret = false;
}

return ret;
}
Expand All @@ -3673,9 +3745,15 @@ bool ControlBoardRemapper::setControlModes(const int n_joints, const int *joints
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->iMode->setControlModes(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
bool ok = true;
if (p->iMode) {
ok = p->iMode->setControlModes(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
selectedJointsBuffers.m_bufferForSubControlBoardControlModes[ctrlBrd].data());
}
else {
ok = false;
}
ret = ret && ok;
}

Expand All @@ -3693,9 +3771,15 @@ bool ControlBoardRemapper::setControlModes(int *modes)
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->iMode->setControlModes(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
bool ok = true;
if (p->iMode) {
ok = p->iMode->setControlModes(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
allJointsBuffers.m_bufferForSubControlBoardControlModes[ctrlBrd].data());
}
else {
ok = false;
}
ret = ret && ok;
}

Expand Down Expand Up @@ -3733,9 +3817,15 @@ bool ControlBoardRemapper::setPositions(const int n_joints, const int *joints, c
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->posDir->setPositions(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
bool ok = true;
if (p->posDir) {
ok = p->posDir->setPositions(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
}
else {
ok = false;
}
ret = ret && ok;
}

Expand All @@ -3753,9 +3843,15 @@ bool ControlBoardRemapper::setPositions(const double *refs)
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->posDir->setPositions(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
bool ok = true;
if (p->posDir) {
ok = p->posDir->setPositions(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
allJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
}
else {
ok = false;
}
ret = ret && ok;
}

Expand Down Expand Up @@ -3899,9 +3995,16 @@ bool ControlBoardRemapper::velocityMove(const int n_joints, const int *joints, c
{
RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

bool ok = p->vel->velocityMove(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
bool ok = true;
if (p->vel) {
ok = p->vel->velocityMove(selectedJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
selectedJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
selectedJointsBuffers.m_bufferForSubControlBoard[ctrlBrd].data());
}
else
{
ok = false;
}
ret = ret && ok;
}

Expand Down
30 changes: 22 additions & 8 deletions src/devices/controlBoardRemapper/ControlBoardRemapperHelpers.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,26 +174,22 @@ bool RemappedSubControlBoard::attach(yarp::dev::PolyDriver *d, const std::string
// checking minimum set of interfaces required
if( !(pos) )
{
yCError(CONTROLBOARDREMAPPER, "IPositionControl interface was not found in subdevice. Quitting");
return false;
yCWarning(CONTROLBOARDREMAPPER, "IPositionControl interface was not found in subdevice");
}

if( ! (vel) )
{
yCError(CONTROLBOARDREMAPPER, "IVelocityControl interface was not found in subdevice. Quitting");
return false;
yCWarning(CONTROLBOARDREMAPPER, "IVelocityControl interface was not found in subdevice");
}

if(!iJntEnc)
{
yCError(CONTROLBOARDREMAPPER, "IEncoderTimed interface was not found in subdevice, exiting.");
return false;
yCWarning(CONTROLBOARDREMAPPER, "IEncoderTimed interface was not found in subdevice");
}

if(!iMode)
{
yCError(CONTROLBOARDREMAPPER, "IControlMode interface was not found in subdevice, exiting.");
return false;
yCWarning(CONTROLBOARDREMAPPER, "IControlMode interface was not found in subdevice");
}

int deviceJoints=0;
Expand All @@ -210,6 +206,24 @@ bool RemappedSubControlBoard::attach(yarp::dev::PolyDriver *d, const std::string
return false;
}
}
else if(info!=nullptr)
{
if (!info->getAxes(&deviceJoints))
{
yCError(CONTROLBOARDREMAPPER) << "failed to get axes number for subdevice" << k.c_str();
return false;
}
if(deviceJoints <= 0)
{
yCError(CONTROLBOARDREMAPPER, "attached device has an invalid number of joints (%d)", deviceJoints);
return false;
}
}
else
{
yCError(CONTROLBOARDREMAPPER, "attached device has no IPositionControl nor IAxisInfo interface");
return false;
}

attachedF=true;
return true;
Expand Down
Loading

0 comments on commit e4762b0

Please sign in to comment.