From 31630227419d9e49d10d6370ebe806d5f19a5e81 Mon Sep 17 00:00:00 2001 From: Alberto Cardellino Date: Wed, 7 Dec 2016 15:02:29 +0100 Subject: [PATCH 1/5] RemoteControlBoard: drop compatibility with old version of ControlBoardWrapper with a single state port --- .../RemoteControlBoard/RemoteControlBoard.cpp | 690 ++++-------------- 1 file changed, 128 insertions(+), 562 deletions(-) diff --git a/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp b/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp index 8a1384ed89..b3e10d7c39 100644 --- a/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp +++ b/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp @@ -313,7 +313,6 @@ class yarp::dev::RemoteControlBoard : // yarp::os::Port extendedIntputStatePort; // Port /stateExt:i reading the state of the joints jointData last_wholePart; // tmp to store last received data for whole part - bool controlBoardWrapper1_compatibility; ConstString remote; ConstString local; mutable Stamp lastStamp; //this is shared among all calls that read encoders @@ -1031,7 +1030,6 @@ class yarp::dev::RemoteControlBoard : njIsKnown = false; writeStrict_singleJoint = true; writeStrict_moreJoints = false; - controlBoardWrapper1_compatibility = false; } /** @@ -1188,7 +1186,6 @@ class yarp::dev::RemoteControlBoard : ok = Network::connect(s1, extendedIntputStatePort.getName(), carrier); if (ok) { - controlBoardWrapper1_compatibility = false; // set the QoS preferences for the 'state' port if (config.check("local_qos") || config.check("remote_qos")) NetworkBase::setConnectionQos(s1, extendedIntputStatePort.getName(), remoteQos, localQos, false); @@ -1197,8 +1194,6 @@ class yarp::dev::RemoteControlBoard : { yError("*** Extended port %s was not found on the controlBoardWrapper I'm connecting to. Falling back to compatibility behaviour\n", s1.c_str()); yWarning("Updating to newer yarp and the usage of controlBoardWrapper2 is suggested***\n"); - //connectionProblem = true; // for compatibility - controlBoardWrapper1_compatibility = true; } } @@ -1619,20 +1614,13 @@ class yarp::dev::RemoteControlBoard : virtual bool getEncoder(int j, double *v) { double localArrivalTime = 0.0; - bool ret; - if(controlBoardWrapper1_compatibility) - { - ret=state_p.getLast(j, *v, lastStamp, localArrivalTime); - } - else - { - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastSingle(j, VOCAB_ENCODER, v, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } - if (ret && Time::now()-localArrivalTime>TIMEOUT) - ret=false; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_ENCODER, v, lastStamp, localArrivalTime); + extendedPortMutex.post(); + + if (ret && ( (Time::now()-localArrivalTime) > TIMEOUT) ) + ret = false; return ret; } @@ -1646,21 +1634,14 @@ class yarp::dev::RemoteControlBoard : virtual bool getEncoderTimed(int j, double *v, double *t) { double localArrivalTime = 0.0; - bool ret = false; - if(controlBoardWrapper1_compatibility) - { - ret=state_p.getLast(j, *v, lastStamp, localArrivalTime); - } - else - { - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastSingle(j, VOCAB_ENCODER, v, lastStamp, localArrivalTime); - *t=lastStamp.getTime(); - extendedPortMutex.post(); - } - if (ret && Time::now()-localArrivalTime>TIMEOUT) - ret=false; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_ENCODER, v, lastStamp, localArrivalTime); + *t=lastStamp.getTime(); + extendedPortMutex.post(); + + if (ret && ( (Time::now()-localArrivalTime) > TIMEOUT) ) + ret = false; return ret; } @@ -1675,39 +1656,12 @@ class yarp::dev::RemoteControlBoard : * from the server or that they are not being streamed with the expected rate. */ virtual bool getEncoders(double *encs) { - if (!isLive()) return false; - // particular case, does not use RPC - bool ret = false; - double localArrivalTime=0.0; - - if(controlBoardWrapper1_compatibility) - { - Vector tmp(nj); - - // mutex.wait(); - ret=state_p.getLast(tmp,lastStamp,localArrivalTime); - // mutex.post(); - - if (ret) - { - if (tmp.size() != (size_t)nj) - yWarning("tmp.size: %d nj %d\n", (int)tmp.size(), nj); + double localArrivalTime = 0.0; - yAssert (tmp.size() == (size_t)nj); - memcpy (encs, &(tmp.operator [](0)), sizeof(double)*nj); + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastVector(VOCAB_ENCODERS, encs, lastStamp, localArrivalTime); + extendedPortMutex.post(); - ////////////////////////// HANDLE TIMEOUT - // fill the vector anyway - if (Time::now()-localArrivalTime>TIMEOUT) - ret=false; - } - } - else - { - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastVector(VOCAB_ENCODERS, encs, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } return ret; } @@ -1718,40 +1672,13 @@ class yarp::dev::RemoteControlBoard : * @return true/false on success/failure */ virtual bool getEncodersTimed(double *encs, double *ts) { - if (!isLive()) return false; - // particular case, does not use RPC double localArrivalTime=0.0; - bool ret=false; - if(controlBoardWrapper1_compatibility) - { - Vector tmp(nj); - // mutex.wait(); - ret=state_p.getLast(tmp,lastStamp,localArrivalTime); - // mutex.post(); - - if (ret) - { - if (tmp.size() != (size_t)nj) - yWarning("tmp.size: %d nj %d\n", (int)tmp.size(), nj); - - yAssert (tmp.size() == (size_t)nj); - for(int j=0; j TIMEOUT) ret=false; @@ -1765,18 +1692,11 @@ class yarp::dev::RemoteControlBoard : */ virtual bool getEncoderSpeed(int j, double *sp) { - bool ret(false); - if(controlBoardWrapper1_compatibility) - { - ret = get1V1I1D(VOCAB_ENCODER_SPEED, j, sp); - } - else - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastSingle(j, VOCAB_ENCODER_SPEED, sp, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } + double localArrivalTime=0.0; + + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_ENCODER_SPEED, sp, lastStamp, localArrivalTime); + extendedPortMutex.post(); return ret; } @@ -1788,18 +1708,11 @@ class yarp::dev::RemoteControlBoard : */ virtual bool getEncoderSpeeds(double *spds) { - bool ret(false); - if(controlBoardWrapper1_compatibility) - { - ret = get1VDA(VOCAB_ENCODER_SPEEDS, spds); - } - else - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastVector(VOCAB_ENCODER_SPEEDS, spds, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } + double localArrivalTime=0.0; + + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastVector(VOCAB_ENCODER_SPEEDS, spds, lastStamp, localArrivalTime); + extendedPortMutex.post(); return ret; } @@ -1811,18 +1724,10 @@ class yarp::dev::RemoteControlBoard : virtual bool getEncoderAcceleration(int j, double *acc) { - bool ret(false); - if(controlBoardWrapper1_compatibility) - { - ret = get1V1I1D(VOCAB_ENCODER_ACCELERATION, j, acc); - } - else - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastSingle(j, VOCAB_ENCODER_ACCELERATION, acc, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_ENCODER_ACCELERATION, acc, lastStamp, localArrivalTime); + extendedPortMutex.post(); return ret; } @@ -1833,18 +1738,10 @@ class yarp::dev::RemoteControlBoard : */ virtual bool getEncoderAccelerations(double *accs) { - bool ret(false); - if(controlBoardWrapper1_compatibility) - { - ret = get1VDA(VOCAB_ENCODER_ACCELERATIONS, accs); - } - else - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastVector(VOCAB_ENCODER_ACCELERATIONS, accs, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastVector(VOCAB_ENCODER_ACCELERATIONS, accs, lastStamp, localArrivalTime); + extendedPortMutex.post(); return ret; } @@ -1989,18 +1886,12 @@ class yarp::dev::RemoteControlBoard : virtual bool getMotorEncoder(int j, double *v) { double localArrivalTime = 0.0; - bool ret; - if(controlBoardWrapper1_compatibility) - { - ret = get1V1I1D(VOCAB_MOTOR_ENCODER, j, v); - } - else - { - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastSingle(j, VOCAB_MOTOR_ENCODER, v, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } - if (ret && Time::now()-localArrivalTime>TIMEOUT) + + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_MOTOR_ENCODER, v, lastStamp, localArrivalTime); + extendedPortMutex.post(); + + if(ret && ((Time::now()-localArrivalTime) > TIMEOUT) ) ret=false; return ret; @@ -2015,20 +1906,13 @@ class yarp::dev::RemoteControlBoard : virtual bool getMotorEncoderTimed(int j, double *v, double *t) { double localArrivalTime = 0.0; - bool ret(false); - if(controlBoardWrapper1_compatibility) - { - return get1V1I1D(VOCAB_MOTOR_ENCODER, j, v); - } - else - { - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastSingle(j, VOCAB_MOTOR_ENCODER, v, lastStamp, localArrivalTime); - *t=lastStamp.getTime(); - extendedPortMutex.post(); - } - if (ret && Time::now()-localArrivalTime>TIMEOUT) + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_MOTOR_ENCODER, v, lastStamp, localArrivalTime); + *t=lastStamp.getTime(); + extendedPortMutex.post(); + + if(ret && ((Time::now()-localArrivalTime) > TIMEOUT) ) ret=false; return ret; @@ -2045,39 +1929,12 @@ class yarp::dev::RemoteControlBoard : */ virtual bool getMotorEncoders(double *encs) { - if (!isLive()) return false; - // particular case, does not use RPC - bool ret = false; double localArrivalTime=0.0; - if(controlBoardWrapper1_compatibility) - { - Vector tmp(nj); - - // mutex.wait(); - return get1VDA(VOCAB_MOTOR_ENCODERS, tmp.data()); - // mutex.post(); - - if (ret) - { - if (tmp.size() != (size_t)nj) - yWarning("tmp.size: %d nj %d\n", (int)tmp.size(), nj); - - yAssert (tmp.size() == (size_t)nj); - memcpy (encs, &(tmp.operator [](0)), sizeof(double)*nj); + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastVector(VOCAB_MOTOR_ENCODERS, encs, lastStamp, localArrivalTime); + extendedPortMutex.post(); - ////////////////////////// HANDLE TIMEOUT - // fill the vector anyway - if (Time::now()-localArrivalTime>TIMEOUT) - ret=false; - } - } - else - { - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastVector(VOCAB_MOTOR_ENCODERS, encs, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } return ret; } @@ -2089,42 +1946,14 @@ class yarp::dev::RemoteControlBoard : */ virtual bool getMotorEncodersTimed(double *encs, double *ts) { - if (!isLive()) return false; - // particular case, does not use RPC double localArrivalTime=0.0; - bool ret=false; - double time = yarp::os::Time::now(); - if(controlBoardWrapper1_compatibility) - { - Vector tmp(nj); - // mutex.wait(); - return get1VDA(VOCAB_MOTOR_ENCODERS, tmp.data()); - // mutex.post(); + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastVector(VOCAB_MOTOR_ENCODERS, encs, lastStamp, localArrivalTime); + std::fill_n(ts, nj, lastStamp.getTime()); + extendedPortMutex.post(); - if (ret) - { - if (tmp.size() != (size_t)nj) - yWarning("tmp.size: %d nj %d\n", (int)tmp.size(), nj); - - yAssert (tmp.size() == (size_t)nj); - for(int j=0; jTIMEOUT) + if(ret && ((Time::now()-localArrivalTime) > TIMEOUT) ) ret=false; return ret; @@ -2137,16 +1966,11 @@ class yarp::dev::RemoteControlBoard : */ virtual bool getMotorEncoderSpeed(int j, double *sp) { - if(controlBoardWrapper1_compatibility) - return get1V1I1D(VOCAB_MOTOR_ENCODER_SPEED, j, sp); - else - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_MOTOR_ENCODER_SPEED, sp, lastStamp, localArrivalTime); - extendedPortMutex.post(); - return ret; - } + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_MOTOR_ENCODER_SPEED, sp, lastStamp, localArrivalTime); + extendedPortMutex.post(); + return ret; } /** @@ -2156,18 +1980,10 @@ class yarp::dev::RemoteControlBoard : */ virtual bool getMotorEncoderSpeeds(double *spds) { - bool ret(false); - if(!controlBoardWrapper1_compatibility) - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastVector(VOCAB_MOTOR_ENCODER_SPEEDS, spds, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } - else - { - ret = get1VDA(VOCAB_MOTOR_ENCODER_SPEEDS, spds); - } + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastVector(VOCAB_MOTOR_ENCODER_SPEEDS, spds, lastStamp, localArrivalTime); + extendedPortMutex.post(); return ret; } @@ -2179,18 +1995,10 @@ class yarp::dev::RemoteControlBoard : virtual bool getMotorEncoderAcceleration(int j, double *acc) { - bool ret(false); - if(!controlBoardWrapper1_compatibility) - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastSingle(j, VOCAB_MOTOR_ENCODER_ACCELERATION, acc, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } - else - { - ret = get1V1I1D(VOCAB_MOTOR_ENCODER_ACCELERATION, j, acc); - } + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_MOTOR_ENCODER_ACCELERATION, acc, lastStamp, localArrivalTime); + extendedPortMutex.post(); return ret; } @@ -2201,18 +2009,10 @@ class yarp::dev::RemoteControlBoard : */ virtual bool getMotorEncoderAccelerations(double *accs) { - bool ret(false); - if(!controlBoardWrapper1_compatibility) - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastVector(VOCAB_MOTOR_ENCODER_SPEEDS, accs, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } - else - { - ret = get1VDA(VOCAB_MOTOR_ENCODER_ACCELERATIONS, accs); - } + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastVector(VOCAB_MOTOR_ENCODER_SPEEDS, accs, lastStamp, localArrivalTime); + extendedPortMutex.post(); return ret; } @@ -2984,35 +2784,19 @@ class yarp::dev::RemoteControlBoard : bool getTorque(int j, double *t) { - bool ret(false); - if(controlBoardWrapper1_compatibility) - { - ret = get2V1I1D(VOCAB_TORQUE, VOCAB_TRQ, j, t); - } - else - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastSingle(j, VOCAB_TRQ, t, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_TRQ, t, lastStamp, localArrivalTime); + extendedPortMutex.post(); return ret; } bool getTorques(double *t) { - bool ret(false); - if(controlBoardWrapper1_compatibility) - { - ret = get2V1DA(VOCAB_TORQUE, VOCAB_TRQS, t); - } - else - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastVector(VOCAB_TRQS, t, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastVector(VOCAB_TRQS, t, lastStamp, localArrivalTime); + extendedPortMutex.post(); return ret; } @@ -3238,118 +3022,39 @@ class yarp::dev::RemoteControlBoard : bool getControlMode(int j, int *mode) { - bool ok = false; - if(controlBoardWrapper1_compatibility) - { - Bottle cmd, resp; - cmd.addVocab(VOCAB_GET); - cmd.addVocab(VOCAB_ICONTROLMODE); - cmd.addVocab(VOCAB_CM_CONTROL_MODE); - cmd.addInt(j); - - ok = rpc_p.write(cmd, resp); - if (CHECK_FAIL(ok, resp)) - { - *mode=resp.get(2).asVocab(); - } - } - else - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ok = extendedIntputStatePort.getLastSingle(j, VOCAB_CM_CONTROL_MODE, mode, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } - return ok; - + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_CM_CONTROL_MODE, mode, lastStamp, localArrivalTime); + extendedPortMutex.post(); + return ret; } // IControlMode2 bool getControlModes(const int n_joint, const int *joints, int *modes) { - bool ok = false; - if(controlBoardWrapper1_compatibility) + double localArrivalTime=0.0; + + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastVector(VOCAB_CM_CONTROL_MODES, last_wholePart.controlMode.data(), lastStamp, localArrivalTime); + if(ret) { - Bottle cmd, resp; - cmd.addVocab(VOCAB_GET); - cmd.addVocab(VOCAB_ICONTROLMODE); - cmd.addVocab(VOCAB_CM_CONTROL_MODE_GROUP); - cmd.addInt(n_joint); - Bottle& l1 = cmd.addList(); for (int i = 0; i < n_joint; i++) - l1.addInt(joints[i]); - - ok = rpc_p.write(cmd, resp); - - if (CHECK_FAIL(ok, resp)) - { - Bottle* lp = resp.get(2).asList(); - if (lp == 0) - return false; - Bottle& l = *lp; - - if (n_joint != l.size()) - { - yError("getControlModes group received an answer of wrong length. expected %d, actual size is %d", n_joint, l.size()); - return false; - } - - for (int i = 0; i < n_joint; i++) - modes[i] = l.get(i).asInt(); - } + modes[i] = last_wholePart.controlMode[joints[i]]; } else - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ok = extendedIntputStatePort.getLastVector(VOCAB_CM_CONTROL_MODES, last_wholePart.controlMode.data(), lastStamp, localArrivalTime); + ret = false; - if(ok) - { - for (int i = 0; i < n_joint; i++) - modes[i] = last_wholePart.controlMode[joints[i]]; - } - else - { - ok = false; - } - extendedPortMutex.post(); - } - return ok; + extendedPortMutex.post(); + return ret; } // IControlMode bool getControlModes(int *modes) { - bool ret = false; - if(controlBoardWrapper1_compatibility) - { - if (!isLive()) return false; - Bottle cmd, resp; - cmd.addVocab(VOCAB_GET); - cmd.addVocab(VOCAB_ICONTROLMODE); - cmd.addVocab(VOCAB_CM_CONTROL_MODES); - - ret = rpc_p.write(cmd, resp); - if (CHECK_FAIL(ret, resp)) - { - Bottle* lp = resp.get(2).asList(); - if (lp == 0) - return false; - Bottle& l = *lp; - int njs = l.size(); - yAssert (nj == njs); - for (int i = 0; i < nj; i++) - modes[i] = l.get(i).asInt(); - } - } - else - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastVector(VOCAB_CM_CONTROL_MODES, modes, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastVector(VOCAB_CM_CONTROL_MODES, modes, lastStamp, localArrivalTime); + extendedPortMutex.post(); return ret; } @@ -3617,144 +3322,37 @@ class yarp::dev::RemoteControlBoard : // Interaction Mode interface bool getInteractionMode(int axis, yarp::dev::InteractionModeEnum* mode) { - bool ok = false; - if(controlBoardWrapper1_compatibility) - { - Bottle cmd, response; - - if (!isLive()) return false; - - cmd.addVocab(VOCAB_GET); - cmd.addVocab(VOCAB_INTERFACE_INTERACTION_MODE); - cmd.addVocab(VOCAB_INTERACTION_MODE); - cmd.addInt(axis); - - ok = rpc_p.write(cmd, response); - - if (CHECK_FAIL(ok, response)) - { - yAssert (response.size()>=1); - *mode = (InteractionModeEnum) response.get(0).asVocab(); - } - } - else - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ok = extendedIntputStatePort.getLastSingle(axis, VOCAB_INTERACTION_MODE, (int*) mode, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } - return ok; + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastSingle(axis, VOCAB_INTERACTION_MODE, (int*) mode, lastStamp, localArrivalTime); + extendedPortMutex.post(); + return ret; } bool getInteractionModes(int n_joints, int *joints, yarp::dev::InteractionModeEnum* modes) { - bool ok = false; - if(controlBoardWrapper1_compatibility) - { - Bottle cmd, response; - if (!isLive()) return false; - - cmd.addVocab(VOCAB_GET); - cmd.addVocab(VOCAB_INTERFACE_INTERACTION_MODE); - cmd.addVocab(VOCAB_INTERACTION_MODE_GROUP); - - cmd.addInt(n_joints); + double localArrivalTime=0.0; - Bottle& l1 = cmd.addList(); + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastVector(VOCAB_CM_CONTROL_MODES, last_wholePart.interactionMode.data(), lastStamp, localArrivalTime); + if(ret) + { for (int i = 0; i < n_joints; i++) - l1.addInt(joints[i]); - - ok = rpc_p.write(cmd, response); - - if (CHECK_FAIL(ok, response)) - { - int i; - Bottle& list = *(response.get(0).asList()); - yAssert(list.size() >= n_joints) - - if (list.size() != n_joints) - { - yError("getInteractionModes, length of response does not match: expected %d, received %d\n ", n_joints , list.size() ); - return false; - } - else - { - for (i = 0; i < n_joints; i++) - { - modes[i] = (yarp::dev::InteractionModeEnum) list.get(i).asVocab(); - } - return true; - } - } + modes[i] = (yarp::dev::InteractionModeEnum)last_wholePart.interactionMode[joints[i]]; } else - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ok = extendedIntputStatePort.getLastVector(VOCAB_CM_CONTROL_MODES, last_wholePart.interactionMode.data(), lastStamp, localArrivalTime); + ret = false; - if(ok) - { - for (int i = 0; i < n_joints; i++) - modes[i] = (yarp::dev::InteractionModeEnum)last_wholePart.interactionMode[joints[i]]; - } - else - { - ok = false; - } - extendedPortMutex.post(); - } - return ok; + extendedPortMutex.post(); + return ret; } bool getInteractionModes(yarp::dev::InteractionModeEnum* modes) { - bool ret = false; - if(!controlBoardWrapper1_compatibility) - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastVector(VOCAB_INTERACTION_MODES, (int*) modes, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } - else - { - Bottle cmd, response; - if (!isLive()) return false; - - cmd.addVocab(VOCAB_GET); - cmd.addVocab(VOCAB_INTERFACE_INTERACTION_MODE); - cmd.addVocab(VOCAB_INTERACTION_MODES); - - bool ok = rpc_p.write(cmd, response); - - if (CHECK_FAIL(ok, response)) - { - int i; - Bottle& list = *(response.get(0).asList()); - yAssert(list.size() >= nj) - - if (list.size() != nj) - { - yError("getInteractionModes, length of response does not match: expected %d, received %d\n ", nj , list.size() ); - return false; - - } - else - { - for (i = 0; i < nj; i++) - { - modes[i] = (yarp::dev::InteractionModeEnum) list.get(i).asVocab(); - } - ret = true; - } - } - else - { - ret = false; - } - } + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastVector(VOCAB_INTERACTION_MODES, (int*) modes, lastStamp, localArrivalTime); + extendedPortMutex.post(); return ret; } @@ -3896,34 +3494,10 @@ class yarp::dev::RemoteControlBoard : virtual bool getOutput(int j, double *out) { - bool ret(false); - if(!controlBoardWrapper1_compatibility) - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastSingle(j, VOCAB_OUTPUT, out, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } - else - { - // both iOpenLoop and iPid getOutputs will pass here and use the VOCAB_OPENLOOP_INTERFACE - Bottle cmd, response; - cmd.addVocab(VOCAB_GET); - // cmd.addVocab(VOCAB_OPENLOOP_INTERFACE); - cmd.addVocab(VOCAB_OUTPUT); - cmd.addInt(j); - ret = rpc_p.write(cmd, response); - - if (CHECK_FAIL(ret, response)) - { - *out = response.get(2).asDouble(); - - getTimeStamp(response, lastStamp); - ret = true; - } - else - ret = false; - } + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastSingle(j, VOCAB_OUTPUT, out, lastStamp, localArrivalTime); + extendedPortMutex.post(); return ret; } @@ -3934,18 +3508,10 @@ class yarp::dev::RemoteControlBoard : */ virtual bool getOutputs(double *outs) { - bool ret(false); - if(!controlBoardWrapper1_compatibility) - { - double localArrivalTime=0.0; - extendedPortMutex.wait(); - ret = extendedIntputStatePort.getLastVector(VOCAB_OUTPUTS, outs, lastStamp, localArrivalTime); - extendedPortMutex.post(); - } - else - { - ret = get1VDA(VOCAB_OUTPUTS, outs); - } + double localArrivalTime=0.0; + extendedPortMutex.wait(); + bool ret = extendedIntputStatePort.getLastVector(VOCAB_OUTPUTS, outs, lastStamp, localArrivalTime); + extendedPortMutex.post(); return ret; } From acbc476b48b636ccbd27b608aaf6d31ad1a91d14 Mon Sep 17 00:00:00 2001 From: Alberto Cardellino Date: Wed, 7 Dec 2016 14:59:32 +0100 Subject: [PATCH 2/5] ControlBoardWrapper2: update protocol version number --- .../src/devices/ControlBoardWrapper/ControlBoardWrapper.h | 4 ++-- .../src/devices/RemoteControlBoard/RemoteControlBoard.cpp | 7 +++---- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.h b/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.h index c374b69f50..ed095aafc2 100644 --- a/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.h +++ b/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.h @@ -52,8 +52,8 @@ #endif #define PROTOCOL_VERSION_MAJOR 1 -#define PROTOCOL_VERSION_MINOR 5 -#define PROTOCOL_VERSION_TWEAK 1 +#define PROTOCOL_VERSION_MINOR 6 +#define PROTOCOL_VERSION_TWEAK 0 /* * To optimize memory allocation, for group of joints we can have one mem reserver for rpc port diff --git a/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp b/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp index b3e10d7c39..8f4e84c417 100644 --- a/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp +++ b/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp @@ -30,8 +30,8 @@ #include #define PROTOCOL_VERSION_MAJOR 1 -#define PROTOCOL_VERSION_MINOR 5 -#define PROTOCOL_VERSION_TWEAK 1 +#define PROTOCOL_VERSION_MINOR 6 +#define PROTOCOL_VERSION_TWEAK 0 using namespace yarp::os; using namespace yarp::dev; @@ -3549,11 +3549,10 @@ class yarp::dev::RemoteControlBoard : return true; // protocol did not match - yError("expecting protocol %d %d %d, but remotecontrolboard returned protocol version %d %d %d\n", + yError("expecting protocol %d %d %d, but the device we are connectin to has protocol version %d %d %d\n", PROTOCOL_VERSION_MAJOR, PROTOCOL_VERSION_MINOR, PROTOCOL_VERSION_TWEAK, protocolVersion.major, protocolVersion.minor, protocolVersion.tweak); - bool ret; if (ignore) { From 7378840d5cd6627f1e27621251ccb9f25f9188b2 Mon Sep 17 00:00:00 2001 From: Alberto Cardellino Date: Mon, 12 Dec 2016 15:01:05 +0100 Subject: [PATCH 3/5] RemoteControlBoard: remove /.../state:i input port from client --- .../RemoteControlBoard/RemoteControlBoard.cpp | 174 +----------------- .../stateExtendedReader.hpp | 2 +- 2 files changed, 10 insertions(+), 166 deletions(-) diff --git a/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp b/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp index 8f4e84c417..ae5e9104cf 100644 --- a/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp +++ b/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp @@ -11,12 +11,12 @@ #include #include #include -#include +#include #include #include #include -#include #include +#include #include @@ -25,7 +25,7 @@ #include #include #include -#include + #include @@ -46,6 +46,9 @@ namespace yarp{ #ifndef DOXYGEN_SHOULD_SKIP_THIS +const int DIAGNOSTIC_THREAD_RATE=1000; +const double TIMEOUT=0.5; + inline bool getTimeStamp(Bottle &bot, Stamp &st) { if (bot.get(3).asVocab()==VOCAB_TIMESTAMP) @@ -67,150 +70,16 @@ struct yarp::dev::ProtocolVersion }; -// encoders should arrive at least every 0.5s to be considered valide -// getEncoders will return false otherwise. -const double TIMEOUT=0.5; - -class StateInputPort : public yarp::os::BufferedPort -{ - yarp::sig::Vector last; - Semaphore mutex; - Stamp lastStamp; - double deltaT; - double deltaTMax; - double deltaTMin; - double prev; - double now; - - bool valid; - int count; -public: - - inline void resetStat() - { - mutex.wait(); - count=0; - deltaT=0; - deltaTMax=0; - deltaTMin=1e22; - now=Time::now(); - prev=now; - mutex.post(); - } - - StateInputPort() - { - valid=false; - resetStat(); - } - - using yarp::os::BufferedPort::onRead; - virtual void onRead(yarp::sig::Vector &v) - { - now=Time::now(); - mutex.wait(); - - if (count>0) - { - double tmpDT=now-prev; - deltaT+=tmpDT; - if (tmpDT>deltaTMax) - deltaTMax=tmpDT; - if (tmpDT=last.size()) - return false; - - mutex.wait(); - bool ret=valid; - if (ret) - { - v=last[j]; - stamp=lastStamp; - localArrivalTime=now; - } - mutex.post(); - return ret; - } - - inline bool getLast(yarp::sig::Vector &n, Stamp &stmp, double &localArrivalTime) - { - mutex.wait(); - bool ret=valid; - if (ret) - { - n=last; - stmp=lastStamp; - localArrivalTime=now; - } - mutex.post(); - - return ret; - } - - inline int getIterations() - { - mutex.wait(); - int ret=count; - mutex.post(); - return ret; - } - - // time is in ms - void getEstFrequency(int &ite, double &av, double &min, double &max) - { - mutex.wait(); - ite=count; - min=deltaTMin*1000; - max=deltaTMax*1000; - if (count<1) - { - av=0; - } - else - { - av=deltaT/count; - } - av=av*1000; - mutex.post(); - } - -}; - - - -#include - -using namespace yarp::os; - -const int DIAGNOSTIC_THREAD_RATE=1000; - class DiagnosticThread: public RateThread { - StateInputPort *owner; + StateExtendedInputPort *owner; ConstString ownerName; public: DiagnosticThread(int r): RateThread(r) { owner=0; } - void setOwner(StateInputPort *o) + void setOwner(StateExtendedInputPort *o) { owner=o; ownerName=owner->getName().c_str(); @@ -299,7 +168,6 @@ class yarp::dev::RemoteControlBoard : DiagnosticThread *diagnosticThread; PortReaderBuffer state_buffer; - StateInputPort state_p; PortWriterBuffer command_buffer; bool writeStrict_singleJoint; bool writeStrict_moreJoints; @@ -1117,17 +985,11 @@ class yarp::dev::RemoteControlBoard : s1 = local; s1 += "/command:o"; if (!command_p.open(s1.c_str())) { portProblem = true; } - s1 = local; - s1 += "/state:i"; - if (!state_p.open(s1.c_str())) { portProblem = true; } - s1 = local; s1 += "/stateExt:i"; if (!extendedIntputStatePort.open(s1.c_str())) { portProblem = true; } - //new code if (!portProblem) { - state_p.useCallback(); extendedIntputStatePort.useCallback(); } } @@ -1164,20 +1026,6 @@ class yarp::dev::RemoteControlBoard : if (config.check("local_qos") || config.check("remote_qos")) NetworkBase::setConnectionQos(command_p.getName(), s1.c_str(), localQos, remoteQos, false); - s1 = remote; - s1 += "/state:o"; - s2 = local; - s2 += "/state:i"; - ok = Network::connect(s1, state_p.getName(), carrier); - - if (!ok) { - yError("Problem connecting to %s from %s, is the remote device available?\n", s1.c_str(), state_p.getName().c_str()); - connectionProblem = true; - } - // set the QoS preferences for the 'state' port - if (config.check("local_qos") || config.check("remote_qos")) - NetworkBase::setConnectionQos(s1, state_p.getName(), remoteQos, localQos, false); - s1 = remote; s1 += "/stateExt:o"; s2 = local; @@ -1201,7 +1049,6 @@ class yarp::dev::RemoteControlBoard : rpc_p.close(); command_p.close(); - state_p.close(); extendedIntputStatePort.close(); return false; } @@ -1215,7 +1062,6 @@ class yarp::dev::RemoteControlBoard : command_buffer.detach(); rpc_p.close(); command_p.close(); - state_p.close(); extendedIntputStatePort.close(); return false; } @@ -1226,7 +1072,6 @@ class yarp::dev::RemoteControlBoard : command_buffer.detach(); rpc_p.close(); command_p.close(); - state_p.close(); extendedIntputStatePort.close(); return false; } @@ -1235,7 +1080,7 @@ class yarp::dev::RemoteControlBoard : if (config.check("diagnostic")) { diagnosticThread = new DiagnosticThread(DIAGNOSTIC_THREAD_RATE); - diagnosticThread->setOwner(&state_p); + diagnosticThread->setOwner(&extendedIntputStatePort); diagnosticThread->start(); } else @@ -1282,7 +1127,6 @@ class yarp::dev::RemoteControlBoard : rpc_p.close(); command_p.close(); - state_p.close(); extendedIntputStatePort.close(); return true; } diff --git a/src/libYARP_dev/src/devices/RemoteControlBoard/stateExtendedReader.hpp b/src/libYARP_dev/src/devices/RemoteControlBoard/stateExtendedReader.hpp index 49c88564f0..09e4b03a82 100644 --- a/src/libYARP_dev/src/devices/RemoteControlBoard/stateExtendedReader.hpp +++ b/src/libYARP_dev/src/devices/RemoteControlBoard/stateExtendedReader.hpp @@ -58,7 +58,7 @@ class StateExtendedInputPort:public yarp::os::BufferedPort StateExtendedInputPort(); - inline void resetStat(); + void resetStat(); void init(int numberOfJoints); using yarp::os::BufferedPort::onRead; From 7d72f204e8ed4cd5588f204ae3a76a54a6e711b3 Mon Sep 17 00:00:00 2001 From: Alberto Cardellino Date: Tue, 13 Dec 2016 15:14:07 +0100 Subject: [PATCH 4/5] update doc release [ci skip] --- doc/release/v2_3_70.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/doc/release/v2_3_70.md b/doc/release/v2_3_70.md index ea37eaf7bd..63c3392fcf 100644 --- a/doc/release/v2_3_70.md +++ b/doc/release/v2_3_70.md @@ -11,6 +11,10 @@ Important Changes * YARP_math can no longer be built using GSL. The `CREATE_LIB_MATH_USING_GSL` option was removed. Only Eigen is supported. `FindGSL.cmake` is no longer installed. +* 31/12/2016: `RemoteControlBoard` device is no longer compatible with `ControlBoardWrapper2` + devices that does not have the `stateExt:o` port. This change was introduced in + 30/07/2014 and back compatibility was mainteined up to now. `state:o` port in the + wrapper is still available for encoder reading. New Features From be10c02664badd0807d6e2c8410b66df5471b9dd Mon Sep 17 00:00:00 2001 From: Alberto Cardellino Date: Tue, 13 Dec 2016 15:16:15 +0100 Subject: [PATCH 5/5] remoteControlBoard: fix typo in error message [ci skip] --- .../src/devices/RemoteControlBoard/RemoteControlBoard.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp b/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp index ae5e9104cf..bc3e3e569d 100644 --- a/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp +++ b/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp @@ -3393,7 +3393,7 @@ class yarp::dev::RemoteControlBoard : return true; // protocol did not match - yError("expecting protocol %d %d %d, but the device we are connectin to has protocol version %d %d %d\n", + yError("expecting protocol %d %d %d, but the device we are connecting to has protocol version %d %d %d\n", PROTOCOL_VERSION_MAJOR, PROTOCOL_VERSION_MINOR, PROTOCOL_VERSION_TWEAK, protocolVersion.major, protocolVersion.minor, protocolVersion.tweak);