Implemented URStateParser, URRTStateParser and URProducer

include/ur_modern_driver/parser.h

@@ -0,0 +1,8 @@
+#pragma once
+#include "ur_modern_driver/bin_parser.h"
+#include "ur_modern_driver/packet.h"
+
+class Parser {
+public:
+    virtual std::unique_ptr<Packet> parse(BinParser &bp) = 0;
+};

include/ur_modern_driver/ur/parser.h

@@ -0,0 +1,52 @@
+#pragma once
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/parser.h"
+#include "ur_modern_driver/ur/state.h"
+
+template <typename T>
+class URStateParser : public Parser {
+    std::unique_ptr<Packet> parse(BinParser &bp)  {
+        int32_t packet_size = bp.peek<int32_t>();
+        message_type type;
+
+        if(!bp.check_size(packet_size)) {
+            LOG_ERROR("Buffer len shorter than expected packet length\n");
+            return std::unique_ptr<Packet>(nullptr);        
+        }
+
+        bp.parse(packet_size); //consumes the peeked data
+        bp.parse(type);
+
+        if(type != message_type::ROBOT_STATE) {
+            LOG_ERROR("Invalid message type recieved: %u\n", static_cast<uint8_t>(type));
+            return std::unique_ptr<Packet>(nullptr);
+        }
+
+        std::unique_ptr<Packet> obj(new T);
+        if(obj->parse_with(bp))
+            return obj;
+        
+        return std::unique_ptr<Packet>(nullptr);
+    }
+};
+
+
+template <typename T>
+class URRTStateParser : public Parser {
+    std::unique_ptr<Packet> parse(BinParser &bp)  {
+        int32_t packet_size = bp.peek<int32_t>();
+
+        if(!bp.check_size(packet_size)) {
+            LOG_ERROR("Buffer len shorter than expected packet length\n");
+            return std::unique_ptr<Packet>(nullptr);        
+        }
+
+        bp.parse(packet_size); //consumes the peeked data
+
+        std::unique_ptr<Packet> obj(new T);
+        if(obj->parse_with(bp))
+            return obj;
+        
+        return std::unique_ptr<Packet>(nullptr);
+    }
+};

include/ur_modern_driver/ur/producer.h

@@ -0,0 +1,21 @@
+#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"
+
+class URProducer : public IProducer<Packet> {
+private:
+    URStream &_stream;
+    Parser &_parser;
+
+public:
+    URProducer(URStream &stream, Parser &parser) 
+        : _stream(stream), 
+        _parser(parser) { }
+    
+    void setup_producer();
+    void teardown_producer();
+    void stop_producer();
+    std::unique_ptr<Packet> try_get();
+};

src/ur/producer.cpp

@@ -0,0 +1,54 @@
+#include "ur_modern_driver/ur/producer.h"
+#include "ur_modern_driver/log.h"
+
+void URProducer::setup_producer() {
+    _stream.connect();
+}
+
+void URProducer::teardown_producer() {
+
+}
+
+void URProducer::stop_producer() {
+    _stream.disconnect();
+}
+
+std::unique_ptr<Packet> URProducer::try_get() {
+    //4KB should be enough to hold any packet received from UR
+    uint8_t buf[4096];
+    
+    ssize_t total = 0;
+    int32_t packet_size = 0;
+
+    //deal with partial recieves 
+    while(total <= sizeof(buf)) {
+        uint8_t *pos = buf + total;
+        size_t size = sizeof(buf) - total;
+
+        //blocking call
+        ssize_t len = _stream.receive(pos, size);
+        
+        if(len < 1) {
+            LOG_DEBUG("Read nothing from stream\n");
+            return std::unique_ptr<Packet>(nullptr);
+        }
+
+        total += len;
+        BinParser bp(buf, static_cast<size_t>(total));
+
+        if(packet_size == 0) {
+            packet_size = bp.peek<int32_t>();
+            //TODO: check other wrong packet sizes?
+            if(packet_size > sizeof(buf)) {
+                LOG_ERROR("A packet with 'len' larger than buffer was received, discarding...\n");
+                return std::unique_ptr<Packet>(nullptr);
+            }
+        }
+
+        if(total < packet_size){
+            LOG_DEBUG("Partial packet recieved\n");
+            continue;
+        }
+        return std::move(_parser.parse(bp));
+    }
+}