Implemented initial robot state and parsing

include/ur_modern_driver/packet.h

@@ -0,0 +1,7 @@
+#pragma once
+#include "ur_modern_driver/bin_parser.h"
+
+class Packet {
+public:
+    virtual bool parse_with(BinParser &bp) = 0;
+};

include/ur_modern_driver/ur/master_board.h

@@ -0,0 +1,84 @@
+#pragma once
+
+#include <cstddef>
+#include <inttypes.h>
+#include "ur_modern_driver/types.h"
+#include "ur_modern_driver/bin_parser.h"
+
+
+class SharedMasterBoardData {
+public:
+    virtual bool parse_with(BinParser &bp);
+    
+    int8_t analog_input_range0; 
+    int8_t analog_input_range1;
+    double analog_input0; 
+    double analog_input1;
+    int8_t analog_output_domain0; 
+    int8_t analog_output_domain1;
+    double analog_output0; 
+    double analog_output1;
+    float master_board_temperature;
+    float robot_voltage_48V;
+    float robot_current;
+    float master_IO_current;
+
+    bool euromap67_interface_installed;
+
+    //euromap fields are dynamic so don't include in SIZE
+    int32_t euromap_input_bits;
+    int32_t euromap_output_bits;
+
+    static const size_t SIZE = sizeof(double) * 4 
+        + sizeof(int8_t) * 4
+        + sizeof(float) * 4
+        + sizeof(uint8_t);
+
+    static const size_t EURO_SIZE = sizeof(int32_t) * 2;
+};
+
+class MasterBoardData_V1_X : public SharedMasterBoardData {
+public:
+    virtual bool parse_with(BinParser &bp);
+
+    int16_t digital_input_bits;
+    int16_t digital_output_bits;
+
+    uint8_t master_safety_state;
+    bool master_on_off_state;
+
+    //euromap fields are dynamic so don't include in SIZE
+    int16_t euromap_voltage;
+    int16_t euromap_current;
+
+
+    static const size_t SIZE = SharedMasterBoardData::SIZE
+        + sizeof(int16_t) * 2
+        + sizeof(uint8_t) * 2;
+
+    static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE
+        + sizeof(int16_t) * 2;
+};
+
+class MasterBoardData_V3_X : public SharedMasterBoardData {
+public:
+    virtual bool parse_with(BinParser &bp);
+
+    int32_t digital_input_bits;
+    int32_t digital_output_bits;
+
+    uint8_t safety_mode;
+    bool in_reduced_mode;
+
+    //euromap fields are dynamic so don't include in SIZE
+    float euromap_voltage;
+    float euromap_current;
+
+
+    static const size_t SIZE = SharedMasterBoardData::SIZE
+        + sizeof(int32_t) * 2
+        + sizeof(uint8_t) * 2;
+
+    static const size_t EURO_SIZE = SharedMasterBoardData::EURO_SIZE
+        + sizeof(float) * 2;
+};

include/ur_modern_driver/ur/messages.h

@@ -0,0 +1,20 @@
+#pragma once
+
+class MessageBase {
+public:
+    virtual bool parse_with(BinParser &bp) = 0;
+
+    uint64_t timestamp;
+    uint8_t source;
+};
+
+class VersionMessage : public MessageBase {
+public:
+    bool parse_with(BinParser &bp);
+
+    std::string project_name;
+    uint8_t major_version;
+    uint8_t minor_version;
+    int32_t svn_version;
+    std::string build_date;
+}

include/ur_modern_driver/ur/robot_mode.h

