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