From 466fc03e31988bff262f4ffd28a9e6d0f3240849 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 10 Apr 2019 16:20:42 +0200 Subject: [PATCH] Implemented parsing and toString for RTDE data package --- CMakeLists.txt | 6 +- include/ur_rtde_driver/rtde/data_package.h | 44 +++- include/ur_rtde_driver/types.h | 17 ++ package.xml | 2 + src/rtde/data_package.cpp | 224 +++++++++++++++++++++ 5 files changed, 288 insertions(+), 5 deletions(-) create mode 100644 src/rtde/data_package.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 02e9c4f..725bcef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,7 +23,7 @@ find_package(catkin REQUIRED trajectory_msgs ur_msgs ) - +find_package(Boost REQUIRED) catkin_package( INCLUDE_DIRS @@ -41,6 +41,8 @@ catkin_package( sensor_msgs trajectory_msgs ur_msgs + DEPENDS + Boost ) @@ -64,6 +66,7 @@ add_compile_options(-Wno-unused-parameter) include_directories( include ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} ) add_library(ur_rtde_driver @@ -79,6 +82,7 @@ add_library(ur_rtde_driver src/primary/robot_message.cpp src/primary/robot_message/version_message.cpp src/primary/robot_state/kinematics_info.cpp + src/rtde/data_package.cpp src/rtde/request_protocol_version.cpp src/rtde/get_urcontrol_version.cpp src/rtde/text_message.cpp diff --git a/include/ur_rtde_driver/rtde/data_package.h b/include/ur_rtde_driver/rtde/data_package.h index 26e3663..a3b881e 100644 --- a/include/ur_rtde_driver/rtde/data_package.h +++ b/include/ur_rtde_driver/rtde/data_package.h @@ -28,20 +28,56 @@ #ifndef UR_RTDE_DRIVER_DATA_PACKAGE_H_INCLUDED #define UR_RTDE_DRIVER_DATA_PACKAGE_H_INCLUDED +#include + +#include "ur_rtde_driver/types.h" #include "ur_rtde_driver/rtde/rtde_package.h" +#include namespace ur_driver { namespace rtde_interface { +struct ParseVisitor : public boost::static_visitor<> +{ + template + void operator()(T& d, comm::BinParser& bp) const + { + bp.parse(d); + } +}; +struct StringVisitor : public boost::static_visitor +{ + template + std::string operator()(T& d) const + { + std::stringstream ss; + ss << d; + return ss.str(); + } +}; + + class DataPackage : public RTDEPackage { -private: - uint8_t recipe_id_; - public: - DataPackage() = default; + using _rtde_type_variant = boost::variant; + + DataPackage() = delete; + DataPackage(const std::vector& recipe) : recipe_(recipe) + { + } virtual ~DataPackage() = default; + + virtual bool parseWith(comm::BinParser& bp); + virtual std::string toString() const; + +private: + // Const would be better here + static std::unordered_map type_list_; + std::unordered_map data_; + std::vector recipe_; }; } // namespace rtde_interface diff --git a/include/ur_rtde_driver/types.h b/include/ur_rtde_driver/types.h index 9aaacfb..173ecb3 100644 --- a/include/ur_rtde_driver/types.h +++ b/include/ur_rtde_driver/types.h @@ -20,6 +20,7 @@ #include #include +#include namespace ur_driver { @@ -49,4 +50,20 @@ inline bool operator==(const cartesian_coord_t& lhs, const cartesian_coord_t& rh { return lhs.position == rhs.position && lhs.rotation == rhs.rotation; } + +template +std::ostream& operator<<(std::ostream& out, const std::array& item) +{ + out << "["; + for (size_t i = 0; i < item.size(); ++i) + { + out << item[i]; + if (i != item.size() - 1) + { + out << ", "; + } + } + out << "]"; + return out; +} } // namespace ur_driver diff --git a/package.xml b/package.xml index 2c24c36..f722e9b 100644 --- a/package.xml +++ b/package.xml @@ -18,6 +18,8 @@ catkin + boost + hardware_interface controller_manager actionlib diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp new file mode 100644 index 0000000..1243a5a --- /dev/null +++ b/src/rtde/data_package.cpp @@ -0,0 +1,224 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-04-10 + * + */ +//---------------------------------------------------------------------- + +#include "ur_rtde_driver/rtde/data_package.h" +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() }, + { "joint_mode", vector6int32_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() } +}; + +bool rtde_interface::DataPackage ::parseWith(comm::BinParser& bp) +{ + for (auto& item : recipe_) + { + if (type_list_.find(item) != type_list_.end()) + { + _rtde_type_variant entry = type_list_[item]; + auto bound_visitor = std::bind(ParseVisitor(), std::placeholders::_1, bp); + boost::apply_visitor(bound_visitor, entry); + data_[item] = entry; + } + else + { + return false; + } + } + return true; +} + +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); + } + return ss.str(); +} +} // namespace rtde_interface +} // namespace ur_driver