From c322bcb2f2ce82631e16786bc0b5429f6085216e Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 11 Apr 2019 14:39:47 +0200 Subject: [PATCH] implemented usage of recipe read by rtde client in the parser and the rtde data package --- include/ur_rtde_driver/rtde/data_package.h | 1 + include/ur_rtde_driver/rtde/package_header.h | 4 +- include/ur_rtde_driver/rtde/rtde_package.h | 2 + include/ur_rtde_driver/rtde/rtde_parser.h | 38 ++- src/rtde/control_package_pause.cpp | 2 +- src/rtde/control_package_setup_inputs.cpp | 2 +- src/rtde/control_package_setup_outputs.cpp | 2 +- src/rtde/control_package_start.cpp | 2 +- src/rtde/data_package.cpp | 312 +++++++++---------- src/rtde/rtde_client.cpp | 15 +- src/rtde/rtde_package.cpp | 10 +- src/rtde/text_message.cpp | 2 +- 12 files changed, 210 insertions(+), 182 deletions(-) diff --git a/include/ur_rtde_driver/rtde/data_package.h b/include/ur_rtde_driver/rtde/data_package.h index 5b9cfeb..636b80a 100644 --- a/include/ur_rtde_driver/rtde/data_package.h +++ b/include/ur_rtde_driver/rtde/data_package.h @@ -100,6 +100,7 @@ public: private: // Const would be better here static std::unordered_map type_list_; + uint8_t recipe_id_; std::unordered_map data_; std::vector recipe_; }; diff --git a/include/ur_rtde_driver/rtde/package_header.h b/include/ur_rtde_driver/rtde/package_header.h index 31c52a2..7f5ee08 100644 --- a/include/ur_rtde_driver/rtde/package_header.h +++ b/include/ur_rtde_driver/rtde/package_header.h @@ -25,7 +25,7 @@ namespace ur_driver { namespace rtde_interface { -enum class PackageType +enum class PackageType : uint8_t { RTDE_REQUEST_PROTOCOL_VERSION = 86, // ascii V RTDE_GET_URCONTROL_VERSION = 118, // ascii v @@ -55,7 +55,7 @@ public: static size_t serializeHeader(uint8_t* buffer, PackageType package_type, uint16_t payload_length) { uint16_t header_size = sizeof(_package_size_type) + sizeof(PackageType); - uint16_t size = header_size+payload_length; + uint16_t size = header_size + payload_length; comm::PackageSerializer::serialize(buffer, size); comm::PackageSerializer::serialize(buffer + sizeof(size), package_type); return header_size; diff --git a/include/ur_rtde_driver/rtde/rtde_package.h b/include/ur_rtde_driver/rtde/rtde_package.h index 10e0bd4..b294a57 100644 --- a/include/ur_rtde_driver/rtde/rtde_package.h +++ b/include/ur_rtde_driver/rtde/rtde_package.h @@ -55,6 +55,8 @@ public: virtual std::string toString() const; protected: + std::unique_ptr buffer_; + size_t buffer_length_; PackageType type_; }; diff --git a/include/ur_rtde_driver/rtde/rtde_parser.h b/include/ur_rtde_driver/rtde/rtde_parser.h index 67cf686..4241e2d 100644 --- a/include/ur_rtde_driver/rtde/rtde_parser.h +++ b/include/ur_rtde_driver/rtde/rtde_parser.h @@ -22,21 +22,27 @@ #include "ur_rtde_driver/comm/bin_parser.h" #include "ur_rtde_driver/comm/pipeline.h" -#include "ur_rtde_driver/rtde/package_header.h" +#include "ur_rtde_driver/rtde/control_package_pause.h" +#include "ur_rtde_driver/rtde/control_package_setup_inputs.h" +#include "ur_rtde_driver/rtde/control_package_setup_outputs.h" +#include "ur_rtde_driver/rtde/control_package_start.h" #include "ur_rtde_driver/rtde/data_package.h" -#include "ur_rtde_driver/rtde/text_message.h" -#include "ur_rtde_driver/rtde/request_protocol_version.h" #include "ur_rtde_driver/rtde/get_urcontrol_version.h" +#include "ur_rtde_driver/rtde/package_header.h" +#include "ur_rtde_driver/rtde/request_protocol_version.h" +#include "ur_rtde_driver/rtde/text_message.h" namespace ur_driver { namespace rtde_interface { -template -class RTDEParser : comm::Parser +class RTDEParser : public comm::Parser { public: - RTDEParser() = default; + RTDEParser() = delete; + RTDEParser(const std::vector& recipe) : recipe_(recipe) + { + } virtual ~RTDEParser() = default; bool parse(comm::BinParser& bp, std::vector>>& results) @@ -47,7 +53,7 @@ public: bp.parse(size); bp.parse(type); - if (!bp.checkSize(size)) + if (!bp.checkSize(size - sizeof(size) - sizeof(type))) { LOG_ERROR("Buffer len shorter than expected packet length"); return false; @@ -57,12 +63,11 @@ public: { case PackageType::RTDE_DATA_PACKAGE: { - // TODO: get proper recipe here - std::vector recipe; - std::unique_ptr package(new DataPackage(recipe)); + std::unique_ptr package(new DataPackage(recipe_)); if (!package->parseWith(bp)) { + LOG_ERROR("Package parsing of type %d failed!", static_cast(type)); return false; } results.push_back(std::move(package)); @@ -92,6 +97,7 @@ public: } private: + std::vector recipe_; RTDEPackage* package_from_type(PackageType type) { switch (type) @@ -105,6 +111,18 @@ private: case PackageType::RTDE_REQUEST_PROTOCOL_VERSION: return new RequestProtocolVersion; break; + case PackageType::RTDE_CONTROL_PACKAGE_PAUSE: + return new ControlPackagePause; + break; + case PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS: + return new ControlPackageSetupInputs; + break; + case PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: + return new ControlPackageSetupOutputs; + break; + case PackageType::RTDE_CONTROL_PACKAGE_START: + return new ControlPackageStart; + break; default: return new RTDEPackage(type); } diff --git a/src/rtde/control_package_pause.cpp b/src/rtde/control_package_pause.cpp index 1c05a68..68888d7 100644 --- a/src/rtde/control_package_pause.cpp +++ b/src/rtde/control_package_pause.cpp @@ -40,7 +40,7 @@ bool ControlPackagePause::parseWith(comm::BinParser& bp) std::string ControlPackagePause::toString() const { std::stringstream ss; - ss << "accepted: " << accepted_; + ss << "accepted: " << static_cast(accepted_); return ss.str(); } diff --git a/src/rtde/control_package_setup_inputs.cpp b/src/rtde/control_package_setup_inputs.cpp index 6561f3e..74966b4 100644 --- a/src/rtde/control_package_setup_inputs.cpp +++ b/src/rtde/control_package_setup_inputs.cpp @@ -41,7 +41,7 @@ bool ControlPackageSetupInputs::parseWith(comm::BinParser& bp) std::string ControlPackageSetupInputs::toString() const { std::stringstream ss; - ss << "input recipe id: " << input_recipe_id_ << std::endl; + ss << "input recipe id: " << static_cast(input_recipe_id_) << std::endl; ss << "variable types: " << variable_types_; return ss.str(); diff --git a/src/rtde/control_package_setup_outputs.cpp b/src/rtde/control_package_setup_outputs.cpp index 2b6f1ba..2ad6f06 100644 --- a/src/rtde/control_package_setup_outputs.cpp +++ b/src/rtde/control_package_setup_outputs.cpp @@ -41,7 +41,7 @@ bool ControlPackageSetupOutputs::parseWith(comm::BinParser& bp) std::string ControlPackageSetupOutputs::toString() const { std::stringstream ss; - ss << "output recipe id: " << output_recipe_id_ << std::endl; + ss << "output recipe id: " << static_cast(output_recipe_id_) << std::endl; ss << "variable types: " << variable_types_; return ss.str(); diff --git a/src/rtde/control_package_start.cpp b/src/rtde/control_package_start.cpp index 536412b..f80cd85 100644 --- a/src/rtde/control_package_start.cpp +++ b/src/rtde/control_package_start.cpp @@ -40,7 +40,7 @@ bool ControlPackageStart::parseWith(comm::BinParser& bp) std::string ControlPackageStart::toString() const { std::stringstream ss; - ss << "accepted: " << accepted_; + ss << "accepted: " << static_cast(accepted_); return ss.str(); } diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index 1243a5a..a6ce07e 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -31,165 +31,166 @@ namespace ur_driver namespace rtde_interface { std::unordered_map DataPackage::type_list_{ - { "timestamp_", double() }, - { "target_q_", vector6d_t() }, - { "target_qd_", vector6d_t() }, - { "target_qdd_", vector6d_t() }, - { "target_current_", vector6d_t() }, - { "target_moment_", vector6d_t() }, - { "actual_q_", vector6d_t() }, - { "actual_qd_", vector6d_t() }, - { "actual_qdd_", vector6d_t() }, - { "actual_current_", vector6d_t() }, - { "actual_moment_", vector6d_t() }, - { "joint_control_output_", vector6d_t() }, - { "actual_TCP_pose_", vector6d_t() }, - { "actual_TCP_speed_", vector6d_t() }, - { "actual_TCP_force_", vector6d_t() }, - { "target_TCP_pose_", vector6d_t() }, - { "target_TCP_speed_", vector6d_t() }, - { "actual_digital_input_bits_", uint64_t() }, - { "joint_temperatures_", vector6d_t() }, - { "actual_execution_time_", double() }, - { "robot_mode_", int32_t() }, + { "timestamp", double() }, + { "target_q", vector6d_t() }, + { "target_qd", vector6d_t() }, + { "target_qdd", vector6d_t() }, + { "target_current", vector6d_t() }, + { "target_moment", vector6d_t() }, + { "actual_q", vector6d_t() }, + { "actual_qd", vector6d_t() }, + { "actual_qdd", vector6d_t() }, + { "actual_current", vector6d_t() }, + { "actual_moment", vector6d_t() }, + { "joint_control_output", vector6d_t() }, + { "actual_TCP_pose", vector6d_t() }, + { "actual_TCP_speed", vector6d_t() }, + { "actual_TCP_force", vector6d_t() }, + { "target_TCP_pose", vector6d_t() }, + { "target_TCP_speed", vector6d_t() }, + { "actual_digital_input_bits", uint64_t() }, + { "joint_temperatures", vector6d_t() }, + { "actual_execution_time", double() }, + { "robot_mode", int32_t() }, { "joint_mode", vector6int32_t() }, - { "safety_mode_", int32_t() }, + { "safety_mode", int32_t() }, { "actual_tool_accelerometer", vector3d_t() }, - { "speed_scaling_", double() }, - { "target_speed_fraction_", double() }, - { "actual_momentum_", double() }, - { "actial_main_voltage_", double() }, - { "actual_robot_voltage_", double() }, - { "actual_robot_current_", double() }, - { "actual_joint_voltage_", vector6d_t() }, - { "actual_digital_output_bits_", uint64_t() }, - { "runtime_state_", uint32_t() }, - { "elbow_position_", vector3d_t() }, - { "elbow_velocity_", vector3d_t() }, - { "robot_status_bits_", uint32_t() }, - { "safety_status_bits_", uint32_t() }, - { "analog_io_types_", uint32_t() }, - { "standard_analog_input0_", double() }, - { "standard_analog_input1_", double() }, - { "standard_analog_output0_", double() }, - { "standard_analog_output1_", double() }, - { "io_current_", double() }, - { "euromap67_input_bits_", uint32_t() }, - { "euromap67_output_bits_", uint32_t() }, - { "euromap67_24V_voltage_", double() }, - { "euromap67_24V_current_", double() }, - { "tool_mode_", uint32_t() }, - { "tool_analog_input_types_", uint32_t() }, - { "tool_analog_input0_", double() }, - { "tool_analog_input1_", double() }, - { "tool_output_voltage_", int32_t() }, - { "tool_output_current_", double() }, - { "tool_temperature_", double() }, - { "tool_force_scalar_", double() }, - { "output_bit_registers0_to_31_", uint32_t() }, - { "output_bit_registers32_to_63_", uint32_t() }, - { "output_int_register_0_", int32_t() }, - { "output_int_register_1_", int32_t() }, - { "output_int_register_2_", int32_t() }, - { "output_int_register_3_", int32_t() }, - { "output_int_register_4_", int32_t() }, - { "output_int_register_5_", int32_t() }, - { "output_int_register_6_", int32_t() }, - { "output_int_register_7_", int32_t() }, - { "output_int_register_8_", int32_t() }, - { "output_int_register_9_", int32_t() }, - { "output_int_register_10_", int32_t() }, - { "output_int_register_11_", int32_t() }, - { "output_int_register_12_", int32_t() }, - { "output_int_register_13_", int32_t() }, - { "output_int_register_14_", int32_t() }, - { "output_int_register_15_", int32_t() }, - { "output_int_register_16_", int32_t() }, - { "output_int_register_17_", int32_t() }, - { "output_int_register_18_", int32_t() }, - { "output_int_register_19_", int32_t() }, - { "output_int_register_20_", int32_t() }, - { "output_int_register_21_", int32_t() }, - { "output_int_register_22_", int32_t() }, - { "output_int_register_23_", int32_t() }, - { "output_double_register_0_", double() }, - { "output_double_register_1_", double() }, - { "output_double_register_2_", double() }, - { "output_double_register_3_", double() }, - { "output_double_register_4_", double() }, - { "output_double_register_5_", double() }, - { "output_double_register_6_", double() }, - { "output_double_register_7_", double() }, - { "output_double_register_8_", double() }, - { "output_double_register_9_", double() }, - { "output_double_register_10_", double() }, - { "output_double_register_11_", double() }, - { "output_double_register_12_", double() }, - { "output_double_register_13_", double() }, - { "output_double_register_14_", double() }, - { "output_double_register_15_", double() }, - { "output_double_register_16_", double() }, - { "output_double_register_17_", double() }, - { "output_double_register_18_", double() }, - { "output_double_register_19_", double() }, - { "output_double_register_20_", double() }, - { "output_double_register_21_", double() }, - { "output_double_register_22_", double() }, - { "output_double_register_23_", double() }, - { "input_bit_registers0_to_31_", uint32_t() }, - { "input_bit_registers32_to_63_", uint32_t() }, - { "input_int_register_0_", int32_t() }, - { "input_int_register_1_", int32_t() }, - { "input_int_register_2_", int32_t() }, - { "input_int_register_3_", int32_t() }, - { "input_int_register_4_", int32_t() }, - { "input_int_register_5_", int32_t() }, - { "input_int_register_6_", int32_t() }, - { "input_int_register_7_", int32_t() }, - { "input_int_register_8_", int32_t() }, - { "input_int_register_9_", int32_t() }, - { "input_int_register_10_", int32_t() }, - { "input_int_register_11_", int32_t() }, - { "input_int_register_12_", int32_t() }, - { "input_int_register_13_", int32_t() }, - { "input_int_register_14_", int32_t() }, - { "input_int_register_15_", int32_t() }, - { "input_int_register_16_", int32_t() }, - { "input_int_register_17_", int32_t() }, - { "input_int_register_18_", int32_t() }, - { "input_int_register_19_", int32_t() }, - { "input_int_register_20_", int32_t() }, - { "input_int_register_21_", int32_t() }, - { "input_int_register_22_", int32_t() }, - { "input_int_register_23_", int32_t() }, - { "input_double_register_0_", double() }, - { "input_double_register_1_", double() }, - { "input_double_register_2_", double() }, - { "input_double_register_3_", double() }, - { "input_double_register_4_", double() }, - { "input_double_register_5_", double() }, - { "input_double_register_6_", double() }, - { "input_double_register_7_", double() }, - { "input_double_register_8_", double() }, - { "input_double_register_9_", double() }, - { "input_double_register_10_", double() }, - { "input_double_register_11_", double() }, - { "input_double_register_12_", double() }, - { "input_double_register_13_", double() }, - { "input_double_register_14_", double() }, - { "input_double_register_15_", double() }, - { "input_double_register_16_", double() }, - { "input_double_register_17_", double() }, - { "input_double_register_18_", double() }, - { "input_double_register_19_", double() }, - { "input_double_register_20_", double() }, - { "input_double_register_21_", double() }, - { "input_double_register_22_", double() }, - { "input_double_register_23_", double() } + { "speed_scaling", double() }, + { "target_speed_fraction", double() }, + { "actual_momentum", double() }, + { "actial_main_voltage", double() }, + { "actual_robot_voltage", double() }, + { "actual_robot_current", double() }, + { "actual_joint_voltage", vector6d_t() }, + { "actual_digital_output_bits", uint64_t() }, + { "runtime_state", uint32_t() }, + { "elbow_position", vector3d_t() }, + { "elbow_velocity", vector3d_t() }, + { "robot_status_bits", uint32_t() }, + { "safety_status_bits", uint32_t() }, + { "analog_io_types", uint32_t() }, + { "standard_analog_input0", double() }, + { "standard_analog_input1", double() }, + { "standard_analog_output0", double() }, + { "standard_analog_output1", double() }, + { "io_current", double() }, + { "euromap67_input_bits", uint32_t() }, + { "euromap67_output_bits", uint32_t() }, + { "euromap67_24V_voltage", double() }, + { "euromap67_24V_current", double() }, + { "tool_mode", uint32_t() }, + { "tool_analog_input_types", uint32_t() }, + { "tool_analog_input0", double() }, + { "tool_analog_input1", double() }, + { "tool_output_voltage", int32_t() }, + { "tool_output_current", double() }, + { "tool_temperature", double() }, + { "tool_force_scalar", double() }, + { "output_bit_registers0_to_31", uint32_t() }, + { "output_bit_registers32_to_63", uint32_t() }, + { "output_int_register_0", int32_t() }, + { "output_int_register_1", int32_t() }, + { "output_int_register_2", int32_t() }, + { "output_int_register_3", int32_t() }, + { "output_int_register_4", int32_t() }, + { "output_int_register_5", int32_t() }, + { "output_int_register_6", int32_t() }, + { "output_int_register_7", int32_t() }, + { "output_int_register_8", int32_t() }, + { "output_int_register_9", int32_t() }, + { "output_int_register_10", int32_t() }, + { "output_int_register_11", int32_t() }, + { "output_int_register_12", int32_t() }, + { "output_int_register_13", int32_t() }, + { "output_int_register_14", int32_t() }, + { "output_int_register_15", int32_t() }, + { "output_int_register_16", int32_t() }, + { "output_int_register_17", int32_t() }, + { "output_int_register_18", int32_t() }, + { "output_int_register_19", int32_t() }, + { "output_int_register_20", int32_t() }, + { "output_int_register_21", int32_t() }, + { "output_int_register_22", int32_t() }, + { "output_int_register_23", int32_t() }, + { "output_double_register_0", double() }, + { "output_double_register_1", double() }, + { "output_double_register_2", double() }, + { "output_double_register_3", double() }, + { "output_double_register_4", double() }, + { "output_double_register_5", double() }, + { "output_double_register_6", double() }, + { "output_double_register_7", double() }, + { "output_double_register_8", double() }, + { "output_double_register_9", double() }, + { "output_double_register_10", double() }, + { "output_double_register_11", double() }, + { "output_double_register_12", double() }, + { "output_double_register_13", double() }, + { "output_double_register_14", double() }, + { "output_double_register_15", double() }, + { "output_double_register_16", double() }, + { "output_double_register_17", double() }, + { "output_double_register_18", double() }, + { "output_double_register_19", double() }, + { "output_double_register_20", double() }, + { "output_double_register_21", double() }, + { "output_double_register_22", double() }, + { "output_double_register_23", double() }, + { "input_bit_registers0_to_31", uint32_t() }, + { "input_bit_registers32_to_63", uint32_t() }, + { "input_int_register_0", int32_t() }, + { "input_int_register_1", int32_t() }, + { "input_int_register_2", int32_t() }, + { "input_int_register_3", int32_t() }, + { "input_int_register_4", int32_t() }, + { "input_int_register_5", int32_t() }, + { "input_int_register_6", int32_t() }, + { "input_int_register_7", int32_t() }, + { "input_int_register_8", int32_t() }, + { "input_int_register_9", int32_t() }, + { "input_int_register_10", int32_t() }, + { "input_int_register_11", int32_t() }, + { "input_int_register_12", int32_t() }, + { "input_int_register_13", int32_t() }, + { "input_int_register_14", int32_t() }, + { "input_int_register_15", int32_t() }, + { "input_int_register_16", int32_t() }, + { "input_int_register_17", int32_t() }, + { "input_int_register_18", int32_t() }, + { "input_int_register_19", int32_t() }, + { "input_int_register_20", int32_t() }, + { "input_int_register_21", int32_t() }, + { "input_int_register_22", int32_t() }, + { "input_int_register_23", int32_t() }, + { "input_double_register_0", double() }, + { "input_double_register_1", double() }, + { "input_double_register_2", double() }, + { "input_double_register_3", double() }, + { "input_double_register_4", double() }, + { "input_double_register_5", double() }, + { "input_double_register_6", double() }, + { "input_double_register_7", double() }, + { "input_double_register_8", double() }, + { "input_double_register_9", double() }, + { "input_double_register_10", double() }, + { "input_double_register_11", double() }, + { "input_double_register_12", double() }, + { "input_double_register_13", double() }, + { "input_double_register_14", double() }, + { "input_double_register_15", double() }, + { "input_double_register_16", double() }, + { "input_double_register_17", double() }, + { "input_double_register_18", double() }, + { "input_double_register_19", double() }, + { "input_double_register_20", double() }, + { "input_double_register_21", double() }, + { "input_double_register_22", double() }, + { "input_double_register_23", double() } }; bool rtde_interface::DataPackage ::parseWith(comm::BinParser& bp) { + bp.parse(recipe_id_); for (auto& item : recipe_) { if (type_list_.find(item) != type_list_.end()) @@ -210,13 +211,10 @@ bool rtde_interface::DataPackage ::parseWith(comm::BinParser& bp) std::string rtde_interface::DataPackage::toString() const { std::stringstream ss; - vector3d_t vec1; - vector6uint32_t vec2; - ss << vec1 << vec2; for (auto& item : data_) { ss << item.first << ": "; - ss << boost::apply_visitor(StringVisitor{}, item.second); + ss << boost::apply_visitor(StringVisitor{}, item.second) << std::endl; } return ss.str(); } diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 8cd8109..a0c5f97 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -32,8 +32,10 @@ namespace ur_driver namespace rtde_interface { RTDEClient::RTDEClient(std::string ROBOT_IP, comm::INotifier& notifier) - : stream_(ROBOT_IP, UR_RTDE_PORT), parser_(), prod_(stream_, parser_), - pipeline_(prod_, PIPELINE_NAME, notifier) + : stream_(ROBOT_IP, UR_RTDE_PORT) + , parser_(readRecipe()) + , prod_(stream_, parser_) + , pipeline_(prod_, PIPELINE_NAME, notifier) { } @@ -73,11 +75,10 @@ std::vector RTDEClient::readRecipe() return recipe; } -bool RTDEClient::getDataPackage( - std::unique_ptr>& data_package, - std::chrono::milliseconds timeout) +bool RTDEClient::getDataPackage(std::unique_ptr>& data_package, + std::chrono::milliseconds timeout) { return pipeline_.getLatestProduct(data_package, timeout); } -} // namespace rtde_interface -} // namespace ur_driver +} // namespace rtde_interface +} // namespace ur_driver diff --git a/src/rtde/rtde_package.cpp b/src/rtde/rtde_package.cpp index 13e8231..06dfa7a 100644 --- a/src/rtde/rtde_package.cpp +++ b/src/rtde/rtde_package.cpp @@ -32,13 +32,21 @@ namespace rtde_interface { bool RTDEPackage::parseWith(comm::BinParser& bp) { + bp.rawData(buffer_, buffer_length_); return true; } std::string rtde_interface::RTDEPackage ::toString() const { std::stringstream ss; - ss << "Type: " << static_cast(type_); + ss << "Type: " << static_cast(type_) << std::endl; + ss << "Raw byte stream: "; + for (size_t i = 0; i < buffer_length_; ++i) + { + uint8_t* buf = buffer_.get(); + ss << std::hex << static_cast(buf[i]) << " "; + } + ss << std::endl; return ss.str(); } } // namespace rtde_interface diff --git a/src/rtde/text_message.cpp b/src/rtde/text_message.cpp index 1a35fef..925c9bc 100644 --- a/src/rtde/text_message.cpp +++ b/src/rtde/text_message.cpp @@ -46,7 +46,7 @@ std::string TextMessage::toString() const std::stringstream ss; ss << "message: " << message_ << std::endl; ss << "source: " << source_ << std::endl; - ss << "warning level: " << warning_level_; + ss << "warning level: " << static_cast(warning_level_); return ss.str(); }