Completed parsing of UR state, messages and RT

include/ur_modern_driver/pipeline.h

@@ -2,6 +2,8 @@
 
 #include <thread>
 #include <atomic>
+#include <vector>
+#include "ur_modern_driver/log.h"
 #include "ur_modern_driver/queue/readerwriterqueue.h"
 
 using namespace moodycamel; 
@@ -9,23 +11,26 @@ using namespace std;
 
 
 template <typename T>
-class IProducer {
+class IConsumer {
 public:
-    virtual void setup_producer() = 0;
-    virtual void teardown_producer() = 0;
-    virtual void stop_producer() = 0;
-    virtual unique_ptr<T> try_get() = 0;
+    virtual void setup_consumer() { }
+    virtual void teardown_consumer() { }
+    virtual void stop_consumer() { }
+
+    virtual bool consume(unique_ptr<T> product) = 0;
 };
 
 template <typename T>
-class IConsumer {
+class IProducer {
 public:
-    virtual void setup_consumer() = 0;
-    virtual void teardown_consumer() = 0;
-    virtual void stop_consumer() = 0;
-    virtual bool push(unique_ptr<T> product) = 0;
+    virtual void setup_producer() { }
+    virtual void teardown_producer() { }
+    virtual void stop_producer() { }
+
+    virtual bool try_get(std::vector<unique_ptr<T>> &products) = 0;
 };
 
+
 template <typename T>
 class Pipeline {
 private:
@@ -37,15 +42,19 @@ private:
 
     void run_producer() {
         _producer.setup_producer();
+        std::vector<unique_ptr<T>> products;
         while(_running) {
-            unique_ptr<T> product(_producer.try_get());
-            
-            if(product == nullptr)
+            if(!_producer.try_get(products)) {
                 break;
-
-            if(!_queue.try_enqueue(std::move(product))) {
-                //log dropped product
             }
+            
+            for(auto &p : products) {
+                if(!_queue.try_enqueue(std::move(p))) {
+                    LOG_WARN("Pipeline owerflowed!");
+                }
+            }
+
+            products.clear();
         }
         _producer.teardown_producer();
         //todo cleanup
@@ -53,10 +62,10 @@ private:
 
     void run_consumer() {
         _consumer.setup_consumer();
+        unique_ptr<T> product;
         while(_running) {
-            unique_ptr<T> product;
             _queue.wait_dequeue(product);
-            if(!_consumer.push(std::move(product))) 
+            if(!_consumer.consume(std::move(product))) 
                 break;
         }
         _consumer.teardown_consumer();
@@ -86,6 +95,8 @@ public:
         _consumer.stop_consumer();
         _producer.stop_producer();
 
+        _running = false;
+
         _pThread.join();
         _cThread.join();
     }

include/ur_modern_driver/ur/consumer.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#include "ur_modern_driver/pipeline.h"
+#include "ur_modern_driver/ur/state.h"
+#include "ur_modern_driver/ur/master_board.h"
+#include "ur_modern_driver/ur/robot_mode.h"
+#include "ur_modern_driver/ur/rt_state.h"
+#include "ur_modern_driver/ur/messages.h"
+
+class URRTPacketConsumer : public IConsumer<RTPacket> {
+public:
+    virtual bool consume(unique_ptr<RTPacket> packet) {
+        return packet->consume_with(*this);
+    }
+
+    virtual bool consume(RTState_V1_6__7 &state) = 0;
+    virtual bool consume(RTState_V1_8 &state) = 0;
+    virtual bool consume(RTState_V3_0__1 &state) = 0;
+    virtual bool consume(RTState_V3_2__3 &state) = 0;
+};
+
+
+class URStatePacketConsumer : public IConsumer<StatePacket> {
+public:
+    virtual bool consume(unique_ptr<StatePacket> packet) {
+        return packet->consume_with(*this);
+    }
+
+    virtual bool consume(MasterBoardData_V1_X &data) = 0;
+    virtual bool consume(MasterBoardData_V3_0__1 &data) = 0;
+    virtual bool consume(MasterBoardData_V3_2 &data) = 0;
+    
+    virtual bool consume(RobotModeData_V1_X &data) = 0;
+    virtual bool consume(RobotModeData_V3_0__1 &data) = 0;
+    virtual bool consume(RobotModeData_V3_2 &data) = 0;  
+};
+
+
+class URMessagePacketConsumer : public IConsumer<MessagePacket> {
+public:
+    virtual bool consume(unique_ptr<MessagePacket> packet) {
+        return packet->consume_with(*this);
+    }
+
+    virtual bool consume(VersionMessage &message) = 0;
+};

include/ur_modern_driver/ur/factory.h

@@ -0,0 +1,85 @@
+#pragma once
+
+#include <cstdlib>
+#include "ur_modern_driver/ur/stream.h"
+#include "ur_modern_driver/ur/consumer.h"
+#include "ur_modern_driver/ur/producer.h"
+#include "ur_modern_driver/ur/parser.h"
+#include "ur_modern_driver/ur/state_parser.h"
+#include "ur_modern_driver/ur/rt_parser.h"
+#include "ur_modern_driver/ur/messages_parser.h"
+
+
+class URFactory : private URMessagePacketConsumer {
+private:
+    URStream _stream;
+    URMessageParser _parser;
+
+    uint8_t _major_version;
+    uint8_t _minor_version;
+
+    bool consume(VersionMessage &vm) {
+        LOG_INFO("Got VersionMessage:");
+        LOG_INFO("project name: %s", vm.project_name.c_str());
+        LOG_INFO("version: %u.%u.%d", vm.major_version, vm.minor_version, vm.svn_version);
+        LOG_INFO("build date: %s", vm.build_date.c_str());
+
+        _major_version = vm.major_version;
+        _minor_version = vm.minor_version;
+        
+        return true;
+    }
+
+    void setup_consumer() { }
+    void teardown_consumer() { }
+    void stop_consumer() { }
+
+public:
+    URFactory(std::string &host) : _stream(host, 30001) {
+        URProducer<MessagePacket> p(_stream, _parser);
+        std::vector<unique_ptr<MessagePacket>> results;
+
+        p.setup_producer();
+
+        if(!p.try_get(results) || results.size() == 0) {
+            LOG_FATAL("No version message received, init failed!");            
+            std::exit(EXIT_FAILURE);
+        }
+        
+        for(auto const& p : results) {
+            p->consume_with(*this);
+        }
+
+        if(_major_version == 0 && _minor_version == 0) {
+            LOG_FATAL("No version message received, init failed!");
+            std::exit(EXIT_FAILURE);
+        }
+
+        p.teardown_producer();
+    }
+
+    std::unique_ptr<URParser<StatePacket>> get_state_parser() {
+        if(_major_version == 1) {
+            return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V1_X);
+        } else {
+            if(_minor_version < 3)
+                return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_0__1); 
+            else
+                return std::unique_ptr<URParser<StatePacket>>(new URStateParser_V3_2);
+        }
+    }
+
+    std::unique_ptr<URParser<RTPacket>> get_rt_parser() {
+        if(_major_version == 1) {
+            if(_minor_version < 8)
+                return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_6__7);
+            else
+                return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V1_8);
+        } else {
+            if(_minor_version < 3)
+                return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_0__1); 
+            else 
+                return std::unique_ptr<URParser<RTPacket>>(new URRTStateParser_V3_2__3);
+        }
+    }
+};

