mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 10:00:48 +02:00
Implemented parsing and toString for RTDE data package
This commit is contained in:
Felix Mauch
@@ -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
|
||||
|
||||
@@ -28,20 +28,56 @@
|
||||
#ifndef UR_RTDE_DRIVER_DATA_PACKAGE_H_INCLUDED
|
||||
#define UR_RTDE_DRIVER_DATA_PACKAGE_H_INCLUDED
|
||||
|
||||
#include <unordered_map>
|
||||
|
||||
#include "ur_rtde_driver/types.h"
|
||||
#include "ur_rtde_driver/rtde/rtde_package.h"
|
||||
#include <boost/variant.hpp>
|
||||
|
||||
namespace ur_driver
|
||||
{
|
||||
namespace rtde_interface
|
||||
{
|
||||
struct ParseVisitor : public boost::static_visitor<>
|
||||
{
|
||||
template <typename T>
|
||||
void operator()(T& d, comm::BinParser& bp) const
|
||||
{
|
||||
bp.parse(d);
|
||||
}
|
||||
};
|
||||
struct StringVisitor : public boost::static_visitor<std::string>
|
||||
{
|
||||
template <typename T>
|
||||
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<bool, uint8_t, uint32_t, uint64_t, int32_t, double, vector3d_t, vector6d_t,
|
||||
vector6int32_t, vector6uint32_t, std::string>;
|
||||
|
||||
DataPackage() = delete;
|
||||
DataPackage(const std::vector<std::string>& 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<std::string, _rtde_type_variant> type_list_;
|
||||
std::unordered_map<std::string, _rtde_type_variant> data_;
|
||||
std::vector<std::string> recipe_;
|
||||
};
|
||||
|
||||
} // namespace rtde_interface
|
||||
|
||||
@@ -20,6 +20,7 @@
|
||||
|
||||
#include <inttypes.h>
|
||||
#include <array>
|
||||
#include <iostream>
|
||||
|
||||
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 <class T, std::size_t N>
|
||||
std::ostream& operator<<(std::ostream& out, const std::array<T, N>& 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
|
||||
|
||||
@@ -18,6 +18,8 @@
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<build_depend>boost</build_depend>
|
||||
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>controller_manager</depend>
|
||||
<depend>actionlib</depend>
|
||||
|
||||
224
src/rtde/data_package.cpp
Normal file
224
src/rtde/data_package.cpp
Normal file
@@ -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<std::string, DataPackage::_rtde_type_variant> 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
|
||||
Reference in New Issue
Block a user