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>();
+        int32_t packet_size;
         message_type type;
-
+        bp.parse(packet_size);
-        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));
+            LOG_ERROR("Invalid message type recieved: %u", static_cast<uint8_t>(type));
             return std::unique_ptr<Packet>(nullptr);
         }
 
@@ -37,7 +33,7 @@ class URRTStateParser : public Parser {
         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);        
         }
 
@@ -50,3 +46,43 @@ 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
@@ -57,3 +57,38 @@ public:
     //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

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

src/ur/master_board.cpp

@@ -40,8 +40,8 @@ bool MasterBoardData_V1_X::parse_with(BinParser &bp) {
     return true;
 }
 
-bool MasterBoardData_V3_X::parse_with(BinParser &bp) {
+bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) {
-    if(!bp.check_size<MasterBoardData_V3_X>())
+    if(!bp.check_size<MasterBoardData_V3_0__1>())
         return false;
 
     bp.parse(digital_input_bits);
@@ -54,13 +54,29 @@ bool MasterBoardData_V3_X::parse_with(BinParser &bp) {
     bp.parse(euromap67_interface_installed);
 
     if(euromap67_interface_installed) {
-        if(!bp.check_size(MasterBoardData_V3_X::EURO_SIZE))
+        if(!bp.check_size(MasterBoardData_V3_0__1::EURO_SIZE))
             return false;
             
         bp.parse(euromap_voltage);
         bp.parse(euromap_current);
     }
 
+    bp.consume(sizeof(uint32_t));
+
+    return true;
+}
+
+
+
+bool MasterBoardData_V3_2::parse_with(BinParser &bp) {
+    if(!bp.check_size<MasterBoardData_V3_2>())
+        return false;
+
+    MasterBoardData_V3_0__1::parse_with(bp);
+
+    bp.parse(operational_mode_selector_input);
+    bp.parse(three_position_enabling_device_input);
+
     return true;
 }
 

src/ur/messages.cpp

@@ -13,10 +13,9 @@ bool VersionMessage::parse_with(BinParser &bp) {
     bp.parse(project_name);
     bp.parse(major_version);
     bp.parse(minor_version);
-    bp.parse(svn_version); //net to host?
+    bp.parse(svn_version);
-
+    bp.consume(sizeof(uint32_t)); //undocumented field??
-    // how to parse this without length??
+    bp.parse_remainder(build_date);
-    //bp.parse(build_date);
 
     return true; //not really possible to check dynamic size packets    
 }

src/ur/producer.cpp

@@ -17,38 +17,16 @@ 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);
+    ssize_t len = _stream.receive(buf, sizeof(buf));
+
+    LOG_DEBUG("Read %d bytes from stream", len);
 
     if(len < 1) {
-            LOG_DEBUG("Read nothing from stream");
+        LOG_WARN("Read nothing from stream");
         return std::unique_ptr<Packet>(nullptr);
     }
 
-        total += len;
+    BinParser bp(buf, static_cast<size_t>(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' (%d) larger than buffer was received, discarding...", packet_size);
-                return std::unique_ptr<Packet>(nullptr);
-            }
-        }
-
-        if(total < packet_size){
-            LOG_DEBUG("Partial packet recieved");
-            continue;
-        }
     return std::move(_parser.parse(bp));
-    }
 }

src/ur/state.cpp

