Refactoring, improvements and fixes

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) {
-    if(!bp.check_size<MasterBoardData_V3_X>())
+bool MasterBoardData_V3_0__1::parse_with(BinParser &bp) {
+    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?
-
-    // how to parse this without length??
-    //bp.parse(build_date);
+    bp.parse(svn_version);
+    bp.consume(sizeof(uint32_t)); //undocumented field??
+    bp.parse_remainder(build_date);
 
     return true; //not really possible to check dynamic size packets    
 }

src/ur/producer.cpp

@@ -16,39 +16,17 @@ void URProducer::stop_producer() {
 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(buf, sizeof(buf));
 
-        //blocking call
-        ssize_t len = _stream.receive(pos, size);
-        
-        if(len < 1) {
-            LOG_DEBUG("Read nothing from stream");
-            return std::unique_ptr<Packet>(nullptr);
-        }
+    LOG_DEBUG("Read %d bytes from stream", len);
 
-        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' (%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));
+    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/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
-    package_type type;
-    bp.parse(type);
+
+template <typename T>
+bool parse_advanced(BinParser &bp, T &pkg) {
+    if(parse_base(bp, pkg))
+        return true;
+
+    package_type type = bp.peek<package_type>();
 
     switch(type) {
-        case package_type::ROBOT_MODE_DATA:
-            robot_mode.parse_with(bp);
-            break;
-        case package_type::JOINT_DATA:
-            //TODO
-            break;
-        case package_type::TOOL_DATA:
-            //TODO
-            break;
-        case package_type::MASTERBOARD_DATA:
-            master_board.parse_with(bp);
-            break;
-        case package_type::CARTESIAN_INFO:
-            //TODO
+        case package_type::KINEMATICS_INFO:
+        case package_type::CONFIGURATION_DATA:
+        case package_type::ADDITIONAL_INFO:
+        case package_type::CALIBRATION_DATA:
+        case package_type::FORCE_MODE_DATA:
+            LOG_DEBUG("Skipping kinematics, config, additional, calibration or force mode data");        
+            //for unhandled packages we consume the rest of the 
+            //package buffer
+            bp.consume();
             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>();
-    switch(type) {
-        case package_type::KINEMATICS_INFO:
-            break;
-        case package_type::CONFIGURATION_DATA:
-            break;
-        case package_type::ADDITIONAL_INFO:
-            break;
-        case package_type::CALIBRATION_DATA:
-            break;
-        default:
-            return RobotState_V1_6__7::parse_package(bp);
-    }
-    return true;
+    return parse_advanced(bp, *this);
+}
+
+bool RobotState_V3_0__1::parse_package(BinParser &bp) {
+    return parse_advanced(bp, *this);
+}
+
+bool RobotState_V3_2::parse_package(BinParser &bp) {
+    return parse_advanced(bp, *this);
 }

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) 
-            return _stopping ? 0 : -1;
+        if(sent <= 0) 
+            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?
-    return recv(_socket_fd, buf, buf_len, 0);
+    if(!_initialized)
+        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;
 }