Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 01:50:46 +02:00
Code Releases Activity

implemented usage of recipe read by rtde client in the parser and the

Browse Source 
rtde data package
This commit is contained in:
Felix Mauch
2019-04-11 14:39:47 +02:00
committed by Tristan Schnell
parent 420e04530f
commit c322bcb2f2
12 changed files with 210 additions and 182 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
include/ur_rtde_driver/rtde/data_package.h
View File

@@ -100,6 +100,7 @@ public:
private:
// Const would be better here
static std::unordered_map<std::string, _rtde_type_variant> type_list_;
uint8_t recipe_id_;
std::unordered_map<std::string, _rtde_type_variant> data_;
std::vector<std::string> recipe_;
};

4
include/ur_rtde_driver/rtde/package_header.h
View File

@@ -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;

2
include/ur_rtde_driver/rtde/rtde_package.h
View File

@@ -55,6 +55,8 @@ public:
virtual std::string toString() const;
protected:
std::unique_ptr<uint8_t> buffer_;
size_t buffer_length_;
PackageType type_;
};

38
include/ur_rtde_driver/rtde/rtde_parser.h
View File

@@ -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 <typename T>
class RTDEParser : comm::Parser<rtde_interface::PackageHeader>
class RTDEParser : public comm::Parser<PackageHeader>
{
public:
RTDEParser() = default;
RTDEParser() = delete;
RTDEParser(const std::vector<std::string>& recipe) : recipe_(recipe)
{
}
virtual ~RTDEParser() = default;
bool parse(comm::BinParser& bp, std::vector<std::unique_ptr<comm::URPackage<PackageHeader>>>& 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<std::string> recipe;
std::unique_ptr<RTDEPackage> package(new DataPackage(recipe));
std::unique_ptr<RTDEPackage> package(new DataPackage(recipe_));
if (!package->parseWith(bp))
{
LOG_ERROR("Package parsing of type %d failed!", static_cast<int>(type));
return false;
}
results.push_back(std::move(package));
@@ -92,6 +97,7 @@ public:
}
private:
std::vector<std::string> 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);
}

2
src/rtde/control_package_pause.cpp
View File

@@ -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<int>(accepted_);
return ss.str();
}

2
src/rtde/control_package_setup_inputs.cpp
View File

@@ -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<int>(input_recipe_id_) << std::endl;
ss << "variable types: " << variable_types_;
return ss.str();

2
src/rtde/control_package_setup_outputs.cpp
View File

@@ -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<int>(output_recipe_id_) << std::endl;
ss << "variable types: " << variable_types_;
return ss.str();

2
src/rtde/control_package_start.cpp
View File

@@ -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<int>(accepted_);
return ss.str();
}

312
src/rtde/data_package.cpp
View File

@@ -31,165 +31,166 @@ 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() },
{ "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();
}

9
src/rtde/rtde_client.cpp
View File

@@ -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,8 +75,7 @@ std::vector<std::string> RTDEClient::readRecipe()
return recipe;
}
bool RTDEClient::getDataPackage(
std::unique_ptr<comm::URPackage<PackageHeader>>& data_package,
bool RTDEClient::getDataPackage(std::unique_ptr<comm::URPackage<PackageHeader>>& data_package,
std::chrono::milliseconds timeout)
{
return pipeline_.getLatestProduct(data_package, timeout);

10
src/rtde/rtde_package.cpp
View File

@@ -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<int>(type_);
ss << "Type: " << static_cast<int>(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<int>(buf[i]) << " ";
}
ss << std::endl;
return ss.str();
}
} // namespace rtde_interface

2
src/rtde/text_message.cpp
View File

@@ -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<int>(warning_level_);
return ss.str();
}