Understand why the getControlModes sometimes fails about bipedal-locomotion-framework HOT 7 CLOSED

The ControlBoardRemapper calls the getControlModes() to which it is attached, and in the case of the remotecontrolboardremapper the internal device is being attached to the remote_controlboard in https://github.com/robotology/yarp/blob/9478e6a2aa627b4bc0c4bca6337f8e559c95bafb/src/devices/ControlBoardRemapper/RemoteControlBoardRemapper.cpp#L148 .
For the remote_controlboard, the implementation of that method is in https://github.com/robotology/yarp/blob/9478e6a2aa627b4bc0c4bca6337f8e559c95bafb/src/devices/RemoteControlBoard/RemoteControlBoard.cpp#L2262, and you can see that it returns false only when StateExtendedInputPort::getLastVector return false, and as you can see from https://github.com/robotology/yarp/blob/9478e6a2aa627b4bc0c4bca6337f8e559c95bafb/src/devices/RemoteControlBoard/stateExtendedReader.cpp#L204 this happens when the first message from the stateExt:o port still needs to be received, so this is quite expected if you just opened the device and you right away call the getControlModes method. If you prefer this "wait" is done in blocking mode during the configuration, then I think you can add a waiting loop in the configuration part of the code, as done for example in the WB-Toolbox's RobotInterface class (that indeed shares several aspects with the equivalent classes in BLF) : https://github.com/robotology/wb-toolbox/blob/4278d56dc16e4a76e0b4ef6b872bc3e26c43e451/toolbox/base/src/RobotInterface.cpp#L83 .

from bipedal-locomotion-framework.

The second (or any successive) call returns the latest received data, unless a timeout has passed. You can verify it yourself from https://github.com/robotology/yarp/blob/9478e6a2aa627b4bc0c4bca6337f8e559c95bafb/src/devices/RemoteControlBoard/RemoteControlBoard.cpp#L1375 and https://github.com/robotology/yarp/blob/9478e6a2aa627b4bc0c4bca6337f8e559c95bafb/src/devices/RemoteControlBoard/stateExtendedReader.cpp#L262 .

Really really thank you @traversaro

I think that this may interest all the YARP user in @dic-iit/dynamic-interaction-control

from bipedal-locomotion-framework.

In other words, does the second call to getEncoders always return the current joint values (even if are the same joints1), or it return false?

The second (or any successive) call returns the latest received data, unless a timeout has passed. You can verify it yourself from https://github.com/robotology/yarp/blob/9478e6a2aa627b4bc0c4bca6337f8e559c95bafb/src/devices/RemoteControlBoard/RemoteControlBoard.cpp#L1375 and https://github.com/robotology/yarp/blob/9478e6a2aa627b4bc0c4bca6337f8e559c95bafb/src/devices/RemoteControlBoard/stateExtendedReader.cpp#L262 .

from bipedal-locomotion-framework.

This problem convinced me that #111 is really important

from bipedal-locomotion-framework.

Just speculating here, if it's related to the mutex implementations in the underlying ControlBoardRemapper implementations? Could you trace the code to also paste the underlying implementation of getControlModes() here?

As we discussed f2f earlier, I have never faced this before actually? Or maybe I believe so.

from bipedal-locomotion-framework.

The method called in this case is ControlBoardRemapper::getControlModes(int *modes)

bool ControlBoardRemapper::getControlModes(int *modes)
{
    bool ret=true;
    std::lock_guard<std::mutex> lock(allJointsBuffers.mutex);

    for(size_t ctrlBrd=0; ctrlBrd < remappedControlBoards.getNrOfSubControlBoards(); ctrlBrd++)
    {
        RemappedSubControlBoard *p=remappedControlBoards.getSubControlBoard(ctrlBrd);

        bool ok;

        if( p->iMode )
        {
            ok = p->iMode->getControlModes(allJointsBuffers.m_nJointsInSubControlBoard[ctrlBrd],
                                            allJointsBuffers.m_jointsInSubControlBoard[ctrlBrd].data(),
                                            allJointsBuffers.m_bufferForSubControlBoardControlModes[ctrlBrd].data());
        }
        else
        {
            ok = false;
        }

        ret = ret && ok;
    }

    allJointsBuffers.fillCompleteJointVectorFromSubControlBoardBuffers(modes,remappedControlBoards);

    return ret;
}

here it may fail only if p->iMode == nullptr or p->iMode->getControlModes() returns false.

From this point, I'm a bit lost since I don't know which getControlModes() is called.

from bipedal-locomotion-framework.

Thank you @traversaro, so accordingly on what you wrote in #115 (comment), the get methods of the interfaces reytrns false only if the first message is not arrived.

I'm asking this beacouse I was wandering if this code may work or not.
Suppose we have a pointer to the IEncodersTimed and I try to access the joint encoders values as:

std::vector<double> joints1(numberOfJoints);
iEncoderInterface->getEncoders(joints1.data());

// some code......
std::vector<double> joints2(numberOfJoints);
iEncoderInterface->getEncoders(joints2.data());

In other words, does the second call to getEncoders always return the current joint values (even if are the same joints1), or it return false?

I'm asking this beacouse in our infrastructure we currently have the IRobotControl interface that reads the encoders every time a position direct command is sent
https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp#L196

And on the other hand, we have the ISensorBridge interface that is in charge of expose all the signal generated by the robot (e.g. the joint encoders values).
https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h#L1407

So in both IRobotControl and ISensorBridge, there is a call to the getEncoders() method of the IEncoder generated from the exactly same polydriver.

from bipedal-locomotion-framework.