Completed parsing of UR state, messages and RT

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);
-    }
+class URParser {
+public:
+    virtual bool parse(BinParser &bp, std::vector<std::unique_ptr<T>> &results) = 0;
 };
-
-
-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;
-    }
-};

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;