From ffe2bbe96a00c130a1327c0467042f98955b317f Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Mon, 6 Feb 2017 14:22:28 +0100 Subject: [PATCH] Implemented initial robot state and parsing --- include/ur_modern_driver/packet.h | 7 ++ include/ur_modern_driver/ur/master_board.h | 84 +++++++++++++++++ include/ur_modern_driver/ur/messages.h | 20 ++++ include/ur_modern_driver/ur/robot_mode.h | 104 +++++++++++++++++++++ include/ur_modern_driver/ur/rt_state.h | 58 ++++++++++++ include/ur_modern_driver/ur/state.h | 59 ++++++++++++ src/ur/master_board.cpp | 66 +++++++++++++ src/ur/messages.cpp | 22 +++++ src/ur/robot_mode.cpp | 56 +++++++++++ src/ur/rt_state.cpp | 37 ++++++++ src/ur/state.cpp | 73 +++++++++++++++ 11 files changed, 586 insertions(+) create mode 100644 include/ur_modern_driver/packet.h create mode 100644 include/ur_modern_driver/ur/master_board.h create mode 100644 include/ur_modern_driver/ur/messages.h create mode 100644 include/ur_modern_driver/ur/robot_mode.h create mode 100644 include/ur_modern_driver/ur/rt_state.h create mode 100644 include/ur_modern_driver/ur/state.h create mode 100644 src/ur/master_board.cpp create mode 100644 src/ur/messages.cpp create mode 100644 src/ur/robot_mode.cpp create mode 100644 src/ur/rt_state.cpp create mode 100644 src/ur/state.cpp diff --git a/include/ur_modern_driver/packet.h b/include/ur_modern_driver/packet.h new file mode 100644 index 0000000..7f316dc --- /dev/null +++ b/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; +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/master_board.h b/include/ur_modern_driver/ur/master_board.h new file mode 100644 index 0000000..e85b2dd --- /dev/null +++ b/include/ur_modern_driver/ur/master_board.h @@ -0,0 +1,84 @@ +#pragma once + +#include +#include +#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; +}; diff --git a/include/ur_modern_driver/ur/messages.h b/include/ur_modern_driver/ur/messages.h new file mode 100644 index 0000000..31ec391 --- /dev/null +++ b/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; +} \ No newline at end of file diff --git a/include/ur_modern_driver/ur/robot_mode.h b/include/ur_modern_driver/ur/robot_mode.h new file mode 100644 index 0000000..13b1952 --- /dev/null +++ b/include/ur_modern_driver/ur/robot_mode.h @@ -0,0 +1,104 @@ +#pragma once + +#include +#include +#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"); +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/rt_state.h b/include/ur_modern_driver/ur/rt_state.h new file mode 100644 index 0000000..bc56aa9 --- /dev/null +++ b/include/ur_modern_driver/ur/rt_state.h @@ -0,0 +1,58 @@ +#pragma once + +#include +#include +#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]; +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ur/state.h b/include/ur_modern_driver/ur/state.h new file mode 100644 index 0000000..f76e8a6 --- /dev/null +++ b/include/ur_modern_driver/ur/state.h @@ -0,0 +1,59 @@ +#pragma once +#include +#include +#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 +}; \ No newline at end of file diff --git a/src/ur/master_board.cpp b/src/ur/master_board.cpp new file mode 100644 index 0000000..084211a --- /dev/null +++ b/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()) + 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()) + 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; +} + diff --git a/src/ur/messages.cpp b/src/ur/messages.cpp new file mode 100644 index 0000000..0e7ceea --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/src/ur/robot_mode.cpp b/src/ur/robot_mode.cpp new file mode 100644 index 0000000..4fea01d --- /dev/null +++ b/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()) + 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()) + 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()) + return false; + + RobotModeData_V3_0__1::parse_with(bp); + + bp.parse(target_speed_fraction_limit); + + return true; +} diff --git a/src/ur/rt_state.cpp b/src/ur/rt_state.cpp new file mode 100644 index 0000000..aad336d --- /dev/null +++ b/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()) + 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()) + return false; + + RTState_V1_6__7::parse_with(bp); + + bp.parse(joint_modes); + + return true; +} \ No newline at end of file diff --git a/src/ur/state.cpp b/src/ur/state.cpp new file mode 100644 index 0000000..460bdf1 --- /dev/null +++ b/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(); + if(!bp.check_size(static_cast(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(); + 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; +} \ No newline at end of file