-
Notifications
You must be signed in to change notification settings - Fork 11
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.
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
Feature/icub device impl #16
Feature/icub device impl #16
Conversation
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This is the first round of review, I still have some doubts but let's start from here.
I changed the base branch to feature/cleanup
in order to have only your commits in this PR @Yeshasvitvs.
devices/ICub/src/ICub.cpp
Outdated
std::mutex mtx; | ||
|
||
using yarp::os::BufferedPort<yarp::os::Bottle>::onRead; | ||
virtual void onRead(yarp::os::Bottle& wrench) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Are you sure you cannot use a callback on yarp::sig::Vector
? The logic of the callback function would be easier than operating on a Bottle
.
|
||
wearable::TimeStamp ICub::getTimeStamp() const | ||
{ | ||
// Stamp count should be always zero |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I suspect that the access of this attribute should be protected by a mutex
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I am not certain of it.
It is not protected by mutex in XSensSuitDevice
https://github.com/robotology-playground/wearables/blob/039fb255fcdedafc1ff42660a197d8f01ad6cadf/devices/XsensSuit/src/XsensSuit.cpp#L1011
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Do not trust blindly XsensSuit
😅 Here the two cases are different, XsensSuit
gathers the timestamp from another class and there is no need to protect the read and the write. In this case, instead, the device populates the timeStamp
struct, and they might collide. See the comment about the writing.
// ========================= | ||
yarp::os::Stamp ICub::getLastInputStamp() | ||
{ | ||
// Stamp count should be always zero |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Same here (mutex)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I am not certain of it.
It is not protected by mutex in XSensSuitDevice
https://github.com/robotology-playground/wearables/blob/039fb255fcdedafc1ff42660a197d8f01ad6cadf/devices/XsensSuit/src/XsensSuit.cpp#L880
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Same as above, mutex is required
|
||
wearable::TimeStamp ICub::getTimeStamp() const | ||
{ | ||
// Stamp count should be always zero |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Do not trust blindly XsensSuit
😅 Here the two cases are different, XsensSuit
gathers the timestamp from another class and there is no need to protect the read and the write. In this case, instead, the device populates the timeStamp
struct, and they might collide. See the comment about the writing.
devices/ICub/src/ICub.cpp
Outdated
} | ||
} | ||
|
||
std::vector<double> getWrench() { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Probably I didn't explain it well. You don't need to use methods to access public class attributes. All-public classes are equivalent to structs.
You can access the wrenchValues
in the following way:
WrenchPort port;
port.wrenchValues = std::vector<double>(10, 0);
devices/ICub/src/ICub.cpp
Outdated
|
||
// Reading wrench from WBD ports | ||
if (this->m_name == icubImpl->wearableName + sensor::IForceTorque6DSensor::getPrefix() + "leftWBDFTSensor") { | ||
icubImpl->leftHandFTPort.useCallback(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think you should enable the callback only once, not every time you want to access data.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Done
icubImpl->leftHandFTPort.useCallback(); | ||
wrench = icubImpl->leftHandFTPort.wrench(); | ||
|
||
icubImpl->timeStamp.time = yarp::os::Time::now(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This is the only place where the timestamp is updated. Maybe this is not the proper location. However, we should have a f2f talk on how to deal with timestamps, the support is present in wearables but we don't have yet any guideline.
devices/ICub/src/ICub.cpp
Outdated
} | ||
else { | ||
while (pImpl->leftHandFTPort.firstRunFlag()) { | ||
pImpl->leftHandFTPort.useCallback(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
As wrote above, useCallback()
should be used only to enable the usage of the onRead()
method, not for calling it explicitly when is needed (it would not be a callback by definition in this case)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This explicit call to useCallback()
is to ensure that the data is read at least once and to set the firstRun
flag.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Removing it is throwing an error from IWearWrapper
[ERROR]IWearWrapper : The status of the IWear interface is not Ok 0 )
if (!icubImpl->leftHandFTPort.firstRunFlag() || !icubImpl->rightHandFTPort.firstRunFlag()) { | ||
auto nonConstThis = const_cast<ICubForceTorque6DSensor*>(this); | ||
nonConstThis->setStatus(wearable::sensor::SensorStatus::Ok); | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I don't understand this logic. I think this should not be here in the constructor. Let's talk f2f on how to improve the way the data is accessed from the wearable sensor and how to retrieve it using the buffered port.
My idea is that the sensors holds a reference to the wrenchValues
buffer and (protected by a mutex) provides this data to the user. The onRead()
callback writes new data into this buffer every time it arrives. The status should be initialized as WaitingForFirstRead in the constructor, and the onRead()
callback updates it to Ok
.
This approach allows to remove the hack in the wearable sensor implementation where it uses the sensor name for picking up the right data.
devices/ICub/src/ICub.cpp
Outdated
} | ||
|
||
while (pImpl->rightHandFTPort.firstRunFlag()) { | ||
pImpl->rightHandFTPort.useCallback(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
As wrote in another comment, this is not the way useCallback
should be used. This method only enables the callback to be called on the event of data received.
remove Yarp Init check in ICub
Rpc port optional
…eaction Add ground reaction FT flag handling
Implement skin sensors wearable device
fix backcompatibility with YARP master
Add use RPC parameter
Given that this PR has grown and it is no longer limited to In particular I have detected 7 possible PRs which group commits relative to single features (most likely independent one from the other, and touching different files):
As suggested by @diegoferigo, it would be also good to clean the commits in the PR. cc @Yeshasvitvs |
Fix Xsens Calibrator device
Accordingly to the previous comment, new PRs opened:
Concerning the |
This PR adds an initial IWear implementation of icub as ICub device. Currently, only the FTSensors from icub hands are exposed through wearable data as wrench sources.
@diegoferigo please check this PR