universal_robots_ros_driver/include/ur_modern_driver/ur/producer.h

#pragma once
#include "ur_modern_driver/pipeline.h"
#include "ur_modern_driver/ur/parser.h"
#include "ur_modern_driver/ur/stream.h"

template <typename T>
class URProducer : public IProducer<T> {
private:
    URStream& _stream;
    URParser<T>& _parser;

public:
    URProducer(URStream& stream, URParser<T>& parser)
        : _stream(stream)
        , _parser(parser)
    {
    }

    void setup_producer() { _stream.connect(); }
    void teardown_producer() { _stream.disconnect(); }
    void stop_producer() { _stream.disconnect(); }

    bool try_get(std::vector<unique_ptr<T> >& products)
    {
        //4KB should be enough to hold any packet received from UR
        uint8_t buf[4096];

        //blocking call
        ssize_t len = _stream.receive(buf, sizeof(buf));

        //LOG_DEBUG("Read %d bytes from stream", len);

        if (len == 0) {
            LOG_WARN("Read nothing from stream");
            return false;
        } else if (len < 0) {
            LOG_WARN("Stream closed");
            return false;
        }

        BinParser bp(buf, static_cast<size_t>(len));
        return _parser.parse(bp, products);
    }
};