@@ -4,7 +4,8 @@
 
 bool RobotState::parse_with(BinParser &bp) {
     //continue as long as there are bytes to read
-    while(bp.check_size(sizeof(uint8_t))) {
+    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;    
@@ -18,56 +19,94 @@ bool RobotState::parse_with(BinParser &bp) {
         
         //deconstruction of a sub parser will increment the position of the parent parser 
         BinParser sub_parser(bp, sub_size);
-        sub_parser.parse(sub_size);
+        sub_parser.consume(sizeof(sub_size));
 
-        if(!parse_package(sub_parser))
+        if(!parse_package(sub_parser)) {
+            LOG_ERROR("Failed to parse sub-package");
+            return false;
+        }
+
+        if(!sub_parser.empty()) {
+            LOG_ERROR("Sub-package was not parsed completely!");
+            return false;
+        }
+    }
+    return true;
+}
+
+template <typename T>
+bool parse_base(BinParser &bp, T &pkg) {
+    package_type type = bp.peek<package_type>();
+    switch(type) {
+        case package_type::ROBOT_MODE_DATA:
+            LOG_DEBUG("Parsing robot_mode");
+            bp.consume(sizeof(package_type));
+            pkg.robot_mode.parse_with(bp);
+            break;
+        case package_type::MASTERBOARD_DATA:
+            LOG_DEBUG("Parsing master_board");
+            bp.consume(sizeof(package_type));
+            pkg.master_board.parse_with(bp);
+            break;
+
+        case package_type::TOOL_DATA:
+        case package_type::CARTESIAN_INFO:
+        case package_type::JOINT_DATA:
+            LOG_DEBUG("Skipping tool, cartesian or joint data");        
+            //for unhandled packages we consume the rest of the 
+            //package buffer
+            bp.consume();
+            break;
+        default:
             return false;
     }
     return true;
 }
 
-bool RobotState_V1_6__7::parse_package(BinParser &bp) {
+
-    //todo: size checks
+template <typename T>
-    package_type type;
+bool parse_advanced(BinParser &bp, T &pkg) {
-    bp.parse(type);
+    if(parse_base(bp, pkg))
+        return true;
+
+    package_type type = bp.peek<package_type>();
 
     switch(type) {
-        case package_type::ROBOT_MODE_DATA:
+        case package_type::KINEMATICS_INFO:
-            robot_mode.parse_with(bp);
+        case package_type::CONFIGURATION_DATA:
-            break;
+        case package_type::ADDITIONAL_INFO:
-        case package_type::JOINT_DATA:
+        case package_type::CALIBRATION_DATA:
-            //TODO
+        case package_type::FORCE_MODE_DATA:
-            break;
+            LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data");        
-        case package_type::TOOL_DATA:
+            //for unhandled packages we consume the rest of the 
-            //TODO
+            //package buffer
-            break;
+            bp.consume();
-        case package_type::MASTERBOARD_DATA:
-            master_board.parse_with(bp);
-            break;
-        case package_type::CARTESIAN_INFO:
-            //TODO
             break;
         default:
-            LOG_ERROR("Invalid package type parsed: %" PRIu8 "", type);
+            LOG_ERROR("Invalid sub-package type parsed: %" PRIu8 "", type);
             return false;
     }
 
     return true;
 }
 
+
+bool RobotState_V1_6__7::parse_package(BinParser &bp) {
+    if(!parse_base(bp, *this)) {
+        LOG_ERROR("Invalid sub-package type parsed: %" PRIu8 "", bp.peek<package_type>());
+        return false;
+    }
+    return true;
+}
+
 bool RobotState_V1_8::parse_package(BinParser &bp) {
-    package_type type = bp.peek<package_type>();
+    return parse_advanced(bp, *this);
-    switch(type) {
+}
-        case package_type::KINEMATICS_INFO:
+
-            break;
+bool RobotState_V3_0__1::parse_package(BinParser &bp) {
-        case package_type::CONFIGURATION_DATA:
+    return parse_advanced(bp, *this);
-            break;
+}
-        case package_type::ADDITIONAL_INFO:
+
-            break;
+bool RobotState_V3_2::parse_package(BinParser &bp) {
-        case package_type::CALIBRATION_DATA:
+    return parse_advanced(bp, *this);
-            break;
-        default:
-            return RobotState_V1_6__7::parse_package(bp);
-    }
-    return true;
 }

src/ur/stream.cpp

@@ -1,6 +1,7 @@
 #include <cstring>
 #include <unistd.h>
 #include <netinet/tcp.h>
+#include <endian.h>
 
 #include "ur_modern_driver/ur/stream.h"
 #include "ur_modern_driver/log.h"
@@ -78,8 +79,8 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) {
     //handle partial sends
     while(total < buf_len) {
         ssize_t sent = ::send(_socket_fd, buf+total, remaining, 0);
-        if(sent == -1) 
+        if(sent <= 0) 
-            return _stopping ? 0 : -1;
+            return _stopping ? 0 : sent;
         total += sent;
         remaining -= sent;
     }
@@ -88,6 +89,32 @@ ssize_t URStream::send(uint8_t *buf, size_t buf_len) {
 }
 
 ssize_t URStream::receive(uint8_t *buf, size_t buf_len) {
-    //TODO: handle reconnect?
+    if(!_initialized)
-    return recv(_socket_fd, buf, buf_len, 0);
+        return -1;
+    if(_stopping)
+        return 0;
+
+    size_t remainder = sizeof(int32_t);
+    uint8_t *buf_pos = buf;
+    bool initial = true;
+
+    do {
+        ssize_t read = recv(_socket_fd, buf_pos, remainder, 0);
+        if(read <= 0) //failed reading from socket
+            return _stopping ? 0 : read; 
+        
+        if(initial) {
+            remainder = be32toh(*(reinterpret_cast<int32_t*>(buf)));
+            if(remainder >= (buf_len - sizeof(int32_t))) {
+                LOG_ERROR("Packet size %d is larger than buffer %d, discarding.", remainder, buf_len);
+                return -1;
+            }
+            initial = false;
+        }
+
+        buf_pos += read;
+        remainder -= read;        
+    } while(remainder > 0);
+
+    return buf_pos - buf;
 }