remote_controlboard: Add timeout check in all data streamed by StateExt:o port

[PeterBowman]

Describe the bug
The remote_controlboard client device connects to an external /<prefix>/stateExt:o port during its initialization, making it able to query robot status as streamed by a controlboardwrapper2 device. However, it doesn't wait for the first messages to arrive, thus returning zeroes on the first request right after the device is created (e.g. with the usual PolyDriver idiom). This is both incorrect and potentially dangerous since the corresponding methods (e.g. getEncoders) will return true and fill the output array with values that do not correspond to the actual robot state.

To Reproduce
Launch a custom controlboardwrapper2 implementation and move any of the robot joints out of the 0.0 position. Then, instantiate a remote_controlboard, pass the port names and ask for the encoder reads right away (i.e. no delays, no other instructions nor remote calls). Sample Python code:

options = yarp.Property()
options.put('device', 'remote_controlboard')
options.put('remote', '/remotePort')
options.put('local', '/localPort')
dd = yarp.PolyDriver(options)
enc = dd.viewIEncoders()
axes = enc.getAxes()
encoderValues = yarp.DVector(axes)
#time.sleep(DELAY)
enc.getEncoders(encoderValues)

encoderValues is filled if zeroes unless a small delay is applied right before the final call.

Screenshots
@triccyx posted a nice screenshot at #1822 that illustrates the problem:

The joint starts at 10º, but remote_controlboard reports 0º.

Configuration (please complete the following information):

• OS: Ubuntu 16.04
• yarp version: 2.3.72.1
• compiler: gcc

Additional context
I wondered if something similar to the isLive() helper check could be of use here:

yarp/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp

Lines 189 to 199 in 5aa3f58

	// Check for number of joints, if needed.
	// This is to allow for delayed connection to the remote control board.
	bool isLive() {
	if (!njIsKnown) {
	bool ok = get1V1I(VOCAB_AXES, nj);
	if (nj!=0 && ok) {
	njIsKnown = true;
	}
	}
	return njIsKnown;
	}

[PeterBowman]

By the way, I'm not sure isLive() is still working as expected. If I'm correct, it allows (or allowed) remote_controlboard to be opened with no remote port assigned at the time the device was created:

yarp/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp

Lines 1142 to 1151 in 5aa3f58

	if (!isLive()) {
	if (remote!="") {
	yError("Problems with obtaining the number of controlled axes\n");
	command_buffer.detach();
	rpc_p.close();
	command_p.close();
	extendedIntputStatePort.close();
	return false;
	}
	}

However, this previous check seems to defeat that purpose:

yarp/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp

Lines 1043 to 1046 in 5aa3f58

	if (remote=="") {
	yError("Problem connecting to remote controlboard, 'remote' port name not given\n");
	return false;
	}

And it wasn't there at the time isLive() was introduced, see open() at f54cec4.

[traversaro]

Is this a regression?

getEncoders has returned false until the first measurement was received in the streaming port, and this was exploited in code such as https://github.com/robotology/icub-tutorials/blob/c91976a32ddb59bcb10a2a56e9da24f129d2bfa3/src/motorControlBasic/tutorial_arm.cpp#L87 for an example code.

I briefly checked the code, and I was expecting this functionality to be provided by a check on the timeout similar to

yarp/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp

Line 1544 in 5aa3f58

if ( (Time::now()-localArrivalTime) > TIMEOUT)

, but apparently no check of that kind is present in the getEncoders method. Furthermore, the now timestamp in the stateExtendedInput reader should not be initialized to now as done in

yarp/src/libYARP_dev/src/devices/RemoteControlBoard/stateExtendedReader.cpp

Line 41 in 5aa3f58

now=Time::now();

, but I guess this has been working for a long time due to the delay induced by opening the remote_controlboard ports.

[barbalberto]

I just made a test with the following C++ code:

int main(int argc, char *argv[])
{
    Network yarp;
    if (!yarp.checkNetwork())
        return -1;


    Property p;
    p.put("device", "remote_controlboard");
    p.put("local", "/localPort");
    p.put("remote", "/icubSim/left_arm");

    double val = -1000000;
    bool ret = false;
    IEncoders *enc;

    PolyDriver drv;
    drv.open(p);
    drv.view(enc);
    ret = enc->getEncoder(0, &val);

    yInfo() << "Ret " << ret << " val " << val;
    return 0;
}

