Skip to content

Control board wrapper improvement2 #3

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

Open
wants to merge 2 commits into
base: devel
Choose a base branch
from
Open

Control board wrapper improvement2 #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
af96848
ControlBoardWrapper2: corner case fix + optimization about encoder re…
Dec 6, 2018
ac03abe
TestMotor: fix getEncoderTimed implementation
Jun 6, 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
91 changes: 42 additions & 49 deletions src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#include <yarp/os/Log.h>
#include <yarp/os/LogStream.h>
#include <sstream>
#include <numeric>
#include <algorithm>

#include <cstring> // for memset function

Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);
Expand All @@ -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());
Expand All @@ -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++)
{
Expand Down
2 changes: 2 additions & 0 deletions src/libYARP_dev/src/devices/ControlBoardWrapper/ControlBoardWrapper.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -286,11 +286,13 @@ class yarp::dev::ControlBoardWrapper: public yarp::dev::DeviceDriver,

bool checkPortName(yarp::os::Searchable &params);

yarp::rosmsg::sensor_msgs::JointState ros_struct;

yarp::os::BufferedPort<yarp::sig::Vector> outputPositionStatePort; // Port /state:o streaming out the encoder positions
yarp::os::BufferedPort<CommandMessage> 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
Expand Down
20 changes: 0 additions & 20 deletions src/libYARP_dev/src/devices/ControlBoardWrapper/SubDevice.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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; }

Expand Down
8 changes: 5 additions & 3 deletions src/libYARP_dev/src/devices/TestMotor/TestMotor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<njoints; i++)
{
getEncoderTimed(i, &encs[i], &time[i]);
}
return true;
}

bool getEncoderSpeed(int j, double *sp) override {
Expand Down