Implemented initial robot state and parsing

src/ur/master_board.cpp

@@ -0,0 +1,66 @@
+#include "ur_modern_driver/ur/master_board.h"
+
+bool SharedMasterBoardData::parse_with(BinParser &bp) {
+    bp.parse(analog_input_range0);
+    bp.parse(analog_input_range1);
+    bp.parse(analog_input0);
+    bp.parse(analog_input1);    
+    bp.parse(analog_output_domain0);
+    bp.parse(analog_output_domain1);
+    bp.parse(analog_output0);
+    bp.parse(analog_output1);
+    bp.parse(master_board_temperature);
+    bp.parse(robot_voltage_48V);
+    bp.parse(robot_current);
+    bp.parse(master_IO_current);
+    return true;
+}
+
+bool MasterBoardData_V1_X::parse_with(BinParser &bp) {
+    if(!bp.check_size<MasterBoardData_V1_X>())
+        return false;
+
+    bp.parse(digital_input_bits);
+    bp.parse(digital_output_bits);
+
+    SharedMasterBoardData::parse_with(bp);
+
+    bp.parse(master_safety_state);
+    bp.parse(master_on_off_state);
+    bp.parse(euromap67_interface_installed);
+
+    if(euromap67_interface_installed) {
+        if(!bp.check_size(MasterBoardData_V1_X::EURO_SIZE))
+            return false;
+
+        bp.parse(euromap_voltage);
+        bp.parse(euromap_current);
+    }
+
+    return true;
+}
+
+bool MasterBoardData_V3_X::parse_with(BinParser &bp) {
+    if(!bp.check_size<MasterBoardData_V3_X>())
+        return false;
+
+    bp.parse(digital_input_bits);
+    bp.parse(digital_output_bits);
+    
+    SharedMasterBoardData::parse_with(bp);
+
+    bp.parse(safety_mode);
+    bp.parse(in_reduced_mode);
+    bp.parse(euromap67_interface_installed);
+
+    if(euromap67_interface_installed) {
+        if(!bp.check_size(MasterBoardData_V3_X::EURO_SIZE))
+            return false;
+            
+        bp.parse(euromap_voltage);
+        bp.parse(euromap_current);
+    }
+
+    return true;
+}
+

src/ur/messages.cpp

@@ -0,0 +1,22 @@
+#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
+}
+
+
+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);
+
+    return true; //not really possible to check dynamic size packets    
+}

src/ur/robot_mode.cpp

@@ -0,0 +1,56 @@
+#include "ur_modern_driver/ur/robot_mode.h"
+
+
+bool SharedRobotModeData::parse_with(BinParser &bp) {
+    bp.parse(timestamp);
+    bp.parse(physical_robot_connected);
+    bp.parse(real_robot_enabled);
+    bp.parse(robot_power_on);
+    bp.parse(emergency_stopped);
+    return true;
+}
+
+
+bool RobotModeData_V1_X::parse_with(BinParser &bp) {
+    if(!bp.check_size<RobotModeData_V1_X>())
+        return false;
+
+    SharedRobotModeData::parse_with(bp);
+
+    bp.parse(security_stopped);
+    bp.parse(program_running);
+    bp.parse(program_paused);
+    bp.parse(robot_mode);
+    bp.parse(speed_fraction);
+
+    return true;
+}
+
+
+bool RobotModeData_V3_0__1::parse_with(BinParser &bp) {
+    if(!bp.check_size<RobotModeData_V3_0__1>())
+        return false;
+
+    SharedRobotModeData::parse_with(bp);
+    
+    bp.parse(protective_stopped);
+    bp.parse(program_running);
+    bp.parse(program_paused);
+    bp.parse(robot_mode);
+    bp.parse(control_mode);
+    bp.parse(target_speed_fraction);
+    bp.parse(speed_scaling);
+
+    return true;
+}
+
+bool RobotModeData_V3_2::parse_with(BinParser &bp) {
+    if(!bp.check_size<RobotModeData_V3_2>())
+        return false;
+
+    RobotModeData_V3_0__1::parse_with(bp);
+
+    bp.parse(target_speed_fraction_limit);
+
+    return true;
+}

src/ur/rt_state.cpp

@@ -0,0 +1,37 @@
+#include "ur_modern_driver/ur/rt_state.h"
+
+bool RTState_V1_6__7::parse_with(BinParser &bp) {
+    if(!bp.check_size<RTState_V1_6__7>())
+        return false;
+
+    bp.parse(time);
+    bp.parse(q_target);
+    bp.parse(qd_target);
+    bp.parse(qdd_target);
+    bp.parse(i_target);
+    bp.parse(m_target);
+    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);
+    bp.parse(digital_input);
+    bp.parse(motor_temperatures);
+    bp.parse(controller_time);
+    bp.parse(robot_mode);
+
+    return true;
+}
+
+bool RTState_V1_8::parse_with(BinParser &bp) {
+    if(!bp.check_size<RTState_V1_8>())
+        return false;
+
+    RTState_V1_6__7::parse_with(bp);
+
+    bp.parse(joint_modes);
+
+    return true;
+}

src/ur/state.cpp

@@ -0,0 +1,73 @@
+#include "ur_modern_driver/log.h"
+#include "ur_modern_driver/ur/state.h"
+
+
+bool RobotState::parse_with(BinParser &bp) {
+    //continue as long as there are bytes to read
+    while(bp.check_size(sizeof(uint8_t))) {
+        if(!bp.check_size(sizeof(uint32_t))){
+            LOG_ERROR("Failed to read sub-package length, there's likely a parsing error\n");
+            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!\n", sub_size);
+            return false;
+        }
+        
+        //deconstruction of a sub parser will increment the position of the parent parser 
+        BinParser sub_parser(bp, sub_size);
+        sub_parser.parse(sub_size);
+
+        if(!parse_package(sub_parser))
+            return false;
+    }
+    return true;
+}
+
+bool RobotState_V1_6__7::parse_package(BinParser &bp) {
+    //todo: size checks
+    package_type type;
+    bp.parse(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
+            break;
+        default:
+            LOG_ERROR("Invalid package type parsed: %" PRIu8 "\n", 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;
+}