Running it multiple times, only 2 cases happen:

1. The encoder value is correctly update:
   [INFO] Ret true val -24.9985
2. The getEncoder is called before the encoder value is updated. It returns false and the encoder value is not touched:
   [INFO] Ret false val -1e+06 --> I initialized the variable to this value, and it is not changed.

So you get zeros because the vector is initialized to zero and the controlboard does not change them, since it has no updated values. And this is correct.
Return value is correctly set to false.

getEncoder method is meant to by non bloking, and there is specific code to deal with the situation you describe.
Therefore I'm not able to reproduce the bug. The only possible problem is if the python binding returns a different value from the C code. Try to clear the build, recompile the bindings and test again.

[traversaro]

@barbalberto getEncoder seems to have the correct check in

yarp/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp

Line 1484 in 5aa3f58

if (ret && ( (Time::now()-localArrivalTime) > TIMEOUT) )

.
Can you check getEncoders with one parameters, that is the method used by @PeterBowman and surprisingly seems to be missing the correct check (see in

yarp/src/libYARP_dev/src/devices/RemoteControlBoard/RemoteControlBoard.cpp

Line 1520 in 5aa3f58

virtual bool getEncoders(double *encs) override {

).

[barbalberto]

I tested both and they work correctly, the timeout is not involved in this case.

The function extendedIntputStatePort.getXXX is already returning false, before checking the timeout.

[traversaro]

Ah, got it, the valid member.

[PeterBowman]

Thank you, @traversaro @barbalberto. I'm sorry, this is clearly a mistake from my part as our code actually did not perform a check for the return value of getEncoders (as I initially thought). This issue may be deemed invalid, please close if you think there is no actionable outcome from this (perhaps the isLive thing could merit more attention? #1833 (comment)).

I tested both and they work correctly, the timeout is not involved in this case.

I'm just wondering. Let's suppose that remote_controlboard has not received a streaming message through /stateExt:i in more than TIMEOUT seconds. Then, several methods (most notably getEncoder, getEncoderTimed, and getEncodersTimed, but also getMotorEncoder and so on) are told to invalidate previous (outdated) values and return false. I find it strange that getEncoders is missing that check.

[barbalberto]

3 getEncoderXXX function out of 4 have the timeout check and one is missing. That's probably a copy&paste regression. About the other methods, sometime it is present, sometimes isn't. I think some legacy code was not correctly updated.

I suggest to move the timeout check inside the getLastSingle and getLastVector function, so that every method reading data from streaming port will have it.

[Nicogene]

    /**
     * Read the position of all axes. This object receives encoders periodically
     * from a YARP port. You should check the return value of the function to
     * make sure that encoders have been received at least once and with the expected
     * rate.
     * @param encs pointer to the array that will contain the output
     * @return true/false on success/failure. Failure means encoders have not been received
     * from the server or that they are not being streamed with the expected rate.
     */
    virtual bool getEncoders(double *encs) override;

    /**
     * Read the istantaneous speed of an axis.
     * @param j axis number
     * @param sp pointer to storage for the output
     * @return true if successful, false ... otherwise.
     */
    virtual bool getEncoderSpeed(int j, double *sp) override;

    /**
     * Read the instantaneous speed of all axes.
     * @param spds pointer to storage for the output values
     * @return guess what? (true/false on success or failure).
     */
    virtual bool getEncoderSpeeds(double *spds) override;

    /**
     * Read the instantaneous acceleration of an axis.
     * @param j axis number
     * @param acc pointer to the array that will contain the output
     */

    virtual bool getEncoderAcceleration(int j, double *acc) override;

    /**
     * Read the istantaneous acceleration of all axes.
     * @param accs pointer to the array that will contain the output
     * @return true if all goes well, false if anything bad happens.
     */
    virtual bool getEncoderAccelerations(double *accs) override;

Are missing the timeout check, adding it in getLastSingle and getLastVector we will have in all these functions, is it what we want?
BTW +1000 to move this duplicated(or more n-plicated) code in those functions.

Another question, is there any relationship between this fix and #1822?