Avoid message loss when dumping CAN traffic via YARP ports

[PeterBowman]

As observed in https://github.com/roboticslab-uc3m/yarp-devices/issues/160#issuecomment-577650352, the /dump:o port publishes at such a high rate that messages are dropped/lost when inspected either through yarp read companion command or the dumpCanBus app.

I have replicated this behavior in a reader&writer sample scenario:

cmake_minimum_required(VERSION 3.12)
project(yarp-buffered-port LANGUAGES CXX)
find_package(YARP 3.4 REQUIRED os)
add_executable(reader reader.cpp)
add_executable(writer writer.cpp)
target_link_libraries(reader YARP::YARP_os YARP::YARP_init)
target_link_libraries(writer YARP::YARP_os YARP::YARP_init)

// reader.cpp
#include <yarp/os/Bottle.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/LogStream.h>
#include <yarp/os/RFModule.h>
#include <yarp/os/SystemClock.h>

class Worker : public yarp::os::RFModule, yarp::os::TypedReaderCallback<yarp::os::Bottle>
{
public:
    bool configure(yarp::os::ResourceFinder & rf) override
    {
        delay = rf.check("delay", yarp::os::Value(0.01)).asFloat64();

        if (port.open("/reader"))
        {
            port.useCallback(*this);
            port.setStrict(rf.check("strict", yarp::os::Value(false)).asBool());
            return true;
        }

        return false;
    }

    bool close() override
    {
        port.interrupt();
        port.close();
        return true;
    }

    bool updateModule() override
    { return true; }

    void onRead(yarp::os::Bottle & b) override
    {
        yInfo() << b.toString();
        yarp::os::SystemClock::delaySystem(delay);
    }

private:
    yarp::os::BufferedPort<yarp::os::Bottle> port;
    double delay;
};

int main(int argc, char * argv[])
{
    yarp::os::ResourceFinder rf;
    rf.configure(argc, argv);
    Worker worker;
    return worker.runModule(rf);
}

// writer.cpp
#include <yarp/os/Bottle.h>
#include <yarp/os/BufferedPort.h>
#include <yarp/os/Network.h>
#include <yarp/os/Property.h>
#include <yarp/os/SystemClock.h>

int main(int argc, char * argv[])
{
    yarp::os::Network yarp;

    yarp::os::Property options;
    options.fromCommand(argc, argv);

    auto forceStrict = options.check("force", yarp::os::Value(false)).asBool();
    auto delay = options.check("delay", yarp::os::Value(0.1)).asFloat64();
    auto iters = options.check("iters", yarp::os::Value(10)).asInt32();

    yarp::os::BufferedPort<yarp::os::Bottle> port;
    port.open("/writer");

    yarp::os::Network::connect(port.getName(), "/reader");

    for (auto i = 0; i < iters; i++)
    {
        port.prepare() = {yarp::os::Value(i)};
        port.write(forceStrict);
        yarp::os::SystemClock::delaySystem(delay);
    }

    return 0;
}

Command-line arguments allow to tweak the number of messages sent in bursts (default: 10), the delay between messages (default: 0.1 seconds), whether the writer should wait for any writes to complete (default: false), and whether the reader should not drop old messages (default: false). Note the reader also emulates an intensive operation going on each message reception (default: 10 ms).

Per docs:

By default a BufferedPort attempts to reduce latency between senders and receivers. To do so messages may be dropped by the writer if BufferedPort::write is called too quickly The reader may also drop old messages if BufferedPort::read is not called fast enough, so that new messages can travel with high priority. This policy is sometimes called Oldest Packet Drop (ODP).

If your application cannot afford dropping messages you can change the buffering policy. Use BufferedPort::writeStrict() when writing to a port, this waits for pending transmissions to be finished before writing new data. Call BufferedPort::setStrict() to change the buffering policy to FIFO at the receiver side. In this way all messages will be stored inside the BufferedPort and delivered to the reader. Pay attention that in this case a slow reader may cause increasing latency and memory use.

Our problem is mostly solved with write(true) (or writeStrict()) and setStrict(true). I observe that messages are dropped whenever any of these is not properly set.

Ideas:

• Use writeStrict() in CanBusControlboard's CAN read/write threads.
• Use setStrict() in dumpCanBus. See warning: "if you can't read them as fast as they come in, watch out".
• Optimize. Instead of writing to the YARP network on each CAN message, build a bottle consisting of several messages and send them all at once (i.e. avoid calling write() multiple times: read loop, write loop). Consequently, dumpCanBus should be prepared to accept and decode multiple-list bottles.
• Use TCP instead of UDP in dumpCanBus (ref)?
• Test. In case writeStrict() degrades performance, consider alternatives (aggressive buffering? lower CAN read/write rates?) and/or conditionally disable dump port.

By the way, I could have used setWriteOnly() instead of setInputMode() here.

Also, CAN messages forwarded via YARP could have a timestamp envelope associated and printed on screen by dumpCanBus, if properly configured.

[PeterBowman]

Use TCP instead of UDP in dumpCanBus (ref)?

Better "fast_tcp" than "tcp", see Port and connection protocols, Configuring YARP Connections and https://github.com/robotology/community/discussions/258.

[PeterBowman]

Tested on the left arm. With syncPeriod equal to 50 ms, I usually get a ~15-16% CPU usage, with or without a dump port (regardless of being used or not). With the default 0.1 ms delay in RW threads, blocking writes on this port seem to have no effect. Disclaimer: node ID24 is dead, besides I am not using SYNCed RPDOs.

With syncPeriod equal to 2 ms, there is a difference. If the dump port is commented out, I get a ~24% CPU usage. This is ~5% higher after enabling the port, and almost +15% (~40%) with an active connection to a running dumpCanBus instance.

Results are similar on current master and the WIP feature branch associated to this issue.

PS there is little impact when dropping syncPeriod to 10-20 ms, it's still ~18% (and 20-21% on an active dumpCanBus connection at 10 ms).

[PeterBowman]

Done at https://github.com/roboticslab-uc3m/yarp-devices/commit/f324d110611751d28e6e0c2d4cc2862365ca278e. The dumpCanBus app has learned to accept a new switch, --with-ts, which prepends the timestamp in the console output. I'm not adding another option to disable the dump port (yet).

[PeterBowman]

See https://github.com/robotology/yarp/discussions/2533 regarding CPU usage on buffered ports with no active connection.