Completed parsing of UR state, messages and RT

include/ur_modern_driver/ur/producer.h

@@ -1,21 +1,44 @@
 #pragma once
 #include "ur_modern_driver/pipeline.h"
 #include "ur_modern_driver/ur/stream.h"
-#include "ur_modern_driver/packet.h"
-#include "ur_modern_driver/parser.h"
+#include "ur_modern_driver/ur/parser.h"
 
-class URProducer : public IProducer<Packet> {
+template <typename T>
+class URProducer : public IProducer<T> {
 private:
     URStream &_stream;
-    Parser &_parser;
+    URParser<T> &_parser;
 
 public:
-    URProducer(URStream &stream, Parser &parser) 
+    URProducer(URStream &stream, URParser<T> &parser) 
         : _stream(stream), 
         _parser(parser) { }
     
-    void setup_producer();
-    void teardown_producer();
-    void stop_producer();
-    std::unique_ptr<Packet> try_get();
+    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 < 1) {
+            LOG_WARN("Read nothing from stream");
+            return false;
+        }
+
+        BinParser bp(buf, static_cast<size_t>(len));
+        return _parser.parse(bp, products);
+    }
 };