diff --git a/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.cpp b/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.cpp index e6c1a446d81..288dbfa7cfe 100644 --- a/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.cpp +++ b/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.cpp @@ -15,6 +15,8 @@ #include #include #include +#include +#include #include // for memset function @@ -477,6 +479,12 @@ bool ControlBoardWrapper::open(Searchable& config) return false; } + times.resize(controlledJoints); + ros_struct.name.resize(controlledJoints); + ros_struct.position.resize(controlledJoints); + ros_struct.velocity.resize(controlledJoints); + ros_struct.effort.resize(controlledJoints); + // In case attach is not deferred and the controlboard already owns a valid device // we can start the thread. Otherwise this will happen when attachAll is called if (ownDevices) @@ -875,47 +883,24 @@ void ControlBoardWrapper::run() yWarning() << "number of streaming intput messages to be read is " << inputStreamingPort.getPendingReads() << " and can overflow"; } - // Update the time by averaging all timestamps - double joint_timeStamp = 0.0; - - for (auto& subdevice : device.subdevices) - { - int axes = subdevice.axes; + // Small optimization: Avoid to call getEncoders twice, one for YARP port + // and again for ROS topic. + // + // Calling getStuff here on ros_struct because it is a class member, hence + // always available. In the other side, to have the yarp struct to write into + // it will be rewuired to call port.prepare, that it is something I should + // not do if the wrapper is in ROS_only configuration. - subdevice.refreshJointEncoders(); - subdevice.refreshMotorEncoders(); + bool positionsOk = getEncodersTimed(ros_struct.position.data(), times.data()); + bool speedsOk = getEncoderSpeeds(ros_struct.velocity.data()); + bool torqueOk = getTorques(ros_struct.effort.data()); - for (int l = 0; l < axes; l++) - { - joint_timeStamp+=subdevice.jointEncodersTimes[l]; - } - } - - timeMutex.lock(); - time.update(joint_timeStamp/controlledJoints); - timeMutex.unlock(); + // Update the port envelope time by averaging all timestamps + time.update(std::accumulate(times.begin(), times.end(), 0.0) / controlledJoints); if(useROS != ROS_only) { - yarp::sig::Vector& v = outputPositionStatePort.prepare(); - v.resize(controlledJoints); - - double *joint_encoders=v.data(); - - for (auto& subdevice : device.subdevices) - { - int axes=subdevice.axes; - - for(int l = 0; l < axes; l++) - { - joint_encoders[l]=subdevice.subDev_joint_encoders[l]; - } - joint_encoders+=subdevice.axes; //jump to next group - } - - outputPositionStatePort.setEnvelope(time); - outputPositionStatePort.write(); - + // handle stateExt first jointData &yarp_struct = extendedOutputState_buffer.get(); yarp_struct.jointPosition.resize(controlledJoints); @@ -930,8 +915,18 @@ void ControlBoardWrapper::run() yarp_struct.controlMode.resize(controlledJoints); yarp_struct.interactionMode.resize(controlledJoints); - yarp_struct.jointPosition_isValid = getEncoders(yarp_struct.jointPosition.data()); - yarp_struct.jointVelocity_isValid = getEncoderSpeeds(yarp_struct.jointVelocity.data()); + // Get already stored data from before. This is to avoid a double call to HW device, + // which may require more time. // + yarp_struct.jointPosition_isValid = positionsOk; + std::copy(ros_struct.position.begin(), ros_struct.position.end(), yarp_struct.jointPosition.begin()); + + yarp_struct.jointVelocity_isValid = speedsOk; + std::copy(ros_struct.velocity.begin(), ros_struct.velocity.end(), yarp_struct.jointVelocity.begin()); + + yarp_struct.torque_isValid = torqueOk; + std::copy(ros_struct.effort.begin(), ros_struct.effort.end(), yarp_struct.torque.begin()); + + // Get remaining data from HW yarp_struct.jointAcceleration_isValid = getEncoderAccelerations(yarp_struct.jointAcceleration.data()); yarp_struct.motorPosition_isValid = getMotorEncoders(yarp_struct.motorPosition.data()); yarp_struct.motorVelocity_isValid = getMotorEncoderSpeeds(yarp_struct.motorVelocity.data()); @@ -944,21 +939,19 @@ void ControlBoardWrapper::run() extendedOutputStatePort.setEnvelope(time); extendedOutputState_buffer.write(); + + // handle state:o + yarp::sig::Vector& v = outputPositionStatePort.prepare(); + v.resize(controlledJoints); + std::copy(yarp_struct.jointPosition.begin(), yarp_struct.jointPosition.end(), v.begin()); + + outputPositionStatePort.setEnvelope(time); + outputPositionStatePort.write(); } if(useROS != ROS_disabled) { - yarp::rosmsg::sensor_msgs::JointState ros_struct; - - ros_struct.name.resize(controlledJoints); - ros_struct.position.resize(controlledJoints); - ros_struct.velocity.resize(controlledJoints); - ros_struct.effort.resize(controlledJoints); - - getEncoders(ros_struct.position.data()); - getEncoderSpeeds(ros_struct.velocity.data()); - getTorques(ros_struct.effort.data()); - + // Data from HW have been gathered few lines before JointTypeEnum jType; for(int i=0; i< controlledJoints; i++) { diff --git a/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.h b/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.h index 6f06f54168b..ccf17593831 100644 --- a/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.h +++ b/src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.h @@ -286,11 +286,13 @@ class yarp::dev::ControlBoardWrapper: public yarp::dev::DeviceDriver, bool checkPortName(yarp::os::Searchable ¶ms); + yarp::rosmsg::sensor_msgs::JointState ros_struct; yarp::os::BufferedPort outputPositionStatePort; // Port /state:o streaming out the encoder positions yarp::os::BufferedPort inputStreamingPort; // Input streaming port for high frequency commands yarp::os::Port inputRPCPort; // Input RPC port for set/get remote calls yarp::os::Stamp time; // envelope to attach to the state port + yarp::sig::Vector times; // time for each joint yarp::os::Mutex timeMutex; // Buffer associated to the extendedOutputStatePort port; in this case we will use the type generated diff --git a/src/libYARP_dev/src/devices/ControlBoardWrapper/SubDevice.h b/src/libYARP_dev/src/devices/ControlBoardWrapper/SubDevice.h index f066cb0a0ec..b4128215bdc 100644 --- a/src/libYARP_dev/src/devices/ControlBoardWrapper/SubDevice.h +++ b/src/libYARP_dev/src/devices/ControlBoardWrapper/SubDevice.h @@ -117,26 +117,6 @@ class yarp::dev::impl::SubDevice bool configure(int wbase, int wtop, int base, int top, int axes, const std::string &id, yarp::dev::ControlBoardWrapper *_parent); - inline void refreshJointEncoders() - { - for(int j=base, idx=0; j<(base+axes); j++, idx++) - { - if(iJntEnc) - iJntEnc->getEncoderTimed(j, &subDev_joint_encoders[idx], &jointEncodersTimes[idx]); - } - } - - inline void refreshMotorEncoders() - { - for(int j=base, idx=0; j<(base+axes); j++, idx++) - { - if(iMotEnc) - iMotEnc->getMotorEncoderTimed(j, &subDev_motor_encoders[idx], &motorEncodersTimes[idx]); - } - } - - - bool isAttached() { return attachedF; } diff --git a/src/libYARP_dev/src/devices/TestMotor/TestMotor.h b/src/libYARP_dev/src/devices/TestMotor/TestMotor.h index f6d8514aba0..0d3897caa34 100644 --- a/src/libYARP_dev/src/devices/TestMotor/TestMotor.h +++ b/src/libYARP_dev/src/devices/TestMotor/TestMotor.h @@ -329,9 +329,11 @@ class YARP_dev_API yarp::dev::TestMotor : public DeviceDriver, bool getEncodersTimed(double *encs, double *time) override { - bool ret = getEncoders(encs); - *time = yarp::os::Time::now(); - return ret; + for (int i=0; i