include/ur_modern_driver/ur/master_board.h

@@ -4,6 +4,7 @@
 #include <inttypes.h>
 #include "ur_modern_driver/types.h"
 #include "ur_modern_driver/bin_parser.h"
+#include "ur_modern_driver/ur/state.h"
 
 
 class SharedMasterBoardData {
@@ -37,9 +38,10 @@ public:
     static const size_t EURO_SIZE = sizeof(int32_t) * 2;
 };
 
-class MasterBoardData_V1_X : public SharedMasterBoardData {
+class MasterBoardData_V1_X : public SharedMasterBoardData, public StatePacket {
 public:
     virtual bool parse_with(BinParser &bp);
+    virtual bool consume_with(URStatePacketConsumer &consumer);
 
     int16_t digital_input_bits;
     int16_t digital_output_bits;
@@ -60,9 +62,10 @@ public:
         + sizeof(int16_t) * 2;
 };
 
-class MasterBoardData_V3_0__1 : public SharedMasterBoardData {
+class MasterBoardData_V3_0__1 : public SharedMasterBoardData, public StatePacket {
 public:
     virtual bool parse_with(BinParser &bp);
+    virtual bool consume_with(URStatePacketConsumer &consumer);
 
     int32_t digital_input_bits;
     int32_t digital_output_bits;
@@ -87,6 +90,7 @@ public:
 class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 {
 public:
     virtual bool parse_with(BinParser &bp);
+    virtual bool consume_with(URStatePacketConsumer &consumer);
 
     uint8_t operational_mode_selector_input;
     uint8_t three_position_enabling_device_input;

include/ur_modern_driver/ur/messages.h

@@ -1,7 +1,10 @@
 #pragma once
+
 #include <cstddef>
 #include <inttypes.h>
-#include "ur_modern_driver/packet.h"
+#include "ur_modern_driver/pipeline.h"
+#include "ur_modern_driver/bin_parser.h"
+
 
 enum class robot_message_type : uint8_t {
 	ROBOT_MESSAGE_TEXT = 0,
@@ -15,17 +18,24 @@ enum class robot_message_type : uint8_t {
 	ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
 };
 
-class MessageBase : public Packet {
+class URMessagePacketConsumer;
+
+class MessagePacket {
 public:
+    MessagePacket(uint64_t timestamp, uint8_t source) : timestamp(timestamp), source(source) { }
     virtual bool parse_with(BinParser &bp) = 0;
+    virtual bool consume_with(URMessagePacketConsumer &consumer) = 0;
 
     uint64_t timestamp;
     uint8_t source;
 };
 
-class VersionMessage : public MessageBase {
+class VersionMessage : public MessagePacket {
 public:
-    bool parse_with(BinParser &bp);
+    VersionMessage(uint64_t timestamp, uint8_t source) : MessagePacket(timestamp, source) { }
+    
+    virtual bool parse_with(BinParser &bp);
+    virtual bool consume_with(URMessagePacketConsumer &consumer);
 
     std::string project_name;
     uint8_t major_version;

include/ur_modern_driver/ur/messages_parser.h

@@ -0,0 +1,48 @@
+#pragma once
+#include <vector>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/bin_parser.h"
+#include "ur_modern_driver/pipeline.h"
+#include "ur_modern_driver/ur/parser.h"
+#include "ur_modern_driver/ur/messages.h"
+
+class URMessageParser : public URParser<MessagePacket> {
+public:
+    bool parse(BinParser &bp, std::vector<unique_ptr<MessagePacket>> &results) {
+        int32_t packet_size;
+        message_type type;
+        bp.parse(packet_size);
+        bp.parse(type);
+
+        if(type != message_type::ROBOT_MESSAGE) {
+            LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
+            return false;
+        }
+
+        uint64_t timestamp;
+        uint8_t source;
+        robot_message_type message_type;
+
+        bp.parse(timestamp);
+        bp.parse(source);
+        bp.parse(message_type);
+
+        std::unique_ptr<MessagePacket> result;
+        bool parsed = false;
+
+        switch(message_type) {
+            case robot_message_type::ROBOT_MESSAGE_VERSION: {
+                VersionMessage *vm = new VersionMessage(timestamp, source);
+                parsed = vm->parse_with(bp);
+                result.reset(vm);
+                break;
+            }
+
+            default:
+                return false;
+        }
+
+        results.push_back(std::move(result));
+        return parsed;
+    }
+};

include/ur_modern_driver/ur/parser.h

@@ -1,88 +1,10 @@
 #pragma once
-#include "ur_modern_driver/log.h"
-#include "ur_modern_driver/parser.h"
-#include "ur_modern_driver/ur/state.h"
-#include "ur_modern_driver/ur/rt_state.h"
-#include "ur_modern_driver/ur/messages.h"
+#include <vector>
+#include "ur_modern_driver/pipeline.h"
+#include "ur_modern_driver/bin_parser.h"
 
 template <typename T>
-class URStateParser : public Parser {
-    std::unique_ptr<Packet> parse(BinParser &bp) {
-        int32_t packet_size;
-        message_type type;
-        bp.parse(packet_size);
-        bp.parse(type);
-
-        if(type != message_type::ROBOT_STATE) {
-            LOG_ERROR("Invalid message type recieved: %u", 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");
-            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);
-    }
-};
-
-class URMessageParser : 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");
-            return std::unique_ptr<Packet>(nullptr);        
-        }
-
-        bp.parse(packet_size); //consumes the peeked data
-        bp.parse(type);
-
-        if(type != message_type::ROBOT_MESSAGE) {
-            LOG_ERROR("Invalid message type recieved: %u", static_cast<uint8_t>(type));
-            return std::unique_ptr<Packet>(nullptr);
-        }
-
-        uint64_t timestamp;
-        uint8_t source;
-        robot_message_type message_type;
-
-        bp.parse(timestamp);
-        bp.parse(source);
-        bp.parse(message_type);
-
-        std::unique_ptr<Packet> obj(nullptr);
-
-        switch(message_type) {
-            case robot_message_type::ROBOT_MESSAGE_VERSION:
-                VersionMessage *vm = new VersionMessage();
-                if(vm->parse_with(bp))
-                    obj.reset(vm);
-                break;
-        }
-
-        return obj;
-    }
+class URParser {
+public:
+    virtual bool parse(BinParser &bp, std::vector<std::unique_ptr<T>> &results) = 0;
 };

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);
+    }
 };

include/ur_modern_driver/ur/robot_mode.h

@@ -4,10 +4,11 @@
 #include <inttypes.h>
 #include "ur_modern_driver/types.h"
 #include "ur_modern_driver/bin_parser.h"
+#include "ur_modern_driver/ur/state.h"
 
 class SharedRobotModeData {
 public:
-    bool parse_with(BinParser &bp);
+    virtual bool parse_with(BinParser &bp);
 
     uint64_t timestamp;
     bool physical_robot_connected;
@@ -34,9 +35,11 @@ enum class robot_mode_V1_X : uint8_t {
 	ROBOT_SAFEGUARD_STOP_MODE = 10
 };
 
-class RobotModeData_V1_X : public SharedRobotModeData {
+class RobotModeData_V1_X : public SharedRobotModeData, public StatePacket {
 public:
-    bool parse_with(BinParser &bp);
+    virtual bool parse_with(BinParser &bp);
+    virtual bool consume_with(URStatePacketConsumer &consumer);
+
 
     bool security_stopped;
     robot_mode_V1_X robot_mode;
@@ -69,9 +72,11 @@ enum class robot_control_mode_V3_X : uint8_t {
     TORQUE = 3
 };
 
-class RobotModeData_V3_0__1 : public SharedRobotModeData {
+class RobotModeData_V3_0__1 : public SharedRobotModeData, public StatePacket {
 public:
-    bool parse_with(BinParser &bp);
+    virtual bool parse_with(BinParser &bp);
+    virtual bool consume_with(URStatePacketConsumer &consumer);
+
 
     bool protective_stopped;
 
@@ -93,7 +98,9 @@ public:
 
 class RobotModeData_V3_2 : public RobotModeData_V3_0__1 {
 public:
-    bool parse_with(BinParser &bp);
+    virtual bool parse_with(BinParser &bp);
+    virtual bool consume_with(URStatePacketConsumer &consumer);
+
 
     double target_speed_fraction_limit;
 

include/ur_modern_driver/ur/rt_parser.h

@@ -0,0 +1,36 @@
+#pragma once
+#include <vector>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/bin_parser.h"
+#include "ur_modern_driver/pipeline.h"
+#include "ur_modern_driver/ur/parser.h"
+#include "ur_modern_driver/ur/rt_state.h"
+
+
+template <typename T>
+class URRTStateParser : public URParser<RTPacket> {
+public:
+    bool parse(BinParser &bp, std::vector<std::unique_ptr<RTPacket>> &results) {
+        int32_t packet_size = bp.peek<int32_t>();
+
+        if(!bp.check_size(packet_size)) {
+            LOG_ERROR("Buffer len shorter than expected packet length");
+            return false;        
+        }
+
+        bp.parse(packet_size); //consumes the peeked data
+
+        std::unique_ptr<RTPacket> packet(new T);
+        if(!packet->parse_with(bp))
+            return false;
+        
+        results.push_back(std::move(packet));
+        
+        return true;
+    }
+};
+
+typedef URRTStateParser<RTState_V1_6__7> URRTStateParser_V1_6__7;
+typedef URRTStateParser<RTState_V1_8> URRTStateParser_V1_8;
+typedef URRTStateParser<RTState_V3_0__1> URRTStateParser_V3_0__1;
+typedef URRTStateParser<RTState_V3_2__3> URRTStateParser_V3_2__3;

include/ur_modern_driver/ur/rt_state.h

@@ -4,11 +4,22 @@
 #include <inttypes.h>
 #include "ur_modern_driver/types.h"
 #include "ur_modern_driver/bin_parser.h"
+#include "ur_modern_driver/pipeline.h"
 
-class RTState_V1_6__7 {
+class URRTPacketConsumer;
+
+class RTPacket {
 public:
-    bool parse_with(BinParser &bp);
+    virtual bool parse_with(BinParser &bp) = 0;
+    virtual bool consume_with(URRTPacketConsumer &consumer) = 0;
+};
 
+class RTShared {
+protected:
+    bool parse_shared1(BinParser &bp);
+    bool parse_shared2(BinParser &bp);
+
+public:
     double time;
     double q_target[6];
     double qd_target[6];
@@ -18,21 +29,37 @@ public:
     double q_actual[6];
     double qd_actual[6];
     double i_actual[6];
-    double3_t tool_accelerometer_values;
+
+    //gap here depending on version
+
     double tcp_force[6];
-    cartesian_coord_t tool_vector;
-    double tcp_speed[6];
+    cartesian_coord_t tool_vector_actual;
+    cartesian_coord_t tcp_speed_actual;
+
+    //gap here depending on version
+
     uint64_t digital_input;
     double motor_temperatures[6];
     double controller_time;
     double robot_mode;
 
+
     static const size_t SIZE = sizeof(double) * 3 
-        + sizeof(double[6]) * 11
-        + sizeof(double3_t)
-        + sizeof(cartesian_coord_t)
+        + sizeof(double[6]) * 10
+        + sizeof(cartesian_coord_t) * 2
         + sizeof(uint64_t);
 
+};
+
+class RTState_V1_6__7 : public RTShared, public RTPacket {
+public:
+    bool parse_with(BinParser &bp);
+    virtual bool consume_with(URRTPacketConsumer &consumer);
+
+    double3_t tool_accelerometer_values;
+
+    static const size_t SIZE = RTShared::SIZE
+        + sizeof(double3_t);
 
     static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!");
 };
@@ -40,6 +67,7 @@ public:
 class RTState_V1_8 : public RTState_V1_6__7 {
 public:
     bool parse_with(BinParser &bp);
+    virtual bool consume_with(URRTPacketConsumer &consumer);
 
     double joint_modes[6];
 
@@ -49,10 +77,47 @@ public:
     static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!");
 };
 
-class RTState_V3_0__1 {
+class RTState_V3_0__1 : public RTShared, public RTPacket {
 public:
-    //bool parse_with(BinParser &bp);
+    bool parse_with(BinParser &bp);
+    virtual bool consume_with(URRTPacketConsumer &consumer);
 
-    double m_actual[6];
     double i_control[6];
+    cartesian_coord_t tool_vector_target;
+    cartesian_coord_t tcp_speed_target;
+
+
+    double joint_modes[6];
+    double safety_mode;
+    double3_t tool_accelerometer_values;
+    double speed_scaling;
+    double linear_momentum_norm;
+    double v_main;
+    double v_robot;
+    double i_robot;
+    double v_actual[6];
+
+
+    static const size_t SIZE = RTShared::SIZE 
+        + sizeof(double[6]) * 3
+        + sizeof(double3_t)
+        + sizeof(cartesian_coord_t) * 2
+        + sizeof(double) * 6;
+
+    static_assert(RTState_V3_0__1::SIZE == 920, "RTState_V3_0__1 has mismatched size!");
+};
+
+class RTState_V3_2__3 : public RTState_V3_0__1 {
+public:
+    bool parse_with(BinParser &bp);
+    virtual bool consume_with(URRTPacketConsumer &consumer);
+
+    uint64_t digital_outputs;
+    double program_state;
+
+    static const size_t SIZE = RTState_V3_0__1::SIZE 
+        + sizeof(uint64_t)
+        + sizeof(double);
+
+    static_assert(RTState_V3_2__3::SIZE == 936, "RTState_V3_2__3 has mismatched size!");  
 };

include/ur_modern_driver/ur/state.h

@@ -1,12 +1,10 @@
 #pragma once
+
 #include <cstddef>
 #include <inttypes.h>
+#include "ur_modern_driver/pipeline.h"
 #include "ur_modern_driver/bin_parser.h"
-#include "ur_modern_driver/packet.h"
-#include "ur_modern_driver/ur/master_board.h"
-#include "ur_modern_driver/ur/robot_mode.h"
-
-
+#include "ur_modern_driver/log.h"
 
 enum class package_type : uint8_t {
 	ROBOT_MODE_DATA = 0,
@@ -27,68 +25,10 @@ enum class message_type : uint8_t {
     PROGRAM_STATE_MESSAGE = 25
 };
 
+class URStatePacketConsumer;
 
-class RobotState : public Packet {
+class StatePacket {
 public:
-    bool parse_with(BinParser &bp);
-protected:
-    virtual bool parse_package(BinParser &bp) = 0;
-};
-
-class RobotState_V1_6__7 : public RobotState {
-protected:
-    bool parse_package(BinParser &bp);
-public:
-    RobotModeData_V1_X robot_mode;
-    //JointData
-    //ToolData
-    MasterBoardData_V1_X master_board;
-    //CartesianInfo
-};
-
-class RobotState_V1_8 : public RobotState_V1_6__7 {
-protected:
-    bool parse_package(BinParser &bp);
-public:
-    
-    //KinematicsInfo
-    //ConfigurationData
-    //ForceModeData
-    //AdditionalInfo
-    //CalibrationData
-};
-
-
-class RobotState_V3_0__1 : public RobotState {
-protected:
-    bool parse_package(BinParser &bp);
-public:
-    RobotModeData_V3_0__1 robot_mode;
-    //JointData
-    //ToolData
-    MasterBoardData_V3_0__1 master_board;
-    //CartesianInfo
-
-    //KinematicsInfo
-    //ConfigurationData
-    //ForceModeData
-    //AdditionalInfo
-    //CalibrationData
-};
-
-class RobotState_V3_2 : public RobotState {
-protected:
-    bool parse_package(BinParser &bp);
-public:
-    RobotModeData_V3_2 robot_mode;
-    //JointData
-    //ToolData
-    MasterBoardData_V3_2 master_board;
-    //CartesianInfo
-
-    //KinematicsInfo
-    //ConfigurationData
-    //ForceModeData
-    //AdditionalInfo
-    //CalibrationData
+    virtual bool parse_with(BinParser &bp) = 0;
+    virtual bool consume_with(URStatePacketConsumer &consumer) = 0;
 };

include/ur_modern_driver/ur/state_parser.h

@@ -0,0 +1,82 @@
+#pragma once
+#include <vector>
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/bin_parser.h"
+#include "ur_modern_driver/pipeline.h"
+#include "ur_modern_driver/ur/parser.h"
+#include "ur_modern_driver/ur/state.h"
+#include "ur_modern_driver/ur/master_board.h"
+#include "ur_modern_driver/ur/robot_mode.h"
+
+
+template <typename RMD, typename MBD>
+class URStateParser : public URParser<StatePacket> {
+private:
+    StatePacket* from_type(package_type type) {
+        switch(type) {
+            case package_type::ROBOT_MODE_DATA:
+                return new RMD;
+            case package_type::MASTERBOARD_DATA:
+                return new MBD;
+            default:
+                return nullptr;
+        }
+    }
+
+public:
+    bool parse(BinParser &bp, std::vector<std::unique_ptr<StatePacket>> &results) {
+        int32_t packet_size;
+        message_type type;
+        bp.parse(packet_size);
+        bp.parse(type);
+
+        if(type != message_type::ROBOT_STATE) {
+            LOG_WARN("Invalid message type recieved: %u", static_cast<uint8_t>(type));
+            return false;
+        }
+
+        while(!bp.empty()) {
+            if(!bp.check_size(sizeof(uint32_t))){
+                LOG_ERROR("Failed to read sub-package length, there's likely a parsing error");
+                return false;    
+            }
+            uint32_t sub_size = bp.peek<uint32_t>();
+            if(!bp.check_size(static_cast<size_t>(sub_size))) {
+                LOG_WARN("Invalid sub-package size of %" PRIu32 " received!", sub_size);
+                return false;
+            }
+            
+            //deconstruction of a sub parser will increment the position of the parent parser 
+            BinParser sbp(bp, sub_size);
+            sbp.consume(sizeof(sub_size));
+            package_type type;
+            sbp.parse(type);
+            
+            std::unique_ptr<StatePacket> packet(from_type(type));
+
+            if(packet == nullptr) {
+                sbp.consume();
+                LOG_INFO("Skipping sub-packet of type %d", type);
+                continue;
+            }
+
+            if(!packet->parse_with(sbp)) {
+                LOG_ERROR("Sub-package parsing of type %d failed!", type);
+                return false;
+            }
+
+            results.push_back(std::move(packet));
+
+            if(!sbp.empty()) {
+                LOG_ERROR("Sub-package was not parsed completely!");
+                return false;
+            }
+        }
+
+        return true;
+    }
+};
+
+typedef URStateParser<RobotModeData_V1_X, MasterBoardData_V1_X>         URStateParser_V1_X;
+typedef URStateParser<RobotModeData_V3_0__1, MasterBoardData_V3_0__1>   URStateParser_V3_0__1;
+typedef URStateParser<RobotModeData_V3_2, MasterBoardData_V3_2>         URStateParser_V3_2;

src/ur/master_board.cpp

@@ -1,4 +1,5 @@
 #include "ur_modern_driver/ur/master_board.h"
+#include "ur_modern_driver/ur/consumer.h"
 
 bool SharedMasterBoardData::parse_with(BinParser &bp) {
     bp.parse(analog_input_range0);
@@ -80,3 +81,16 @@ bool MasterBoardData_V3_2::parse_with(BinParser &bp) {
     return true;
 }
 
+
+
+
+
+bool MasterBoardData_V1_X::consume_with(URStatePacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool MasterBoardData_V3_0__1::consume_with(URStatePacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool MasterBoardData_V3_2::consume_with(URStatePacketConsumer &consumer) {
+    return consumer.consume(*this);
+}

src/ur/messages.cpp

@@ -1,11 +1,5 @@
 #include "ur_modern_driver/ur/messages.h"
-
-bool MessageBase::parse_with(BinParser &bp) {
-    bp.parse(timestamp);
-    bp.parse(source);
-
-    return true; //not really possible to check dynamic size packets
-}
+#include "ur_modern_driver/ur/consumer.h"
 
 
 bool VersionMessage::parse_with(BinParser &bp) {
@@ -19,3 +13,8 @@ bool VersionMessage::parse_with(BinParser &bp) {
 
     return true; //not really possible to check dynamic size packets    
 }
+
+
+bool VersionMessage::consume_with(URMessagePacketConsumer &consumer) {
+    return consumer.consume(*this);
+}

src/ur/producer.cpp

@@ -1,32 +0,0 @@
-#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];
-
-    //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 std::unique_ptr<Packet>(nullptr);
-    }
-
-    BinParser bp(buf, static_cast<size_t>(len));
-    return std::move(_parser.parse(bp));
-}

src/ur/robot_mode.cpp

@@ -1,4 +1,5 @@
 #include "ur_modern_driver/ur/robot_mode.h"
+#include "ur_modern_driver/ur/consumer.h"
 
 
 bool SharedRobotModeData::parse_with(BinParser &bp) {
@@ -54,3 +55,16 @@ bool RobotModeData_V3_2::parse_with(BinParser &bp) {
 
     return true;
 }
+
+
+
+
+bool RobotModeData_V1_X::consume_with(URStatePacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool RobotModeData_V3_0__1::consume_with(URStatePacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool RobotModeData_V3_2::consume_with(URStatePacketConsumer &consumer) {
+    return consumer.consume(*this);
+}

src/ur/rt_state.cpp

@@ -1,9 +1,8 @@
 #include "ur_modern_driver/ur/rt_state.h"
+#include "ur_modern_driver/ur/consumer.h"
 
-bool RTState_V1_6__7::parse_with(BinParser &bp) {
-    if(!bp.check_size<RTState_V1_6__7>())
-        return false;
 
+bool RTShared::parse_shared1(BinParser &bp) {
     bp.parse(time);
     bp.parse(q_target);
     bp.parse(qd_target);
@@ -13,14 +12,30 @@ bool RTState_V1_6__7::parse_with(BinParser &bp) {
     bp.parse(q_actual);
     bp.parse(qd_actual);
     bp.parse(i_actual);
-    bp.parse(tool_accelerometer_values);
-    bp.parse(tcp_force);
-    bp.parse(tool_vector);
-    bp.parse(tcp_speed);
+    return true;
+}
+
+bool RTShared::parse_shared2(BinParser &bp) {
     bp.parse(digital_input);
     bp.parse(motor_temperatures);
     bp.parse(controller_time);
     bp.parse(robot_mode);
+    return true;
+}
+
+
+bool RTState_V1_6__7::parse_with(BinParser &bp) {
+    if(!bp.check_size<RTState_V1_6__7>())
+        return false;
+
+    parse_shared1(bp);
+    
+    bp.parse(tool_accelerometer_values);
+    bp.parse(tcp_force);
+    bp.parse(tool_vector_actual);
+    bp.parse(tcp_speed_actual);
+
+    parse_shared2(bp);
 
     return true;
 }
@@ -35,3 +50,60 @@ bool RTState_V1_8::parse_with(BinParser &bp) {
 
     return true;
 }
+
+bool RTState_V3_0__1::parse_with(BinParser &bp) {
+    if(!bp.check_size<RTState_V3_0__1>())
+        return false;
+
+    parse_shared1(bp);
+
+    bp.parse(i_control);
+    bp.parse(tool_vector_actual);
+    bp.parse(tcp_speed_actual);
+    bp.parse(tcp_force);
+    bp.parse(tool_vector_target);
+    bp.parse(tcp_speed_target);
+
+    parse_shared2(bp);
+
+    bp.parse(joint_modes);
+    bp.parse(safety_mode);
+    bp.consume(sizeof(double[6])); //skip undocumented
+    bp.parse(tool_accelerometer_values);
+    bp.consume(sizeof(double[6])); //skip undocumented
+    bp.parse(speed_scaling);
+    bp.parse(linear_momentum_norm);
+    bp.consume(sizeof(double)); //skip undocumented
+    bp.consume(sizeof(double)); //skip undocumented    
+    bp.parse(v_main);
+    bp.parse(v_robot);
+    bp.parse(i_robot);
+    bp.parse(v_actual);
+
+    return true;
+}
+
+bool RTState_V3_2__3::parse_with(BinParser &bp) {
+    if(!bp.check_size<RTState_V3_2__3>())
+        return false;
+
+    RTState_V3_0__1::parse_with(bp);
+
+    bp.parse(digital_outputs);
+    bp.parse(program_state);
+
+    return true;
+}
+
+bool RTState_V1_6__7::consume_with(URRTPacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool RTState_V1_8::consume_with(URRTPacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool RTState_V3_0__1::consume_with(URRTPacketConsumer &consumer) {
+    return consumer.consume(*this);
+}
+bool RTState_V3_2__3::consume_with(URRTPacketConsumer &consumer) {
+    return consumer.consume(*this);
+}

src/ur/state.cpp

@@ -2,6 +2,10 @@
 #include "ur_modern_driver/ur/state.h"
 
 
+
+//StatePacket::~StatePacket() { }
+
+/*
 bool RobotState::parse_with(BinParser &bp) {
     //continue as long as there are bytes to read
     while(!bp.empty()) {
@@ -110,3 +114,4 @@ bool RobotState_V3_0__1::parse_package(BinParser &bp) {
 bool RobotState_V3_2::parse_package(BinParser &bp) {
     return parse_advanced(bp, *this);
 }
+*/