diff --git a/CMakeLists.txt b/CMakeLists.txt index f1ee250..ad9fe17 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,7 +68,6 @@ include_directories( add_library(ur_rtde_driver src/comm/tcp_socket.cpp - src/comm/shell_consumer.cpp #src/ros/service_stopper.cpp #src/ur/commander.cpp #src/ur/master_board.cpp diff --git a/include/ur_rtde_driver/comm/package.h b/include/ur_rtde_driver/comm/package.h index 069e83e..f61f8c7 100644 --- a/include/ur_rtde_driver/comm/package.h +++ b/include/ur_rtde_driver/comm/package.h @@ -46,8 +46,8 @@ public: * \brief Creates a new URPackage object. */ - URPackage(); - virtual ~URPackage(); + URPackage() = default; + virtual ~URPackage() = default; using _header_type = HeaderT; diff --git a/include/ur_rtde_driver/comm/parser.h b/include/ur_rtde_driver/comm/parser.h index a385603..4d22089 100644 --- a/include/ur_rtde_driver/comm/parser.h +++ b/include/ur_rtde_driver/comm/parser.h @@ -35,6 +35,9 @@ class Parser { public: + Parser() = default; + virtual ~Parser() = default; + /*! * \brief declares the parse function. * diff --git a/include/ur_rtde_driver/comm/pipeline.h b/include/ur_rtde_driver/comm/pipeline.h index 0a55c95..4d0c6b6 100644 --- a/include/ur_rtde_driver/comm/pipeline.h +++ b/include/ur_rtde_driver/comm/pipeline.h @@ -138,10 +138,44 @@ public: template class Pipeline { +public: + using _package_type = URPackage; + Pipeline(IProducer& producer, IConsumer<_package_type>& consumer, std::string name, INotifier& notifier) + : producer_(producer), consumer_(consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false } + { + } + + void run() + { + if (running_) + return; + + running_ = true; + pThread_ = std::thread(&Pipeline::run_producer, this); + cThread_ = std::thread(&Pipeline::run_consumer, this); + notifier_.started(name_); + } + + void stop() + { + if (!running_) + return; + + LOG_INFO("Stopping pipeline! <%s>", name_.c_str()); + + consumer_.stopConsumer(); + producer_.stopProducer(); + + running_ = false; + + pThread_.join(); + cThread_.join(); + notifier_.stopped(name_); + } + private: typedef std::chrono::high_resolution_clock Clock; typedef Clock::time_point Time; - using _package_type = URPackage; IProducer& producer_; IConsumer<_package_type>& consumer_; std::string name_; @@ -203,40 +237,6 @@ private: running_ = false; notifier_.stopped(name_); } - -public: - Pipeline(IProducer& producer, IConsumer<_package_type>& consumer, std::string name, INotifier& notifier) - : producer_(producer), consumer_(consumer), name_(name), notifier_(notifier), queue_{ 32 }, running_{ false } - { - } - - void run() - { - if (running_) - return; - - running_ = true; - pThread_ = std::thread(&Pipeline::run_producer, this); - cThread_ = std::thread(&Pipeline::run_consumer, this); - notifier_.started(name_); - } - - void stop() - { - if (!running_) - return; - - LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str()); - - consumer_.stopConsumer(); - producer_.stopProducer(); - - running_ = false; - - pThread_.join(); - cThread_.join(); - notifier_.stopped(name_); - } }; } // namespace comm } // namespace ur_driver diff --git a/include/ur_rtde_driver/comm/producer.h b/include/ur_rtde_driver/comm/producer.h index 3d49650..e309697 100644 --- a/include/ur_rtde_driver/comm/producer.h +++ b/include/ur_rtde_driver/comm/producer.h @@ -34,11 +34,11 @@ class URProducer : public IProducer { private: URStream& stream_; - URParser& parser_; + Parser& parser_; std::chrono::seconds timeout_; public: - URProducer(URStream& stream, URParser& parser) : stream_(stream), parser_(parser), timeout_(1) + URProducer(URStream& stream, Parser& parser) : stream_(stream), parser_(parser), timeout_(1) { } diff --git a/include/ur_rtde_driver/comm/shell_consumer.h b/include/ur_rtde_driver/comm/shell_consumer.h index 1f8c19f..5bce892 100644 --- a/include/ur_rtde_driver/comm/shell_consumer.h +++ b/include/ur_rtde_driver/comm/shell_consumer.h @@ -15,6 +15,7 @@ #ifndef UR_RTDE_DRIVER_SHELL_CONSUMER_H_INCLUDED #define UR_RTDE_DRIVER_SHELL_CONSUMER_H_INCLUDED +#include "ur_rtde_driver/log.h" #include "ur_rtde_driver/comm/pipeline.h" #include "ur_rtde_driver/comm/package.h" @@ -42,7 +43,11 @@ public: { } - virtual bool consume(std::shared_ptr> product); + virtual bool consume(std::shared_ptr> product) + { + LOG_INFO("%s", product->toString().c_str()); + return true; + } private: /* data */ diff --git a/include/ur_rtde_driver/comm/stream.h b/include/ur_rtde_driver/comm/stream.h index b2fafd1..1db9ae8 100644 --- a/include/ur_rtde_driver/comm/stream.h +++ b/include/ur_rtde_driver/comm/stream.h @@ -130,7 +130,7 @@ bool URStream::read(uint8_t* buf, const size_t buf_len, size_t& total) bool initial = true; uint8_t* buf_pos = buf; - size_t remainder = sizeof(HeaderT::_package_size_type); + size_t remainder = sizeof(typename HeaderT::_package_size_type); size_t read = 0; while (remainder > 0 && TCPSocket::read(buf_pos, remainder, read)) @@ -139,7 +139,7 @@ bool URStream::read(uint8_t* buf, const size_t buf_len, size_t& total) if (initial) { remainder = HeaderT::getPackageLength(buf); - if (remainder >= (buf_len - sizeof(HeaderT::_package_size_type))) + if (remainder >= (buf_len - sizeof(typename HeaderT::_package_size_type))) { LOG_ERROR("Packet size %zd is larger than buffer %zu, discarding.", remainder, buf_len); return false; diff --git a/include/ur_rtde_driver/primary/primary_parser.h b/include/ur_rtde_driver/primary/primary_parser.h index 7a91965..1f697f1 100644 --- a/include/ur_rtde_driver/primary/primary_parser.h +++ b/include/ur_rtde_driver/primary/primary_parser.h @@ -24,9 +24,9 @@ #include "ur_rtde_driver/primary/package_header.h" #include "ur_rtde_driver/primary/robot_state.h" #include "ur_rtde_driver/primary/robot_message.h" -#include "ur_rtde_driver/primary/robot_state/robot_mode_data.h" +//#include "ur_rtde_driver/primary/robot_state/robot_mode_data.h" #include "ur_rtde_driver/primary/robot_state/kinematics_info.h" -#include "ur_rtde_driver/primary/robot_state/master_board.h" +//#include "ur_rtde_driver/primary/robot_state/master_board.h" #include "ur_rtde_driver/primary/robot_message/version_message.h" namespace ur_driver @@ -34,54 +34,22 @@ namespace ur_driver namespace primary_interface { using namespace comm; -class PrimaryParser : comm::Parser +class PrimaryParser : public comm::Parser { -private: - RobotState* state_from_type(robot_state_type type) - { - switch (type) - { - /*case robot_state_type::ROBOT_MODE_DATA: - // SharedRobotModeData* rmd = new SharedRobotModeData(); - - //return new rmd; - case robot_state_type::MASTERBOARD_DATA: - return new MBD;*/ - case robot_state_type::KINEMATICS_INFO: - return new KinematicsInfo; - default: - return nullptr; - } - } - - RobotMessage* message_from_type(message_type type, uint64_t timestamp, uint8_t source) - { - switch (type) - { - /*case robot_state_type::ROBOT_MODE_DATA: - // SharedRobotModeData* rmd = new SharedRobotModeData(); - - //return new rmd; - case robot_state_type::MASTERBOARD_DATA: - return new MBD;*/ - case message_type::ROBOT_MESSAGE_VERSION: - return new VersionMessage(timestamp, source); - default: - return nullptr; - } - } - public: - bool parse(BinParser& bp, std::vector>& results) + PrimaryParser() = default; + virtual ~PrimaryParser() = default; + + bool parse(BinParser& bp, std::vector>>& results) { int32_t packet_size; - robot_message_type type; + RobotPackageType type; bp.parse(packet_size); bp.parse(type); switch (type) { - case robot_message_type::ROBOT_STATE: + case RobotPackageType::ROBOT_STATE: { while (!bp.empty()) { @@ -100,7 +68,7 @@ public: // deconstruction of a sub parser will increment the position of the parent parser BinParser sbp(bp, sub_size); sbp.consume(sizeof(sub_size)); - robot_state_type type; + RobotStateType type; sbp.parse(type); std::unique_ptr packet(state_from_type(type)); @@ -132,18 +100,16 @@ public: break; } - case robot_message_type::ROBOT_MESSAGE: + case RobotPackageType::ROBOT_MESSAGE: { uint64_t timestamp; uint8_t source; - message_type message_type; + RobotMessagePackageType message_type; bp.parse(timestamp); bp.parse(source); bp.parse(message_type); - bool parsed = false; - std::unique_ptr packet(message_from_type(message_type, timestamp, source)); if (!packet->parseWith(bp)) { @@ -152,19 +118,54 @@ public: } results.push_back(std::move(packet)); - return parsed; + return true; break; } default: { - LOG_WARN("Invalid state message type recieved: %u", static_cast(type)); + LOG_WARN("Invalid robot package type recieved: %u", static_cast(type)); bp.consume(); return true; } } return true; } + +private: + RobotState* state_from_type(RobotStateType type) + { + switch (type) + { + /*case robot_state_type::ROBOT_MODE_DATA: + // SharedRobotModeData* rmd = new SharedRobotModeData(); + + //return new rmd; + case robot_state_type::MASTERBOARD_DATA: + return new MBD;*/ + case RobotStateType::KINEMATICS_INFO: + return new KinematicsInfo; + default: + return nullptr; + } + } + + RobotMessage* message_from_type(RobotMessagePackageType type, uint64_t timestamp, uint8_t source) + { + switch (type) + { + /*case robot_state_type::ROBOT_MODE_DATA: + // SharedRobotModeData* rmd = new SharedRobotModeData(); + + //return new rmd; + case robot_state_type::MASTERBOARD_DATA: + return new MBD;*/ + case RobotMessagePackageType::ROBOT_MESSAGE_VERSION: + return new VersionMessage(timestamp, source); + default: + return new RobotMessage(timestamp, source); + } + } }; } // namespace primary_interface diff --git a/include/ur_rtde_driver/primary/robot_state.h b/include/ur_rtde_driver/primary/robot_state.h index f829337..fd9499d 100644 --- a/include/ur_rtde_driver/primary/robot_state.h +++ b/include/ur_rtde_driver/primary/robot_state.h @@ -38,6 +38,20 @@ namespace ur_driver { namespace primary_interface { +enum class RobotStateType : 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 +}; + /*! * \brief Abstract class for a RobotState msg. This will never be instanciated, but the underlying * data packages will be used directly. diff --git a/include/ur_rtde_driver/primary/robot_state/robot_mode_data.h b/include/ur_rtde_driver/primary/robot_state/robot_mode_data.h index daf8453..def1d04 100644 --- a/include/ur_rtde_driver/primary/robot_state/robot_mode_data.h +++ b/include/ur_rtde_driver/primary/robot_state/robot_mode_data.h @@ -38,7 +38,7 @@ namespace primary_interface class SharedRobotModeData { public: - virtual bool parseWith(BinParser& bp); + virtual bool parseWith(comm::BinParser& bp); uint64_t timestamp; bool physical_robot_connected; @@ -76,7 +76,7 @@ enum class robot_control_mode_V3_X : uint8_t class RobotModeData_V3_0__1 : public SharedRobotModeData, public RobotState { public: - virtual bool parseWith(BinParser& bp); + virtual bool parseWith(comm::BinParser& bp); robot_mode_V3_X robot_mode; robot_control_mode_V3_X control_mode; @@ -93,8 +93,8 @@ public: class RobotModeData_V3_2 : public RobotModeData_V3_0__1 { public: - virtual bool parseWith(BinParser& bp); - virtual bool consumeWith(URStatePacketConsumer& consumer); + virtual bool parseWith(comm::BinParser& bp); + // virtual bool consumeWith(URStatePacketConsumer& consumer); double target_speed_fraction_limit; @@ -106,8 +106,8 @@ public: class RobotModeData_V3_5 : public RobotModeData_V3_2 { public: - virtual bool parseWith(BinParser& bp); - virtual bool consumeWith(URStatePacketConsumer& consumer); + virtual bool parseWith(comm::BinParser& bp); + // virtual bool consumeWith(URStatePacketConsumer& consumer); unsigned char unknown_internal_use; diff --git a/src/comm/shell_consumer.cpp b/src/comm/shell_consumer.cpp deleted file mode 100644 index b714eae..0000000 --- a/src/comm/shell_consumer.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- - -// -- BEGIN LICENSE BLOCK ---------------------------------------------- -// -- END LICENSE BLOCK ------------------------------------------------ - -//---------------------------------------------------------------------- -/*!\file - * - * \author Felix Mauch mauch@fzi.de - * \date 2019-04-09 - * - */ -//---------------------------------------------------------------------- - -#include "ur_rtde_driver/comm/shell_consumer.h" - -namespace ur_driver -{ -namespace comm -{ -template -bool ShellConsumer::consume(std::shared_ptr> pkg) -{ - LOG_INFO("%s", pkg->toString()); - return true; -} -} // namespace comm - -} // namespace ur_driver diff --git a/src/primary/robot_message.cpp b/src/primary/robot_message.cpp index 41c868e..2f17ee9 100644 --- a/src/primary/robot_message.cpp +++ b/src/primary/robot_message.cpp @@ -32,10 +32,6 @@ namespace primary_interface { bool RobotMessage::parseWith(comm::BinParser& bp) { - bp.parse(timestamp_); - bp.parse(source_); - bp.parse(message_type_); - return true; }