Refactoring, improvements and fixes

include/ur_modern_driver/ur/master_board.h

@@ -60,7 +60,7 @@ public:
         + sizeof(int16_t) * 2;
 };
 
-class MasterBoardData_V3_X : public SharedMasterBoardData {
+class MasterBoardData_V3_0__1 : public SharedMasterBoardData {
 public:
     virtual bool parse_with(BinParser &bp);
 
@@ -77,8 +77,20 @@ public:
 
     static const size_t SIZE = SharedMasterBoardData::SIZE
         + sizeof(int32_t) * 2
-        + sizeof(uint8_t) * 2;
+        + sizeof(uint8_t) * 2
+        + sizeof(uint32_t); //UR internal field we skip
 
     static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE
         + sizeof(float) * 2;
 };
+
+class MasterBoardData_V3_2 : public MasterBoardData_V3_0__1 {
+public:
+    virtual bool parse_with(BinParser &bp);
+
+    uint8_t operational_mode_selector_input;
+    uint8_t three_position_enabling_device_input;
+
+    static const size_t SIZE = MasterBoardData_V3_0__1::SIZE
+        + sizeof(uint8_t) * 2;
+};

include/ur_modern_driver/ur/messages.h

@@ -1,6 +1,21 @@
 #pragma once
+#include <cstddef>
+#include <inttypes.h>
+#include "ur_modern_driver/packet.h"
 
-class MessageBase {
+enum class robot_message_type : uint8_t {
+	ROBOT_MESSAGE_TEXT = 0,
+	ROBOT_MESSAGE_PROGRAM_LABEL = 1,
+	PROGRAM_STATE_MESSAGE_VARIABLE_UPDATE = 2,
+	ROBOT_MESSAGE_VERSION = 3,
+	ROBOT_MESSAGE_SAFETY_MODE = 5,
+	ROBOT_MESSAGE_ERROR_CODE = 6,
+	ROBOT_MESSAGE_KEY = 7,
+	ROBOT_MESSAGE_REQUEST_VALUE = 9,
+	ROBOT_MESSAGE_RUNTIME_EXCEPTION = 10
+};
+
+class MessageBase : public Packet {
 public:
     virtual bool parse_with(BinParser &bp) = 0;
 
@@ -17,4 +32,4 @@ public:
     uint8_t minor_version;
     int32_t svn_version;
     std::string build_date;
-}
+};

include/ur_modern_driver/ur/parser.h

@@ -2,23 +2,19 @@
 #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"
 
 template <typename T>
 class URStateParser : public Parser {
-    std::unique_ptr<Packet> parse(BinParser &bp)  {
-        int32_t packet_size = bp.peek<int32_t>();
+    std::unique_ptr<Packet> parse(BinParser &bp) {
+        int32_t packet_size;
         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(packet_size);
         bp.parse(type);
 
         if(type != message_type::ROBOT_STATE) {
-            LOG_ERROR("Invalid message type recieved: %u\n", static_cast<uint8_t>(type));
+            LOG_ERROR("Invalid message type recieved: %u", static_cast<uint8_t>(type));
             return std::unique_ptr<Packet>(nullptr);
         }
 
@@ -33,11 +29,11 @@ class URStateParser : public Parser {
 
 template <typename T>
 class URRTStateParser : public Parser {
-    std::unique_ptr<Packet> parse(BinParser &bp)  {
+    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");
+            LOG_ERROR("Buffer len shorter than expected packet length");
             return std::unique_ptr<Packet>(nullptr);        
         }
 
@@ -49,4 +45,44 @@ class URRTStateParser : public Parser {
         
         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/state.h

@@ -37,7 +37,7 @@ protected:
 
 class RobotState_V1_6__7 : public RobotState {
 protected:
-    virtual bool parse_package(BinParser &bp) = 0;
+    bool parse_package(BinParser &bp);
 public:
     RobotModeData_V1_X robot_mode;
     //JointData
@@ -48,7 +48,7 @@ public:
 
 class RobotState_V1_8 : public RobotState_V1_6__7 {
 protected:
-    virtual bool parse_package(BinParser &bp) = 0;
+    bool parse_package(BinParser &bp);
 public:
     
     //KinematicsInfo
@@ -56,4 +56,39 @@ public:
     //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
+};

include/ur_modern_driver/ur/stream.h

@@ -21,6 +21,10 @@ public:
         _port(port),
         _initialized(false),
         _stopping(false) {}
+    
+    ~URStream() {
+        disconnect();
+    }
 
     bool connect();
     void disconnect();