Drop hardcoded/arbitrary delays in SDO reads

[PeterBowman]

Current CAN bus communications rely on blocking read operations with a hardcoded 1 ms delay. Following the schema of CanBusControlboard::ThreadImpl, the read loop behaves like this:

while (isRunning)
{
    // start listening to incoming traffic with a timeout of 'delay' milliseconds
    bool ok = read_timeout(msg_buffer, delay);

    if (!ok)
    {
        // error, timeout or EOF, loop again
        continue;
    }

    // do something with the received data
    interpretMessage(msg_buffer);
}

On the CAN node side, requests for data retrieval apply the same time value for delays while read operations loop constantly on another thread, as shown above (hopefully expecting that the next incoming message would refer to the exact same information previously requested):

yarp-devices/libraries/YarpPlugins/TechnosoftIpos/IPositionControl2RawImpl.cpp

Lines 283 to 296 in f2f659e

	if( ! send( 0x600, 8, msg_posmode_speed) )
	{
	CD_ERROR("Could not send \"posmode_speed\" query. %s\n", msgToStr(0x600, 8, msg_posmode_speed).c_str() );
	return false;
	}
	CD_SUCCESS("Sent \"posmode_speed\" query. %s\n", msgToStr(0x600, 8, msg_posmode_speed).c_str() );
	//* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
	yarp::os::Time::delay(DELAY); //-- Wait for read update. Could implement semaphore waiting for specific message...
	//* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
	refSpeedSemaphore.wait();
	*ref = refSpeed;
	refSpeedSemaphore.post();

Another approach was taken in icub-main's sharedCan device. It employs a pair of closely cooperating semaphores (look for waitReadMutex and synchroMutex), which are mutually exclusive (one of them initially set to zero), and non-blocking read calls. Seems cleaner, perhaps even more efficient.

In addition, advantage could be taken of the new message buffering feature coming soon in #122. For the time being, these buffers will start with a size of 1.

[PeterBowman]

Interesting lecture: https://stackoverflow.com/q/62814.

Edit 1: all semaphores used across our code are of the binary type (yarp::os::Semaphore). Perhaps most or even all of them should be turned into mutually exclusive (yarp::os::Mutex). Let's review them.

Edit 2: the exclusion system proposed here should be actually of the binary type (yarp::os::Semaphore).

[jgvictores]

1. From what I've read in the past, yarp::os::Mutex is essentially a bool, so you can only involve two actors. yarp::os::Semaphore is more like an int so you can have a full queue of actors.

2. yarp::os::Time::delay(DELAY); //-- Wait for read update. Could implement semaphore waiting for specific message... is a bit complex to implement. At least as proposed, we'd almost need one semaphore/mutex ...for each type of message!

[PeterBowman]

Another approach was taken in icub-main's sharedCan device. It employs a pair of closely cooperating semaphores (look for waitReadMutex and synchroMutex), which are mutually exclusive (one of them initially set to zero), and non-blocking read calls.

For future reference, the dual semaphore construct is only used in two places: ref1, ref2.

[PeterBowman]

Perhaps this SO answer is closer to my original intent: https://stackoverflow.com/a/47614705.

[PeterBowman]

We had to increase tenfold the hardcoded 1 ms delay in order to correctly fetch the actual motion status from the iPOS:

yarp-devices/libraries/YarpPlugins/TechnosoftIpos/IPositionControl2RawImpl.cpp

Line 158 in 9e7c309

yarp::os::Time::delay(DELAY); //-- Wait for read update. Could implement semaphore waiting for specific message...

The default value was causing trouble in our teo-self-presentation demo due to IPositionControl::checkMotionDone not updating an internal flag with the actual motion status fetched from the driver. This resulted in reporting a false state, either by finishing the motion sequence too early or by getting stuck in a "target not reached" mode.

[PeterBowman]

Idea: TPDOs can be configured to emit messages at fixed intervals, and send them with higher frequencies in case the drive state which is being watched changes; see 3.6, TxPDOs mapping example (asynchronous transmission) and 10.3, Cyclic Synchronous Position Mode example (synchronous).

[PeterBowman]

In addition, advantage could be taken of the new message buffering feature coming soon in #122. For the time being, these buffers will start with a size of 1.

WIP at #209.

Idea: TPDOs can be configured to emit messages at fixed intervals, and send them with higher frequencies in case the drive state which is being watched changes; see 3.6, TxPDOs mapping example (asynchronous transmission) and 10.3, Cyclic Synchronous Position Mode example (synchronous).

To be discussed at #223.

[PeterBowman]

Idea: implement a map of binary semaphores (yarp::os::Semaphore) with timeout (waitWithTimeout()), instantiate a new semaphore on every SDO request (value), associate it to a SDO dictionary object index (key), release the semaphore either in the read thread upon receiving the acknowledge/response or when timeout expires.

PS see this SO answer regarding the differences between a mutually exclusion semaphore (yarp::os::Mutex) and a binary semaphore (yarp::os::Semaphore): https://stackoverflow.com/a/86021.

[PeterBowman]

Ready at fec00cf. All SDO read requests now poll an available semaphore and block until the response arrives or a timeout occurs. Additionally, most SDO writes await an acknowledge signal from the driver.

[PeterBowman]

All SDO read requests now poll an available semaphore

Assuming only one active connection can be handled by a single SDO channel, there is no need to poll anything. Commit 29b3e1e simplified things a bit.

[PeterBowman]

This same semaphore-wait-with-timeout thingy proved useful for non-SDO thingies. Look for a StateObserver.hpp file in 972f875. Now, it is possible to signal an event to a observer in a more generic manner: no context data, context data is an integer-type (be it an array of std::uint8_ts or non-byte-length types), or context data is a class type.

[PeterBowman]

Assuming only one active connection can be handled by a single SDO channel, there is no need to poll anything.

Issue #160 allowed simultaneous SDO requests to occur. Commit fd5dd7d guards against this and reports "overrun" errors whenever the current SDO client's observer registers a response/confirmation message a different SDO client was supposed to receive from a previously issued request/indication. The SDO handshake has been made foolproof with a triple check: object index and object subindex must match, and command specifier bits must agree with the expected upload/download protocol.