@@ -0,0 +1,104 @@
+#pragma once
+
+#include <cstddef>
+#include <inttypes.h>
+#include "ur_modern_driver/types.h"
+#include "ur_modern_driver/bin_parser.h"
+
+class SharedRobotModeData {
+public:
+    bool parse_with(BinParser &bp);
+
+    uint64_t timestamp;
+    bool physical_robot_connected;
+    bool real_robot_enabled;
+    bool robot_power_on;
+    bool emergency_stopped;
+    bool program_running;
+    bool program_paused;
+
+    static const size_t SIZE = sizeof(uint64_t) + sizeof(uint8_t) * 6;
+};
+
+enum class robot_mode_V1_X : uint8_t {
+    ROBOT_RUNNING_MODE = 0,
+	ROBOT_FREEDRIVE_MODE = 1,
+	ROBOT_READY_MODE = 2,
+	ROBOT_INITIALIZING_MODE = 3,
+	ROBOT_SECURITY_STOPPED_MODE = 4,
+	ROBOT_EMERGENCY_STOPPED_MODE = 5,
+	ROBOT_FATAL_ERROR_MODE = 6,
+	ROBOT_NO_POWER_MODE = 7,
+	ROBOT_NOT_CONNECTED_MODE = 8,
+	ROBOT_SHUTDOWN_MODE = 9,
+	ROBOT_SAFEGUARD_STOP_MODE = 10
+};
+
+class RobotModeData_V1_X : public SharedRobotModeData {
+public:
+    bool parse_with(BinParser &bp);
+
+    bool security_stopped;
+    robot_mode_V1_X robot_mode;
+    double speed_fraction;
+
+    static const size_t SIZE = SharedRobotModeData::SIZE 
+        + sizeof(uint8_t)
+        + sizeof(robot_mode_V1_X) 
+        + sizeof(double);
+
+    static_assert(RobotModeData_V1_X::SIZE == 24, "RobotModeData_V1_X has missmatched size");
+};
+
+enum class robot_mode_V3_X : uint8_t {
+    DISCONNECTED = 0,
+	CONFIRM_SAFETY = 1,
+	BOOTING = 2,
+	POWER_OFF = 3,
+	POWER_ON = 4,
+	IDLE = 5,
+	BACKDRIVE = 6,
+	RUNNING = 7,
+	UPDATING_FIRMWARE = 8
+};
+
+enum class robot_control_mode_V3_X : uint8_t {
+    POSITION = 0,
+    TEACH = 1,
+    FORCE = 2,
+    TORQUE = 3
+};
+
+class RobotModeData_V3_0__1 : public SharedRobotModeData {
+public:
+    bool parse_with(BinParser &bp);
+
+    bool protective_stopped;
+
+    robot_mode_V3_X robot_mode;
+    robot_control_mode_V3_X control_mode;
+
+    double target_speed_fraction;
+    double speed_scaling;
+    
+    static const size_t SIZE = SharedRobotModeData::SIZE 
+        + sizeof(uint8_t) 
+        + sizeof(robot_mode_V3_X) 
+        + sizeof(robot_control_mode_V3_X)
+        + sizeof(double)
+        + sizeof(double);
+
+    static_assert(RobotModeData_V3_0__1::SIZE == 33, "RobotModeData_V3_0__1 has missmatched size");
+};
+
+class RobotModeData_V3_2 : public RobotModeData_V3_0__1 {
+public:
+    bool parse_with(BinParser &bp);
+
+    double target_speed_fraction_limit;
+
+    static const size_t SIZE = RobotModeData_V3_0__1::SIZE
+        + sizeof(double);
+
+    static_assert(RobotModeData_V3_2::SIZE == 41, "RobotModeData_V3_2 has missmatched size");        
+};

include/ur_modern_driver/ur/rt_state.h

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <cstddef>
+#include <inttypes.h>
+#include "ur_modern_driver/types.h"
+#include "ur_modern_driver/bin_parser.h"
+
+class RTState_V1_6__7 {
+public:
+    bool parse_with(BinParser &bp);
+
+    double time;
+    double q_target[6];
+    double qd_target[6];
+    double qdd_target[6];
+    double i_target[6];
+    double m_target[6];
+    double q_actual[6];
+    double qd_actual[6];
+    double i_actual[6];
+    double3_t tool_accelerometer_values;
+    double tcp_force[6];
+    cartesian_coord_t tool_vector;
+    double tcp_speed[6];
+    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(uint64_t);
+
+
+    static_assert(RTState_V1_6__7::SIZE == 632, "RTState_V1_6__7 has mismatched size!");
+};
+
+class RTState_V1_8 : public RTState_V1_6__7 {
+public:
+    bool parse_with(BinParser &bp);
+
+    double joint_modes[6];
+
+    static const size_t SIZE = RTState_V1_6__7::SIZE
+        + sizeof(double[6]);
+
+    static_assert(RTState_V1_8::SIZE == 680, "RTState_V1_8 has mismatched size!");
+};
+
+class RTState_V3_0__1 {
+public:
+    //bool parse_with(BinParser &bp);
+
+    double m_actual[6];
+    double i_control[6];
+};

include/ur_modern_driver/ur/state.h

@@ -0,0 +1,59 @@
+#pragma once
+#include <cstddef>
+#include <inttypes.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"
+
+
+
+enum class package_type : uint8_t {
+	ROBOT_MODE_DATA = 0,
+	JOINT_DATA = 1,
+	TOOL_DATA = 2,
+	MASTERBOARD_DATA = 3,
+	CARTESIAN_INFO = 4,
+	KINEMATICS_INFO = 5,
+	CONFIGURATION_DATA = 6,
+	FORCE_MODE_DATA = 7,
+	ADDITIONAL_INFO = 8,
+	CALIBRATION_DATA = 9
+};
+
+enum class message_type : uint8_t {
+	ROBOT_STATE = 16, 
+    ROBOT_MESSAGE = 20, 
+    PROGRAM_STATE_MESSAGE = 25
+};
+
+
+class RobotState : public Packet {
+public:
+    bool parse_with(BinParser &bp);
+protected:
+    virtual bool parse_package(BinParser &bp) = 0;
+};
+
+class RobotState_V1_6__7 : public RobotState {
+protected:
+    virtual bool parse_package(BinParser &bp) = 0;
+public:
+    RobotModeData_V1_X robot_mode;
+    //JointData
+    //ToolData
+    MasterBoardData_V1_X master_board;
+    //CartesianInfo
+};
+
+class RobotState_V1_8 : public RobotState_V1_6__7 {
+protected:
+    virtual bool parse_package(BinParser &bp) = 0;
+public:
+    
+    //KinematicsInfo
+    //ConfigurationData
+    //ForceModeData
+    //AdditionalInfo
+    //CalibrationData
+};

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;
+}