Understand why the getControlModes sometimes fails

[GiulioRomualdi]

I built the first application using the IRobotInterface when I run the application on my laptop the application sometimes fails here: https://github.com/dic-iit/bipedal-locomotion-framework/blob/c055c48edb86aa9f23fb7cc572a4ea005f1c626b/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp#L131-L135

Back in time we solved the problem using a timer and checking several times if the yarp interface were able to collect the data. See here.

I would avoid implementing the same trick also here. (Probably we have the same problem with the ISensorBridge implemented by @prashanthr05)

Furthermore the fact that this behavior is completely random bothers me a lot.

Is there someone able to explain to me why this is happening? @traversaro @S-Dafarra @prashanthr05

[GiulioRomualdi]

This problem convinced me that https://github.com/dic-iit/bipedal-locomotion-framework/issues/111 is really important

[prashanthr05]

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.

[GiulioRomualdi]

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.

[traversaro]

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 .

[GiulioRomualdi]

Thank you @traversaro, so accordingly on what you wrote in https://github.com/dic-iit/bipedal-locomotion-framework/issues/115#issuecomment-695189340, 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.

[traversaro]

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 .

[GiulioRomualdi]

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