From a6d5fbcbf25d5e84990793385d2ddf6e303f3106 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 18 Mar 2024 15:44:29 +0100 Subject: [PATCH] controlBoardRemapper: align the mandatory interfaces to controlBoard_nws_yarp --- doc/release/master.md | 6 +- .../ControlBoardRemapper.cpp | 155 ++++++++-- .../ControlBoardRemapperHelpers.cpp | 30 +- .../tests/controlBoardRemapper_t1_test.cpp | 266 ++++++++++++++++++ 4 files changed, 422 insertions(+), 35 deletions(-) diff --git a/doc/release/master.md b/doc/release/master.md index afcea33a9a0..286dba55398 100644 --- a/doc/release/master.md +++ b/doc/release/master.md @@ -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 \ No newline at end of file + See https://github.com/robotology/yarp/discussions/3078 diff --git a/src/devices/controlBoardRemapper/ControlBoardRemapper.cpp b/src/devices/controlBoardRemapper/ControlBoardRemapper.cpp index dd310dc0b20..86ae12b59b1 100644 --- a/src/devices/controlBoardRemapper/ControlBoardRemapper.cpp +++ b/src/devices/controlBoardRemapper/ControlBoardRemapper.cpp @@ -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; } @@ -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; } @@ -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; } @@ -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; } @@ -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; } @@ -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; } @@ -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; } @@ -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; } @@ -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], @@ -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], @@ -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; } @@ -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; } @@ -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; } @@ -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; } @@ -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; } @@ -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; } @@ -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; } @@ -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; } @@ -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; } diff --git a/src/devices/controlBoardRemapper/ControlBoardRemapperHelpers.cpp b/src/devices/controlBoardRemapper/ControlBoardRemapperHelpers.cpp index 1b9762223d2..9f0a5528481 100644 --- a/src/devices/controlBoardRemapper/ControlBoardRemapperHelpers.cpp +++ b/src/devices/controlBoardRemapper/ControlBoardRemapperHelpers.cpp @@ -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; @@ -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; diff --git a/src/devices/controlBoardRemapper/tests/controlBoardRemapper_t1_test.cpp b/src/devices/controlBoardRemapper/tests/controlBoardRemapper_t1_test.cpp index 848bb3203d5..496474d2b79 100644 --- a/src/devices/controlBoardRemapper/tests/controlBoardRemapper_t1_test.cpp +++ b/src/devices/controlBoardRemapper/tests/controlBoardRemapper_t1_test.cpp @@ -36,6 +36,24 @@ const char *fmcC_file_content = "device fakeMotionControl\n" "\n" "AxisName (\"axisC1\" \"axisC2\" \"axisC3\" \"axisC4\") \n"; +const char *fmcA_file_content_micro = "device fakeMotionControlMicro\n" + "[GENERAL]\n" + "Joints 2\n" + "\n" + "AxisName (\"axisA1\" \"axisA2\") \n"; + +const char *fmcB_file_content_micro = "device fakeMotionControlMicro\n" + "[GENERAL]\n" + "Joints 3\n" + "\n" + "AxisName (\"axisB1\" \"axisB2\" \"axisB3\") \n"; + +const char *fmcC_file_content_micro = "device fakeMotionControlMicro\n" + "[GENERAL]\n" + "Joints 4\n" + "\n" + "AxisName (\"axisC1\" \"axisC2\" \"axisC3\" \"axisC4\") \n"; + const char *wrapperA_file_content = "device controlBoard_nws_yarp\n" "name /testRemapperRobot/a\n" "period 0.01\n"; @@ -141,9 +159,82 @@ static void checkRemapper(yarp::dev::PolyDriver & ddRemapper, int rand, size_t n } +static void checkRemapperMicro(yarp::dev::PolyDriver & ddRemapper, int rand, size_t nrOfRemappedAxes) +{ + IPositionControl *pos = nullptr; + REQUIRE(ddRemapper.view(pos)); // interface position correctly opened + REQUIRE(pos); + + int axes = 0; + CHECK(pos->getAxes(&axes)); // getAxes returned correctly + CHECK((size_t) axes == nrOfRemappedAxes); // remapper seems functional + + IPositionDirect *posdir = nullptr; + REQUIRE(ddRemapper.view(posdir)); // direct position interface correctly opened + REQUIRE(posdir); + + IEncoders * encs = nullptr; + REQUIRE(ddRemapper.view(encs)); // encoders interface correctly opened + REQUIRE(encs); + + IControlMode *ctrlmode = nullptr; + REQUIRE(ddRemapper.view(ctrlmode)); // control mode interface correctly opened + REQUIRE(ctrlmode); + + // Vector used for setting/getting data from the controlboard + std::vector setPosition(nrOfRemappedAxes,-10), + setRefSpeeds(nrOfRemappedAxes,-15), + readedPosition(nrOfRemappedAxes,-20), + readedEncoders(nrOfRemappedAxes,-30); + + for(size_t i=0; i < nrOfRemappedAxes; i++) + { + setPosition[i] = i*100.0+50.0 + rand; + setRefSpeeds[i] = i*10.0+5 + rand; + readedPosition[i] = -100 + rand; + } + + // Set the control mode in position direct + std::vector settedControlMode(nrOfRemappedAxes,VOCAB_CM_POSITION_DIRECT); + std::vector readedControlMode(nrOfRemappedAxes,VOCAB_CM_POSITION); + + CHECK_FALSE(ctrlmode->setControlModes(settedControlMode.data())); + + // Check that the read control mode is actually position direct + // Let's try 10 times because if the remapper is using some remotecontrolboards, + // it is possible that this return false if it is called before the first message + // has been received from the controlboard_nws_yarp + bool getControlModesOk = false; + for(int wait=0; wait < 20 && !getControlModesOk; wait++) + { + getControlModesOk = ctrlmode->getControlModes(readedControlMode.data()); + yarp::os::Time::delay(0.005); + } + CHECK_FALSE(getControlModesOk); + + // Test position direct methods + + // Set also the speeds in the mean time, so we are sure that we don't get + // spurios successful set/get of position because of intermediate buffers + CHECK_FALSE(pos->setRefSpeeds(setRefSpeeds.data())); + + // Wait some time to make sure that the vector has been correctly propagated + // back and forth + yarp::os::Time::delay(0.1); + + // Read position + CHECK_FALSE(posdir->getRefPositions(readedPosition.data())); + + // Do a similar test for the encoders + // in fakeMotionControl their value is the one setted with setPosition + CHECK(encs->getEncoders(readedEncoders.data())); // getEncoders correctly called +} + + TEST_CASE("dev::ControlBoardRemapperTest", "[yarp::dev]") { YARP_REQUIRE_PLUGIN("fakeMotionControl", "device"); + YARP_REQUIRE_PLUGIN("fakeMotionControlMicro", "device"); YARP_REQUIRE_PLUGIN("controlboardremapper", "device"); YARP_REQUIRE_PLUGIN("controlBoard_nws_yarp", "device"); YARP_REQUIRE_PLUGIN("remotecontrolboardremapper", "device"); @@ -321,5 +412,180 @@ TEST_CASE("dev::ControlBoardRemapperTest", "[yarp::dev]") } } + SECTION("Test the controlboard remapper micro") + { + // We first allocate three fakeMotionControl boards + // and their wrappers that we will remap using the remapper + std::vector fmcbs; + std::vector wrappers; + fmcbs.resize(3); + wrappers.resize(3); + + std::vector fmcbsSizes; + fmcbsSizes.push_back(2); + fmcbsSizes.push_back(3); + fmcbsSizes.push_back(4); + + std::vector fmcbsNames; + fmcbsNames.push_back("fakeControlBoardAMicro"); + fmcbsNames.push_back("fakeControlBoardBMicro"); + fmcbsNames.push_back("fakeControlBoardCMicro"); + + std::vector wrapperNetworks; + wrapperNetworks.push_back("net_a"); + wrapperNetworks.push_back("net_b"); + wrapperNetworks.push_back("net_c"); + + + for(int i=0; i < 3; i++) + { + fmcbs[i] = new PolyDriver(); + + Property p; + + if(i==0) { p.fromConfig(fmcA_file_content_micro); } + if(i==1) { p.fromConfig(fmcB_file_content_micro); } + if(i==2) { p.fromConfig(fmcC_file_content_micro); } + + REQUIRE(fmcbs[i]->open(p)); // fakeMotionControlBoard open reported successful + + IPositionControl *pos = nullptr; + REQUIRE_FALSE(fmcbs[i]->view(pos)); // interface position correctly not opened + REQUIRE_FALSE(pos); + + int axes = 0; + + IAxisInfoRaw *axisInfo = nullptr; + REQUIRE(fmcbs[i]->view(axisInfo)); // interface axisInfo correctly opened + REQUIRE(axisInfo); + axisInfo->getAxes(&axes); + CHECK(axes == fmcbsSizes[i]); // fakeMotionControlBoard seems functional + + // Open the wrapper + wrappers[i] = new PolyDriver(); + + if(i==0) { p.fromConfig(wrapperA_file_content); } + if(i==1) { p.fromConfig(wrapperB_file_content); } + if(i==2) { p.fromConfig(wrapperC_file_content); } + + REQUIRE(wrappers[i]->open(p)); // controlBoard_nws_yarp open reported successful + + yarp::dev::IMultipleWrapper *iwrap = nullptr; + REQUIRE(wrappers[i]->view(iwrap)); // interface for multiple wrapper correctly opened for the controlBoard_nws_yarp + REQUIRE(iwrap); + + PolyDriverList pdList; + pdList.push(fmcbs[i],wrapperNetworks[i].c_str()); + + REQUIRE(iwrap->attachAll(pdList)); // controlBoard_nws_yarp attached successfully to the device + } + + // Create a list containing all the fake controlboards + yarp::dev::PolyDriverList fmcList; + + for(int i=0; i < 3; i++) + { + fmcList.push(fmcbs[i],fmcbsNames[i].c_str()); + } + + // Open a controlboardremapper with the wrong axisName, + // and make sure that if fails during attachAll + PolyDriver ddRemapperWN; + Property pRemapperWN; + pRemapperWN.put("device","controlboardremapper"); + pRemapperWN.addGroup("axesNames"); + Bottle & axesListWN = pRemapperWN.findGroup("axesNames").addList(); + axesListWN.addString("axisA1"); + axesListWN.addString("axisB1"); + axesListWN.addString("axisC1"); + axesListWN.addString("axisB3"); + axesListWN.addString("axisC3"); + axesListWN.addString("axisA2"); + axesListWN.addString("thisIsAnAxisNameThatDoNotExist"); + + REQUIRE(ddRemapperWN.open(pRemapperWN)); // controlboardremapper with wrong names open reported successful + + yarp::dev::IMultipleWrapper *imultwrapWN = nullptr; + REQUIRE(ddRemapperWN.view(imultwrapWN)); // interface for multiple wrapper with wrong names correctly opened + REQUIRE(imultwrapWN); + + REQUIRE_FALSE(imultwrapWN->attachAll(fmcList)); // attachAll for controlboardremapper with wrong names successful + + // Make sure that a controlboard in which attachAll is not successfull + // closes correctly + CHECK(ddRemapperWN.close()); // controlboardremapper with wrong names close was successful + + // Open the controlboardremapper + PolyDriver ddRemapper; + Property pRemapper; + pRemapper.put("device","controlboardremapper"); + pRemapper.addGroup("axesNames"); + Bottle & axesList = pRemapper.findGroup("axesNames").addList(); + axesList.addString("axisA1"); + axesList.addString("axisB1"); + axesList.addString("axisC1"); + axesList.addString("axisB3"); + axesList.addString("axisC3"); + axesList.addString("axisA2"); + size_t nrOfRemappedAxes = 6; + + REQUIRE(ddRemapper.open(pRemapper)); // controlboardremapper open reported successful + + yarp::dev::IMultipleWrapper *imultwrap = nullptr; + REQUIRE(ddRemapper.view(imultwrap)); // interface for multiple wrapper correctly opened + REQUIRE(imultwrap); + + REQUIRE(imultwrap->attachAll(fmcList)); // attachAll for controlboardremapper successful + + + // Test the controlboardremapper + checkRemapperMicro(ddRemapper,200,nrOfRemappedAxes); + + // Open the remotecontrolboardremapper + PolyDriver ddRemoteRemapper; + Property pRemoteRemapper; + pRemoteRemapper.put("device","remotecontrolboardremapper"); + pRemoteRemapper.addGroup("axesNames"); + Bottle & remoteAxesList = pRemoteRemapper.findGroup("axesNames").addList(); + remoteAxesList.addString("axisA1"); + remoteAxesList.addString("axisB1"); + remoteAxesList.addString("axisC1"); + remoteAxesList.addString("axisB3"); + remoteAxesList.addString("axisC3"); + remoteAxesList.addString("axisA2"); + + Bottle remoteControlBoards; + Bottle & remoteControlBoardsList = remoteControlBoards.addList(); + remoteControlBoardsList.addString("/testRemapperRobot/a"); + remoteControlBoardsList.addString("/testRemapperRobot/b"); + remoteControlBoardsList.addString("/testRemapperRobot/c"); + pRemoteRemapper.put("remoteControlBoards",remoteControlBoards.get(0)); + + pRemoteRemapper.put("localPortPrefix","/test/remoteControlBoardRemapper"); + + Property & opts = pRemoteRemapper.addGroup("REMOTE_CONTROLBOARD_OPTIONS"); + opts.put("writeStrict","on"); + + REQUIRE(ddRemoteRemapper.open(pRemoteRemapper)); // remotecontrolboardremapper open reported successful, testing it + + // Test the remotecontrolboardremapper + checkRemapperMicro(ddRemoteRemapper,100,nrOfRemappedAxes); + + // Close devices + imultwrap->detachAll(); + ddRemapper.close(); + ddRemoteRemapper.close(); + + for(int i=0; i < 3; i++) + { + wrappers[i]->close(); + delete wrappers[i]; + wrappers[i] = nullptr; + fmcbs[i]->close(); + delete fmcbs[i]; + fmcbs[i] = nullptr; + } + } + Network::setLocalMode(false); }