From 489bda33ecf8e4e466ac58939e3c28c6f295f9e3 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2020 07:50:41 +0200 Subject: [PATCH 01/18] source and message types should be signed --- include/ur_client_library/primary/primary_parser.h | 2 +- include/ur_client_library/primary/robot_message.h | 6 +++--- .../primary/robot_message/version_message.h | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index 9eb228007..4b0f429c5 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -115,7 +115,7 @@ class PrimaryParser : public comm::Parser case RobotPackageType::ROBOT_MESSAGE: { uint64_t timestamp; - uint8_t source; + int8_t source; RobotMessagePackageType message_type; bp.parse(timestamp); diff --git a/include/ur_client_library/primary/robot_message.h b/include/ur_client_library/primary/robot_message.h index 6a3af928b..3cf5fcf1f 100644 --- a/include/ur_client_library/primary/robot_message.h +++ b/include/ur_client_library/primary/robot_message.h @@ -37,7 +37,7 @@ namespace primary_interface /*! * \brief Possible RobotMessage types */ -enum class RobotMessagePackageType : uint8_t +enum class RobotMessagePackageType : int8_t { ROBOT_MESSAGE_TEXT = 0, ROBOT_MESSAGE_PROGRAM_LABEL = 1, @@ -62,7 +62,7 @@ class RobotMessage : public PrimaryPackage * \param timestamp Timestamp of the package * \param source The package's source */ - RobotMessage(const uint64_t timestamp, const uint8_t source) : timestamp_(timestamp), source_(source) + RobotMessage(const uint64_t timestamp, const int8_t source) : timestamp_(timestamp), source_(source) { } virtual ~RobotMessage() = default; @@ -94,7 +94,7 @@ class RobotMessage : public PrimaryPackage virtual std::string toString() const; uint64_t timestamp_; - uint8_t source_; + int8_t source_; RobotMessagePackageType message_type_; }; diff --git a/include/ur_client_library/primary/robot_message/version_message.h b/include/ur_client_library/primary/robot_message/version_message.h index a44432278..e64a37d29 100644 --- a/include/ur_client_library/primary/robot_message/version_message.h +++ b/include/ur_client_library/primary/robot_message/version_message.h @@ -47,7 +47,7 @@ class VersionMessage : public RobotMessage * \param timestamp Timestamp of the package * \param source The package's source */ - VersionMessage(uint64_t timestamp, uint8_t source) : RobotMessage(timestamp, source) + VersionMessage(uint64_t timestamp, int8_t source) : RobotMessage(timestamp, source) { } virtual ~VersionMessage() = default; From e525114717344bf4079618518e346a6f9227315a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2020 07:51:44 +0200 Subject: [PATCH 02/18] Added constructor with package type for RobotMessage --- include/ur_client_library/primary/primary_parser.h | 2 +- include/ur_client_library/primary/robot_message.h | 12 ++++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index 4b0f429c5..ab499df47 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -175,7 +175,7 @@ class PrimaryParser : public comm::Parser case RobotMessagePackageType::ROBOT_MESSAGE_VERSION: return new VersionMessage(timestamp, source); default: - return new RobotMessage(timestamp, source); + return new RobotMessage(timestamp, source, type); } } }; diff --git a/include/ur_client_library/primary/robot_message.h b/include/ur_client_library/primary/robot_message.h index 3cf5fcf1f..f118e7e60 100644 --- a/include/ur_client_library/primary/robot_message.h +++ b/include/ur_client_library/primary/robot_message.h @@ -65,6 +65,18 @@ class RobotMessage : public PrimaryPackage RobotMessage(const uint64_t timestamp, const int8_t source) : timestamp_(timestamp), source_(source) { } + + /*! + * \brief Creates a new RobotMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + * \param message_type The package's message type + */ + RobotMessage(const uint64_t timestamp, const int8_t source, const RobotMessagePackageType message_type) + : timestamp_(timestamp), source_(source), message_type_(message_type) + { + } virtual ~RobotMessage() = default; /*! From 19892bac15d6d0617db17151d8d7daac0cadcc22 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2020 07:53:43 +0200 Subject: [PATCH 03/18] Initialize message_type in VersionMessage --- .../ur_client_library/primary/robot_message/version_message.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/ur_client_library/primary/robot_message/version_message.h b/include/ur_client_library/primary/robot_message/version_message.h index e64a37d29..f662b0214 100644 --- a/include/ur_client_library/primary/robot_message/version_message.h +++ b/include/ur_client_library/primary/robot_message/version_message.h @@ -47,7 +47,8 @@ class VersionMessage : public RobotMessage * \param timestamp Timestamp of the package * \param source The package's source */ - VersionMessage(uint64_t timestamp, int8_t source) : RobotMessage(timestamp, source) + VersionMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_VERSION) { } virtual ~VersionMessage() = default; From 236cc6131b8b9d906711044e275d8d354d58794f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2020 08:04:55 +0200 Subject: [PATCH 04/18] Added virtual destructor for URProducer --- include/ur_client_library/comm/producer.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/ur_client_library/comm/producer.h b/include/ur_client_library/comm/producer.h index 027fb75bc..253b8d0d0 100644 --- a/include/ur_client_library/comm/producer.h +++ b/include/ur_client_library/comm/producer.h @@ -57,6 +57,8 @@ class URProducer : public IProducer { } + virtual ~URProducer() = default; + /*! * \brief Triggers the stream to connect to the robot. */ From 986ffc85a82e4e79b51a20944e48a3b10a987558 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 15 Sep 2020 07:38:48 +0200 Subject: [PATCH 05/18] Added TextMessage and KeyMessage --- CMakeLists.txt | 2 + .../primary/primary_parser.h | 6 ++ .../primary/robot_message/key_message.h | 82 +++++++++++++++++++ .../primary/robot_message/text_message.h | 78 ++++++++++++++++++ src/primary/robot_message/key_message.cpp | 58 +++++++++++++ src/primary/robot_message/text_message.cpp | 50 +++++++++++ 6 files changed, 276 insertions(+) create mode 100644 include/ur_client_library/primary/robot_message/key_message.h create mode 100644 include/ur_client_library/primary/robot_message/text_message.h create mode 100644 src/primary/robot_message/key_message.cpp create mode 100644 src/primary/robot_message/text_message.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 291393d3e..a43195996 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,6 +23,8 @@ add_library(urcl SHARED src/primary/robot_message.cpp src/primary/robot_state.cpp src/primary/robot_message/version_message.cpp + src/primary/robot_message/text_message.cpp + src/primary/robot_message/key_message.cpp src/primary/robot_state/kinematics_info.cpp src/rtde/control_package_pause.cpp src/rtde/control_package_setup_inputs.cpp diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index ab499df47..f4f0f2c56 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -27,6 +27,8 @@ #include "ur_client_library/primary/robot_state.h" #include "ur_client_library/primary/robot_message.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" +#include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/robot_message/text_message.h" #include "ur_client_library/primary/robot_message/version_message.h" namespace urcl @@ -174,6 +176,10 @@ class PrimaryParser : public comm::Parser return new MBD;*/ case RobotMessagePackageType::ROBOT_MESSAGE_VERSION: return new VersionMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_TEXT: + return new TextMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_KEY: + return new KeyMessage(timestamp, source); default: return new RobotMessage(timestamp, source, type); } diff --git a/include/ur_client_library/primary/robot_message/key_message.h b/include/ur_client_library/primary/robot_message/key_message.h new file mode 100644 index 000000000..50e092076 --- /dev/null +++ b/include/ur_client_library/primary/robot_message/key_message.h @@ -0,0 +1,82 @@ +// 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, Text 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 Exner exner@fzi.de + * \date 2020-04-23 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_PRIMARY_KEY_MESSAGE_H_INCLUDED +#define UR_RTDE_DRIVER_PRIMARY_KEY_MESSAGE_H_INCLUDED + +#include "ur_robot_driver/primary/robot_message.h" + +namespace ur_driver +{ +namespace primary_interface +{ +/*! + * \brief The KeyMessage class handles the key messages sent via the primary UR interface. + */ +class KeyMessage : public RobotMessage +{ +public: + KeyMessage() = delete; + /*! + * \brief Creates a new KeyMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + KeyMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_KEY) + { + } + virtual ~KeyMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + int32_t message_code_; + int32_t message_argument_; + uint8_t title_size_; + std::string title_; + std::string text_; +}; +} // namespace primary_interface +} // namespace ur_driver + +#endif // ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_message/text_message.h b/include/ur_client_library/primary/robot_message/text_message.h new file mode 100644 index 000000000..531bf8365 --- /dev/null +++ b/include/ur_client_library/primary/robot_message/text_message.h @@ -0,0 +1,78 @@ +// 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, Text 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 Exner exner@fzi.de + * \date 2020-04-23 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED +#define UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED + +#include "ur_robot_driver/primary/robot_message.h" + +namespace ur_driver +{ +namespace primary_interface +{ +/*! + * \brief The TextMessage class handles the text messages sent via the primary UR interface. + */ +class TextMessage : public RobotMessage +{ +public: + TextMessage() = delete; + /*! + * \brief Creates a new TextMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + TextMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_TEXT) + { + } + virtual ~TextMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + std::string text_; +}; +} // namespace primary_interface +} // namespace ur_driver + +#endif // ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/src/primary/robot_message/key_message.cpp b/src/primary/robot_message/key_message.cpp new file mode 100644 index 000000000..890d0a064 --- /dev/null +++ b/src/primary/robot_message/key_message.cpp @@ -0,0 +1,58 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// Copyright 2017, 2018 Simon Rasmussen (refactor) +// +// Copyright 2015, 2016 Thomas Timm Andersen (original version) +// +// 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 Exner exner@fzi.de + * \date 2019-04-08 + * + */ +//---------------------------------------------------------------------- + +#include "ur_robot_driver/log.h" +#include "ur_robot_driver/primary/robot_message/key_message.h" + +namespace ur_driver +{ +namespace primary_interface +{ +bool KeyMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(message_code_); + bp.parse(message_argument_); + bp.parse(title_size_); + bp.parse(title_, title_size_); + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +std::string KeyMessage::toString() const +{ + std::stringstream ss; + ss << "Message code: " << message_code_ << std::endl; + ss << "Title: " << title_ << std::endl; + ss << "Text: " << text_; + return ss.str(); +} +} // namespace primary_interface +} // namespace ur_driver diff --git a/src/primary/robot_message/text_message.cpp b/src/primary/robot_message/text_message.cpp new file mode 100644 index 000000000..4927372b8 --- /dev/null +++ b/src/primary/robot_message/text_message.cpp @@ -0,0 +1,50 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// Copyright 2017, 2018 Simon Rasmussen (refactor) +// +// Copyright 2015, 2016 Thomas Timm Andersen (original version) +// +// 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 Exner exner@fzi.de + * \date 2019-04-08 + * + */ +//---------------------------------------------------------------------- + +#include "ur_robot_driver/log.h" +#include "ur_robot_driver/primary/robot_message/text_message.h" + +namespace ur_driver +{ +namespace primary_interface +{ +bool TextMessage::parseWith(comm::BinParser& bp) +{ + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +std::string TextMessage::toString() const +{ + return text_; +} +} // namespace primary_interface +} // namespace ur_driver From a76c52af15144a4876523ee1ad8c652e6472a696 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 15 Sep 2020 07:45:19 +0200 Subject: [PATCH 06/18] Added error_code and runtime_exception message Note: The error_code message changed between versions 5.1 and 5.2 --- CMakeLists.txt | 2 + .../primary/primary_parser.h | 6 ++ .../robot_message/error_code_message.h | 83 +++++++++++++++++++ .../robot_message/runtime_exception_message.h | 80 ++++++++++++++++++ .../robot_message/error_code_message.cpp | 62 ++++++++++++++ .../runtime_exception_message.cpp | 56 +++++++++++++ 6 files changed, 289 insertions(+) create mode 100644 include/ur_client_library/primary/robot_message/error_code_message.h create mode 100644 include/ur_client_library/primary/robot_message/runtime_exception_message.h create mode 100644 src/primary/robot_message/error_code_message.cpp create mode 100644 src/primary/robot_message/runtime_exception_message.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a43195996..9c1f98292 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,8 @@ add_library(urcl SHARED src/primary/primary_package.cpp src/primary/robot_message.cpp src/primary/robot_state.cpp + src/primary/robot_message/error_code_message.cpp + src/primary/robot_message/runtime_exception_message.cpp src/primary/robot_message/version_message.cpp src/primary/robot_message/text_message.cpp src/primary/robot_message/key_message.cpp diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index f4f0f2c56..6fa31ff52 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -28,6 +28,8 @@ #include "ur_client_library/primary/robot_message.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" #include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/robot_message/runtime_exception_message.h" #include "ur_client_library/primary/robot_message/text_message.h" #include "ur_client_library/primary/robot_message/version_message.h" @@ -180,6 +182,10 @@ class PrimaryParser : public comm::Parser return new TextMessage(timestamp, source); case RobotMessagePackageType::ROBOT_MESSAGE_KEY: return new KeyMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE: + return new ErrorCodeMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_RUNTIME_EXCEPTION: + return new RuntimeExceptionMessage(timestamp, source); default: return new RobotMessage(timestamp, source, type); } diff --git a/include/ur_client_library/primary/robot_message/error_code_message.h b/include/ur_client_library/primary/robot_message/error_code_message.h new file mode 100644 index 000000000..b6449c9b5 --- /dev/null +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -0,0 +1,83 @@ +// 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, Text 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 Exner exner@fzi.de + * \date 2020-04-23 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED +#define UR_RTDE_DRIVER_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/robot_message.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief The ErrorCodeMessage class handles the error code messages sent via the primary UR interface. + */ +class ErrorCodeMessage : public RobotMessage +{ +public: + ErrorCodeMessage() = delete; + /*! + * \brief Creates a new ErrorCodeMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + ErrorCodeMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE) + { + } + virtual ~ErrorCodeMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + int32_t message_code_; + int32_t message_argument_; + int32_t report_level_; + uint8_t data_type_; + uint32_t data_; + std::string text_; +}; +} // namespace primary_interface +} // namespace ur_driver + +#endif // ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_message/runtime_exception_message.h b/include/ur_client_library/primary/robot_message/runtime_exception_message.h new file mode 100644 index 000000000..434f4255b --- /dev/null +++ b/include/ur_client_library/primary/robot_message/runtime_exception_message.h @@ -0,0 +1,80 @@ +// 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, Text 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 Exner exner@fzi.de + * \date 2020-04-23 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_PRIMARY_RUNTIME_EXCEPTION_MESSAGE_H_INCLUDED +#define UR_RTDE_DRIVER_PRIMARY_RUNTIME_EXCEPTION_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/robot_message.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief The RuntimeExceptionMessage class handles the runtime exception messages sent via the primary UR interface. + */ +class RuntimeExceptionMessage : public RobotMessage +{ +public: + RuntimeExceptionMessage() = delete; + /*! + * \brief Creates a new RuntimeExceptionMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + RuntimeExceptionMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_RUNTIME_EXCEPTION) + { + } + virtual ~RuntimeExceptionMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + int32_t line_number_; + int32_t column_number_; + std::string text_; +}; +} // namespace primary_interface +} // namespace ur_driver + +#endif // ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/src/primary/robot_message/error_code_message.cpp b/src/primary/robot_message/error_code_message.cpp new file mode 100644 index 000000000..ca2ed8ba4 --- /dev/null +++ b/src/primary/robot_message/error_code_message.cpp @@ -0,0 +1,62 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// Copyright 2017, 2018 Simon Rasmussen (refactor) +// +// Copyright 2015, 2016 Thomas Timm Andersen (original version) +// +// 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" + +namespace urcl +{ +namespace primary_interface +{ +bool ErrorCodeMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(message_code_); + bp.parse(message_argument_); + bp.parse(report_level_); + bp.parse(data_type_); + bp.parse(data_); + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +std::string ErrorCodeMessage::toString() const +{ + std::stringstream ss; + ss << "Message code: " << message_code_ << std::endl; + ss << "Message argument: " << message_argument_ << std::endl; + ss << "Report level: " << report_level_ << std::endl; + ss << "Datatype: " << static_cast(data_type_) << std::endl; + ss << "Data: " << data_ << std::endl; + ss << "Text: " << text_; + return ss.str(); +} +} // namespace primary_interface +} // namespace ur_driver diff --git a/src/primary/robot_message/runtime_exception_message.cpp b/src/primary/robot_message/runtime_exception_message.cpp new file mode 100644 index 000000000..974f8e561 --- /dev/null +++ b/src/primary/robot_message/runtime_exception_message.cpp @@ -0,0 +1,56 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// Copyright 2017, 2018 Simon Rasmussen (refactor) +// +// Copyright 2015, 2016 Thomas Timm Andersen (original version) +// +// 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_message/runtime_exception_message.h" + +namespace urcl +{ +namespace primary_interface +{ +bool RuntimeExceptionMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(line_number_); + bp.parse(column_number_); + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +std::string RuntimeExceptionMessage::toString() const +{ + std::stringstream ss; + ss << "Runtime error in line " << line_number_; + ss << ", column " << column_number_ << std::endl; + ss << "Error: " << text_; + return ss.str(); +} +} // namespace primary_interface +} // namespace ur_driver From 4a589d40bc52220b862d70910b5a033cbe1500a0 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 15 Sep 2020 07:46:16 +0200 Subject: [PATCH 07/18] fixup! Added TextMessage and KeyMessage --- .../ur_client_library/primary/robot_message/key_message.h | 4 ++-- .../ur_client_library/primary/robot_message/text_message.h | 4 ++-- src/primary/robot_message/key_message.cpp | 6 +++--- src/primary/robot_message/text_message.cpp | 6 +++--- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/include/ur_client_library/primary/robot_message/key_message.h b/include/ur_client_library/primary/robot_message/key_message.h index 50e092076..72d6d8f01 100644 --- a/include/ur_client_library/primary/robot_message/key_message.h +++ b/include/ur_client_library/primary/robot_message/key_message.h @@ -28,9 +28,9 @@ #ifndef UR_RTDE_DRIVER_PRIMARY_KEY_MESSAGE_H_INCLUDED #define UR_RTDE_DRIVER_PRIMARY_KEY_MESSAGE_H_INCLUDED -#include "ur_robot_driver/primary/robot_message.h" +#include "ur_client_library/primary/robot_message.h" -namespace ur_driver +namespace urcl { namespace primary_interface { diff --git a/include/ur_client_library/primary/robot_message/text_message.h b/include/ur_client_library/primary/robot_message/text_message.h index 531bf8365..da5bb60d2 100644 --- a/include/ur_client_library/primary/robot_message/text_message.h +++ b/include/ur_client_library/primary/robot_message/text_message.h @@ -28,9 +28,9 @@ #ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED #define UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED -#include "ur_robot_driver/primary/robot_message.h" +#include "ur_client_library/primary/robot_message.h" -namespace ur_driver +namespace urcl { namespace primary_interface { diff --git a/src/primary/robot_message/key_message.cpp b/src/primary/robot_message/key_message.cpp index 890d0a064..be322648d 100644 --- a/src/primary/robot_message/key_message.cpp +++ b/src/primary/robot_message/key_message.cpp @@ -28,10 +28,10 @@ */ //---------------------------------------------------------------------- -#include "ur_robot_driver/log.h" -#include "ur_robot_driver/primary/robot_message/key_message.h" +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_message/key_message.h" -namespace ur_driver +namespace urcl { namespace primary_interface { diff --git a/src/primary/robot_message/text_message.cpp b/src/primary/robot_message/text_message.cpp index 4927372b8..f9967756a 100644 --- a/src/primary/robot_message/text_message.cpp +++ b/src/primary/robot_message/text_message.cpp @@ -28,10 +28,10 @@ */ //---------------------------------------------------------------------- -#include "ur_robot_driver/log.h" -#include "ur_robot_driver/primary/robot_message/text_message.h" +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_message/text_message.h" -namespace ur_driver +namespace urcl { namespace primary_interface { From 4ee077e0ccd81f7df4e526278ce7560057739f1e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 15 Sep 2020 07:49:10 +0200 Subject: [PATCH 08/18] Added consumer functions to new message types --- .../primary/abstract_primary_consumer.h | 8 ++++++++ .../primary/robot_message/error_code_message.h | 9 +++++++++ .../primary/robot_message/key_message.h | 9 +++++++++ .../primary/robot_message/runtime_exception_message.h | 9 +++++++++ .../primary/robot_message/text_message.h | 9 +++++++++ src/primary/robot_message/error_code_message.cpp | 6 ++++++ src/primary/robot_message/key_message.cpp | 6 ++++++ src/primary/robot_message/runtime_exception_message.cpp | 6 ++++++ src/primary/robot_message/text_message.cpp | 6 ++++++ src/primary/robot_message/version_message.cpp | 2 +- 10 files changed, 69 insertions(+), 1 deletion(-) diff --git a/include/ur_client_library/primary/abstract_primary_consumer.h b/include/ur_client_library/primary/abstract_primary_consumer.h index 94a728fd8..0b7cb2d54 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -30,6 +30,10 @@ #include "ur_client_library/log.h" #include "ur_client_library/comm/pipeline.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/robot_message/runtime_exception_message.h" +#include "ur_client_library/primary/robot_message/text_message.h" #include "ur_client_library/primary/robot_message/version_message.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" @@ -70,6 +74,10 @@ class AbstractPrimaryConsumer : public comm::IConsumer // To be implemented in specific consumers virtual bool consume(RobotMessage& pkg) = 0; virtual bool consume(RobotState& pkg) = 0; + virtual bool consume(ErrorCodeMessage& pkg) = 0; + virtual bool consume(KeyMessage& pkg) = 0; + virtual bool consume(RuntimeExceptionMessage& pkg) = 0; + virtual bool consume(TextMessage& pkg) = 0; virtual bool consume(VersionMessage& pkg) = 0; virtual bool consume(KinematicsInfo& pkg) = 0; diff --git a/include/ur_client_library/primary/robot_message/error_code_message.h b/include/ur_client_library/primary/robot_message/error_code_message.h index b6449c9b5..df9102333 100644 --- a/include/ur_client_library/primary/robot_message/error_code_message.h +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -63,6 +63,15 @@ class ErrorCodeMessage : public RobotMessage */ virtual bool parseWith(comm::BinParser& bp); + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + /*! * \brief Produces a human readable representation of the package object. * diff --git a/include/ur_client_library/primary/robot_message/key_message.h b/include/ur_client_library/primary/robot_message/key_message.h index 72d6d8f01..3375fe527 100644 --- a/include/ur_client_library/primary/robot_message/key_message.h +++ b/include/ur_client_library/primary/robot_message/key_message.h @@ -63,6 +63,15 @@ class KeyMessage : public RobotMessage */ virtual bool parseWith(comm::BinParser& bp); + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + /*! * \brief Produces a human readable representation of the package object. * diff --git a/include/ur_client_library/primary/robot_message/runtime_exception_message.h b/include/ur_client_library/primary/robot_message/runtime_exception_message.h index 434f4255b..7b150d329 100644 --- a/include/ur_client_library/primary/robot_message/runtime_exception_message.h +++ b/include/ur_client_library/primary/robot_message/runtime_exception_message.h @@ -63,6 +63,15 @@ class RuntimeExceptionMessage : public RobotMessage */ virtual bool parseWith(comm::BinParser& bp); + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + /*! * \brief Produces a human readable representation of the package object. * diff --git a/include/ur_client_library/primary/robot_message/text_message.h b/include/ur_client_library/primary/robot_message/text_message.h index da5bb60d2..0f99453e3 100644 --- a/include/ur_client_library/primary/robot_message/text_message.h +++ b/include/ur_client_library/primary/robot_message/text_message.h @@ -63,6 +63,15 @@ class TextMessage : public RobotMessage */ virtual bool parseWith(comm::BinParser& bp); + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + /*! * \brief Produces a human readable representation of the package object. * diff --git a/src/primary/robot_message/error_code_message.cpp b/src/primary/robot_message/error_code_message.cpp index ca2ed8ba4..9395f87e8 100644 --- a/src/primary/robot_message/error_code_message.cpp +++ b/src/primary/robot_message/error_code_message.cpp @@ -30,6 +30,7 @@ #include "ur_client_library/log.h" #include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" namespace urcl { @@ -47,6 +48,11 @@ bool ErrorCodeMessage::parseWith(comm::BinParser& bp) return true; // not really possible to check dynamic size packets } +bool ErrorCodeMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + std::string ErrorCodeMessage::toString() const { std::stringstream ss; diff --git a/src/primary/robot_message/key_message.cpp b/src/primary/robot_message/key_message.cpp index be322648d..2b94e2aa8 100644 --- a/src/primary/robot_message/key_message.cpp +++ b/src/primary/robot_message/key_message.cpp @@ -30,6 +30,7 @@ #include "ur_client_library/log.h" #include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" namespace urcl { @@ -46,6 +47,11 @@ bool KeyMessage::parseWith(comm::BinParser& bp) return true; // not really possible to check dynamic size packets } +bool KeyMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + std::string KeyMessage::toString() const { std::stringstream ss; diff --git a/src/primary/robot_message/runtime_exception_message.cpp b/src/primary/robot_message/runtime_exception_message.cpp index 974f8e561..467c475a4 100644 --- a/src/primary/robot_message/runtime_exception_message.cpp +++ b/src/primary/robot_message/runtime_exception_message.cpp @@ -30,6 +30,7 @@ #include "ur_client_library/log.h" #include "ur_client_library/primary/robot_message/runtime_exception_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" namespace urcl { @@ -44,6 +45,11 @@ bool RuntimeExceptionMessage::parseWith(comm::BinParser& bp) return true; // not really possible to check dynamic size packets } +bool RuntimeExceptionMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + std::string RuntimeExceptionMessage::toString() const { std::stringstream ss; diff --git a/src/primary/robot_message/text_message.cpp b/src/primary/robot_message/text_message.cpp index f9967756a..905dafb5c 100644 --- a/src/primary/robot_message/text_message.cpp +++ b/src/primary/robot_message/text_message.cpp @@ -30,6 +30,7 @@ #include "ur_client_library/log.h" #include "ur_client_library/primary/robot_message/text_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" namespace urcl { @@ -42,6 +43,11 @@ bool TextMessage::parseWith(comm::BinParser& bp) return true; // not really possible to check dynamic size packets } +bool TextMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + std::string TextMessage::toString() const { return text_; diff --git a/src/primary/robot_message/version_message.cpp b/src/primary/robot_message/version_message.cpp index f18e07ab3..30b6fd80f 100644 --- a/src/primary/robot_message/version_message.cpp +++ b/src/primary/robot_message/version_message.cpp @@ -49,7 +49,7 @@ bool VersionMessage::parseWith(comm::BinParser& bp) return true; // not really possible to check dynamic size packets } -bool VersionMessage ::consumeWith(AbstractPrimaryConsumer& consumer) +bool VersionMessage::consumeWith(AbstractPrimaryConsumer& consumer) { return consumer.consume(*this); } From cd984bac048b08fd6a2a8f31605e0aad71055207 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 15 Sep 2020 07:52:28 +0200 Subject: [PATCH 09/18] Added a draft version for a primary client --- CMakeLists.txt | 1 + .../primary/primary_client.h | 59 ++++++++++++ .../primary/primary_shell_consumer.h | 92 +++++++++++++++++++ src/primary/primary_client.cpp | 47 ++++++++++ 4 files changed, 199 insertions(+) create mode 100644 include/ur_client_library/primary/primary_client.h create mode 100644 include/ur_client_library/primary/primary_shell_consumer.h create mode 100644 src/primary/primary_client.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9c1f98292..3924cc143 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -pedantic") add_library(urcl SHARED src/comm/tcp_socket.cpp src/comm/server.cpp + src/primary/primary_client.cpp src/primary/primary_package.cpp src/primary/robot_message.cpp src/primary/robot_state.cpp diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h new file mode 100644 index 000000000..5348bcdb5 --- /dev/null +++ b/include/ur_client_library/primary/primary_client.h @@ -0,0 +1,59 @@ +// 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, Text 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- +#ifndef UR_ROBOT_DRIVER_PRIMARY_CLIENT_H_INCLUDED +#define UR_ROBOT_DRIVER_PRIMARY_CLIENT_H_INCLUDED + +#include +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class PrimaryClient +{ +public: + PrimaryClient() = delete; + PrimaryClient(const std::string& robot_ip); + virtual ~PrimaryClient() = default; + +private: + std::string robot_ip_; + PrimaryParser parser_; + std::unique_ptr> consumer_; + comm::INotifier notifier_; + std::unique_ptr> producer_; + std::unique_ptr> stream_; + std::unique_ptr> pipeline_; +}; + +} // namespace primary_interface +} // namespace ur_driver + +#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_CLIENT_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_shell_consumer.h b/include/ur_client_library/primary/primary_shell_consumer.h new file mode 100644 index 000000000..3a924646f --- /dev/null +++ b/include/ur_client_library/primary/primary_shell_consumer.h @@ -0,0 +1,92 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_ROBOT_DRIVER_PRIMARY_SHELL_CONSUMER_H_INCLUDED +#define UR_ROBOT_DRIVER_PRIMARY_SHELL_CONSUMER_H_INCLUDED + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +class PrimaryShellConsumer : public AbstractPrimaryConsumer +{ +public: + PrimaryShellConsumer() = default; + virtual ~PrimaryShellConsumer() = default; + + virtual bool consume(RobotMessage& msg) override + { + LOG_INFO("---RobotMessage:---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(RobotState& msg) override + { + LOG_INFO("---RobotState:---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(ErrorCodeMessage& msg) override + { + LOG_INFO("---ErrorCodeMessage---%s", msg.toString().c_str()); + return true; + } + virtual bool consume(KeyMessage& msg) override + { + LOG_INFO("---KeyMessage---%s", msg.toString().c_str()); + return true; + } + virtual bool consume(RuntimeExceptionMessage& msg) override + { + LOG_INFO("---RuntimeExceptionMessage---%s", msg.toString().c_str()); + return true; + } + virtual bool consume(TextMessage& msg) override + { + LOG_INFO("---TextMessage---%s", msg.toString().c_str()); + return true; + } + virtual bool consume(VersionMessage& msg) override + { + LOG_INFO("---VersionMessage---%s", msg.toString().c_str()); + return true; + } + virtual bool consume(KinematicsInfo& msg) override + { + LOG_INFO("%s", msg.toString().c_str()); + return true; + } + +private: + /* data */ +}; +} // namespace primary_interface +} // namespace ur_driver + +#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_SHELL_CONSUMER_H_INCLUDED diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp new file mode 100644 index 000000000..abe616908 --- /dev/null +++ b/src/primary/primary_client.cpp @@ -0,0 +1,47 @@ +// 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, Text 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +PrimaryClient::PrimaryClient(const std::string& robot_ip) : robot_ip_(robot_ip) +{ + stream_.reset(new comm::URStream(robot_ip_, UR_PRIMARY_PORT)); + producer_.reset(new comm::URProducer(*stream_, parser_)); + producer_->setupProducer(); + + consumer_.reset(new PrimaryShellConsumer()); + + pipeline_.reset(new comm::Pipeline(*producer_, consumer_.get(), "primary pipeline", notifier_)); + pipeline_->run(); +} +} // namespace primary_interface +} // namespace ur_driver From 53e052bba03e56ec2681827efe3f70263741ece9 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 15 Sep 2020 08:05:12 +0200 Subject: [PATCH 10/18] WIP: Move primary interface logic to primary client --- .../primary/key_message_handler.h | 60 ++++++++ .../primary/primary_client.h | 26 +++- .../primary/primary_consumer.h | 137 ++++++++++++++++++ .../primary/primary_package_handler.h | 58 ++++++++ .../primary/primary_shell_consumer.h | 4 +- .../ur/calibration_checker.h | 38 +---- include/ur_client_library/ur/ur_driver.h | 9 +- src/primary/primary_client.cpp | 58 +++++++- src/ur/calibration_checker.cpp | 34 ++--- src/ur/ur_driver.cpp | 65 ++------- 10 files changed, 371 insertions(+), 118 deletions(-) create mode 100644 include/ur_client_library/primary/key_message_handler.h create mode 100644 include/ur_client_library/primary/primary_consumer.h create mode 100644 include/ur_client_library/primary/primary_package_handler.h diff --git a/include/ur_client_library/primary/key_message_handler.h b/include/ur_client_library/primary/key_message_handler.h new file mode 100644 index 000000000..ddec32cd5 --- /dev/null +++ b/include/ur_client_library/primary/key_message_handler.h @@ -0,0 +1,60 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- +// +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_ROBOT_DRIVER_KEY_MESSAGE_HANDLER_H_INCLUDED +#define UR_ROBOT_DRIVER_KEY_MESSAGE_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class KeyMessageHandler : public IPrimaryPackageHandler +{ +public: + KeyMessageHandler() = default; + virtual ~KeyMessageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(KeyMessage& pkg) override + { + LOG_INFO("%s", pkg.toString().c_str()); + } + +private: + /* data */ +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_ROBOT_DRIVER_KEY_MESSAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index 5348bcdb5..66e63283a 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -31,6 +31,8 @@ #include #include #include +#include +#include namespace urcl { @@ -40,13 +42,31 @@ class PrimaryClient { public: PrimaryClient() = delete; - PrimaryClient(const std::string& robot_ip); + PrimaryClient(const std::string& robot_ip, const std::string& calibration_checksum); virtual ~PrimaryClient() = default; + /*! + * \brief Sends a custom script program to the robot. + * + * The given code must be valid according the UR Scripting Manual. + * + * \param script_code URScript code that shall be executed by the robot. + * + * \returns true on successful upload, false otherwise. + */ + bool sendScript(const std::string& script_code); + + /*! + * \brief Checks if the kinematics information in the used model fits the actual robot. + * + * \param checksum Hash of the used kinematics information + */ + void checkCalibration(const std::string& checksum); + private: std::string robot_ip_; PrimaryParser parser_; - std::unique_ptr> consumer_; + std::unique_ptr consumer_; comm::INotifier notifier_; std::unique_ptr> producer_; std::unique_ptr> stream_; @@ -54,6 +74,6 @@ class PrimaryClient }; } // namespace primary_interface -} // namespace ur_driver +} // namespace urcl #endif // ifndef UR_ROBOT_DRIVER_PRIMARY_CLIENT_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h new file mode 100644 index 000000000..6c78b9448 --- /dev/null +++ b/include/ur_client_library/primary/primary_consumer.h @@ -0,0 +1,137 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- +// +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_ROBOT_DRIVER_PRIMARY_CONSUMER_H_INCLUDED +#define UR_ROBOT_DRIVER_PRIMARY_CONSUMER_H_INCLUDED + +#include "ur_client_library/log.h" +#include "ur_client_library/comm/pipeline.h" +#include "ur_client_library/primary/primary_package_handler.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" +#include "ur_client_library/primary/key_message_handler.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/robot_message/runtime_exception_message.h" +#include "ur_client_library/primary/robot_message/text_message.h" +#include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/robot_state/kinematics_info.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief Primary consumer implementation + * + * This class implements am AbstractPrimaryConsumer such that it can consume all incoming primary + * messages. However, actual work will be done by workers for each specific type. + */ +class PrimaryConsumer : public AbstractPrimaryConsumer +{ +public: + PrimaryConsumer() + { + LOG_INFO("Constructing primary consumer"); + key_message_worker_.reset(new KeyMessageHandler()); + LOG_INFO("Constructed primary consumer"); + } + virtual ~PrimaryConsumer() = default; + + virtual bool consume(RobotMessage& msg) override + { + LOG_INFO("---RobotMessage:---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(RobotState& msg) override + { + // LOG_INFO("---RobotState:---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(ErrorCodeMessage& msg) override + { + LOG_INFO("---ErrorCodeMessage---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(RuntimeExceptionMessage& msg) override + { + LOG_INFO("---RuntimeExceptionMessage---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(TextMessage& msg) override + { + LOG_INFO("---TextMessage---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(VersionMessage& msg) override + { + LOG_INFO("---VersionMessage---\n%s", msg.toString().c_str()); + return true; + } + + /*! + * \brief Handle a KinematicsInfo + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(KinematicsInfo& pkg) override + { + if (kinematics_info_message_worker_ != nullptr) + { + kinematics_info_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a KeyMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(KeyMessage& pkg) override + { + if (key_message_worker_ != nullptr) + { + key_message_worker_->handle(pkg); + return true; + } + return false; + } + + void setKinematicsInfoHandler(const std::shared_ptr>& handler) + { + kinematics_info_message_worker_ = handler; + } + +private: + std::shared_ptr> key_message_worker_; + std::shared_ptr> kinematics_info_message_worker_; +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_CONSUMER_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_package_handler.h b/include/ur_client_library/primary/primary_package_handler.h new file mode 100644 index 000000000..c1e379120 --- /dev/null +++ b/include/ur_client_library/primary/primary_package_handler.h @@ -0,0 +1,58 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- +// +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_ROBOT_DRIVER_PRIMARY_PACKAGE_HANDLER_H_INCLUDED +#define UR_ROBOT_DRIVER_PRIMARY_PACKAGE_HANDLER_H_INCLUDED + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief Interface for a class handling a primary interface package. Classes that implement this + * interface with a specific package type will be able to handle packages of this type. + */ +template +class IPrimaryPackageHandler +{ +public: + IPrimaryPackageHandler() = default; + virtual ~IPrimaryPackageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(PackageT& pkg) = 0; + +private: + /* data */ +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_PACKAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_shell_consumer.h b/include/ur_client_library/primary/primary_shell_consumer.h index 3a924646f..73d78a6ce 100644 --- a/include/ur_client_library/primary/primary_shell_consumer.h +++ b/include/ur_client_library/primary/primary_shell_consumer.h @@ -49,7 +49,7 @@ class PrimaryShellConsumer : public AbstractPrimaryConsumer } virtual bool consume(RobotState& msg) override { - LOG_INFO("---RobotState:---\n%s", msg.toString().c_str()); + // LOG_INFO("---RobotState:---\n%s", msg.toString().c_str()); return true; } virtual bool consume(ErrorCodeMessage& msg) override @@ -87,6 +87,6 @@ class PrimaryShellConsumer : public AbstractPrimaryConsumer /* data */ }; } // namespace primary_interface -} // namespace ur_driver +} // namespace urcl #endif // ifndef UR_ROBOT_DRIVER_PRIMARY_SHELL_CONSUMER_H_INCLUDED diff --git a/include/ur_client_library/ur/calibration_checker.h b/include/ur_client_library/ur/calibration_checker.h index e1eb6d5d4..f703cb11d 100644 --- a/include/ur_client_library/ur/calibration_checker.h +++ b/include/ur_client_library/ur/calibration_checker.h @@ -27,18 +27,17 @@ #ifndef UR_CLIENT_LIBRARY_UR_CALIBRATION_CHECKER_H_INCLUDED #define UR_CLIENT_LIBRARY_UR_CALIBRATION_CHECKER_H_INCLUDED -#include - +#include #include namespace urcl { /*! - * \brief The CalibrationChecker class consumes primary packages ignoring all but KinematicsInfo - * packages. These are then checked against the used kinematics to see if the correct calibration - * is used. + * \brief The CalibrationChecker checks a received KinematicsInfo package against a registered calibration hash + * value. This way we know whether the robot that sent the KinematicsInfo package matches the + * expected calibration. */ -class CalibrationChecker : public comm::IConsumer +class CalibrationChecker : public primary_interface::IPrimaryPackageHandler { public: /*! @@ -50,31 +49,6 @@ class CalibrationChecker : public comm::IConsumer product); + virtual void handle(primary_interface::KinematicsInfo& kin_info) override; /*! * \brief Used to make sure the calibration check is not performed several times. diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index da72a2d41..e5cb7843f 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -35,6 +35,7 @@ #include "ur_client_library/ur/tool_communication.h" #include "ur_client_library/ur/version_information.h" #include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/primary_client.h" #include "ur_client_library/rtde/rtde_writer.h" namespace urcl @@ -185,7 +186,7 @@ class UrDriver * * \param checksum Hash of the used kinematics information */ - void checkCalibration(const std::string& checksum); + // void checkCalibration(const std::string& checksum); /*! * \brief Getter for the RTDE writer used to write to the robot's RTDE interface. @@ -231,8 +232,10 @@ class UrDriver std::unique_ptr rtde_client_; std::unique_ptr reverse_interface_; std::unique_ptr script_sender_; - std::unique_ptr> primary_stream_; - std::unique_ptr> secondary_stream_; + + primary_interface::PrimaryClient primary_client_; + // std::unique_ptr> primary_stream_; + // std::unique_ptr> secondary_stream_; double servoj_time_; uint32_t servoj_gain_; diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index abe616908..a7838f5e2 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -25,6 +25,9 @@ */ //---------------------------------------------------------------------- +#include +#include + #include #include @@ -32,16 +35,65 @@ namespace urcl { namespace primary_interface { -PrimaryClient::PrimaryClient(const std::string& robot_ip) : robot_ip_(robot_ip) +PrimaryClient::PrimaryClient(const std::string& robot_ip, const std::string& calibration_checksum) : robot_ip_(robot_ip) { stream_.reset(new comm::URStream(robot_ip_, UR_PRIMARY_PORT)); producer_.reset(new comm::URProducer(*stream_, parser_)); producer_->setupProducer(); - consumer_.reset(new PrimaryShellConsumer()); + consumer_.reset(new PrimaryConsumer()); + std::shared_ptr calibration_checker(new CalibrationChecker(calibration_checksum)); + consumer_->setKinematicsInfoHandler(calibration_checker); pipeline_.reset(new comm::Pipeline(*producer_, consumer_.get(), "primary pipeline", notifier_)); pipeline_->run(); + + calibration_checker->isChecked(); + while (!calibration_checker->isChecked()) + { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + LOG_DEBUG("Got calibration information from robot."); +} + +bool PrimaryClient::sendScript(const std::string& script_code) +{ + if (stream_ == nullptr) + { + throw std::runtime_error("Sending script to robot requested while there is no primary interface established. This " + "should not happen."); + } + + // urscripts (snippets) must end with a newline, or otherwise the controller's runtime will + // not execute them. To avoid problems, we always just append a newline here, even if + // there may already be one. + auto program_with_newline = script_code + '\n'; + + size_t len = program_with_newline.size(); + const uint8_t* data = reinterpret_cast(program_with_newline.c_str()); + size_t written; + + if (stream_->write(data, len, written)) + { + LOG_INFO("Sent program to robot:\n%s", program_with_newline.c_str()); + return true; + } + LOG_ERROR("Could not send program to robot"); + return false; +} + +void PrimaryClient::checkCalibration(const std::string& checksum) +{ + // if (stream_ == nullptr) + //{ + // throw std::runtime_error("checkCalibration() called without a primary interface connection being established."); + //} + + // while (!consumer.isChecked()) + //{ + // ros::Duration(1).sleep(); + //} + // ROS_DEBUG_STREAM("Got calibration information from robot."); } } // namespace primary_interface -} // namespace ur_driver +} // namespace urcl diff --git a/src/ur/calibration_checker.cpp b/src/ur/calibration_checker.cpp index 0b56eec81..a3fc8f032 100644 --- a/src/ur/calibration_checker.cpp +++ b/src/ur/calibration_checker.cpp @@ -33,30 +33,22 @@ CalibrationChecker::CalibrationChecker(const std::string& expected_hash) : expected_hash_(expected_hash), checked_(false) { } -bool CalibrationChecker::consume(std::shared_ptr product) +void CalibrationChecker::handle(primary_interface::KinematicsInfo& kin_info) { - auto kin_info = std::dynamic_pointer_cast(product); - if (kin_info != nullptr) + if (kin_info.toHash() != expected_hash_) { - // LOG_INFO("%s", product->toString().c_str()); - // - if (kin_info->toHash() != expected_hash_) - { - LOG_ERROR("The calibration parameters of the connected robot don't match the ones from the given kinematics " - "config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use the " - "ur_calibration tool to extract the correct calibration from the robot and pass that into the " - "description. See " - "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] for " - "details."); - } - else - { - LOG_INFO("Calibration checked successfully."); - } - - checked_ = true; + LOG_ERROR("The calibration parameters of the connected robot don't match the ones from the given kinematics " + "config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use the " + "ur_calibration tool to extract the correct calibration from the robot and pass that into the " + "description. See " + "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] for " + "details."); + } + else + { + LOG_INFO("Calibration checked successfully."); } - return true; + checked_ = true; } } // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 56832a8af..19ed9e8f0 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -53,7 +53,8 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ std::unique_ptr tool_comm_setup, const std::string& calibration_checksum, const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, bool non_blocking_read) - : servoj_time_(0.008) + : primary_client_(robot_ip, calibration_checksum) + , servoj_time_(0.008) , servoj_gain_(servoj_gain) , servoj_lookahead_time_(servoj_lookahead_time) , reverse_interface_active_(false) @@ -65,13 +66,14 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ LOG_DEBUG("Initializing RTDE client"); rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe_file, input_recipe_file)); - primary_stream_.reset( - new comm::URStream(robot_ip_, urcl::primary_interface::UR_PRIMARY_PORT)); - secondary_stream_.reset( - new comm::URStream(robot_ip_, urcl::primary_interface::UR_SECONDARY_PORT)); - secondary_stream_->connect(); - LOG_INFO("Checking if calibration data matches connected robot."); - checkCalibration(calibration_checksum); + // primary_stream_.reset( + // new comm::URStream(robot_ip_, ur_driver::primary_interface::UR_PRIMARY_PORT)); + // secondary_stream_.reset(new comm::URStream( + // robot_ip_, ur_driver::primary_interface::UR_SECONDARY_PORT)); + // secondary_stream_->connect(); + // LOG_INFO("Checking if calibration data matches connected robot."); + // checkCalibration(calibration_checksum); + // non_blocking_read_ = non_blocking_read; get_packet_timeout_ = non_blocking_read_ ? 0 : 100; @@ -254,30 +256,6 @@ std::string UrDriver::readKeepalive() } } -void UrDriver::checkCalibration(const std::string& checksum) -{ - if (primary_stream_ == nullptr) - { - throw std::runtime_error("checkCalibration() called without a primary interface connection being established."); - } - primary_interface::PrimaryParser parser; - comm::URProducer prod(*primary_stream_, parser); - prod.setupProducer(); - - CalibrationChecker consumer(checksum); - - comm::INotifier notifier; - - comm::Pipeline pipeline(prod, &consumer, "Pipeline", notifier); - pipeline.run(); - - while (!consumer.isChecked()) - { - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - LOG_DEBUG("Got calibration information from robot."); -} - rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() { return rtde_client_->getWriter(); @@ -285,28 +263,7 @@ rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() bool UrDriver::sendScript(const std::string& program) { - if (secondary_stream_ == nullptr) - { - throw std::runtime_error("Sending script to robot requested while there is no primary interface established. This " - "should not happen."); - } - - // urscripts (snippets) must end with a newline, or otherwise the controller's runtime will - // not execute them. To avoid problems, we always just append a newline here, even if - // there may already be one. - auto program_with_newline = program + '\n'; - - size_t len = program_with_newline.size(); - const uint8_t* data = reinterpret_cast(program_with_newline.c_str()); - size_t written; - - if (secondary_stream_->write(data, len, written)) - { - LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str()); - return true; - } - LOG_ERROR("Could not send program to robot"); - return false; + return primary_client_.sendScript(program); } bool UrDriver::sendRobotProgram() From ac149ff3bb456c27622635cb4bf663c370db0689 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 16 Jun 2020 17:58:38 +0200 Subject: [PATCH 11/18] Print title over key message --- include/ur_client_library/primary/key_message_handler.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ur_client_library/primary/key_message_handler.h b/include/ur_client_library/primary/key_message_handler.h index ddec32cd5..79122d409 100644 --- a/include/ur_client_library/primary/key_message_handler.h +++ b/include/ur_client_library/primary/key_message_handler.h @@ -49,7 +49,7 @@ class KeyMessageHandler : public IPrimaryPackageHandler */ virtual void handle(KeyMessage& pkg) override { - LOG_INFO("%s", pkg.toString().c_str()); + LOG_INFO("---KeyMessage---\n%s", pkg.toString().c_str()); } private: From 9ffd1fe32d5bb74905bd9a71eb23486b1105242f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 15 Sep 2020 08:06:36 +0200 Subject: [PATCH 12/18] Added handler for ErrorCode messages --- .../primary/error_code_message_handler.h | 95 +++++++++++++++++++ .../primary/primary_consumer.h | 22 ++++- .../robot_message/error_code_message.h | 16 +++- .../robot_message/error_code_message.cpp | 6 +- 4 files changed, 131 insertions(+), 8 deletions(-) create mode 100644 include/ur_client_library/primary/error_code_message_handler.h diff --git a/include/ur_client_library/primary/error_code_message_handler.h b/include/ur_client_library/primary/error_code_message_handler.h new file mode 100644 index 000000000..f42e9c748 --- /dev/null +++ b/include/ur_client_library/primary/error_code_message_handler.h @@ -0,0 +1,95 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- +// +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 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 Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_ERROR_CODE_MESSAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_ERROR_CODE_MESSAGE_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class ErrorCodeMessageHandler : public IPrimaryPackageHandler +{ +public: + ErrorCodeMessageHandler() = default; + virtual ~ErrorCodeMessageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(ErrorCodeMessage& pkg) override + { + std::stringstream out_ss; + out_ss << "---ErrorCodeMessage---\n" << pkg.toString().c_str() << std::endl; + switch (pkg.report_level_) + { + case ReportLevel::DEBUG: + case ReportLevel::DEVL_DEBUG: + { + LOG_DEBUG("%s", out_ss.str().c_str()); + break; + } + case ReportLevel::INFO: + case ReportLevel::DEVL_INFO: + { + LOG_INFO("%s", out_ss.str().c_str()); + break; + } + case ReportLevel::WARNING: + { + LOG_WARN("%s", out_ss.str().c_str()); + break; + } + case ReportLevel::VIOLATION: + case ReportLevel::FAULT: + case ReportLevel::DEVL_VIOLATION: + case ReportLevel::DEVL_FAULT: + { + LOG_ERROR("%s", out_ss.str().c_str()); + break; + } + default: + { + std::stringstream ss; + ss << "Unknown report level: " << static_cast(pkg.report_level_) << std::endl << out_ss.str(); + LOG_ERROR("%s", ss.str().c_str()); + } + } + } + +private: + /* data */ +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_ERROR_CODE_MESSAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h index 6c78b9448..b9d3816d9 100644 --- a/include/ur_client_library/primary/primary_consumer.h +++ b/include/ur_client_library/primary/primary_consumer.h @@ -33,6 +33,7 @@ #include "ur_client_library/primary/primary_package_handler.h" #include "ur_client_library/primary/abstract_primary_consumer.h" #include "ur_client_library/primary/key_message_handler.h" +#include "ur_client_library/primary/error_code_message_handler.h" #include "ur_client_library/primary/robot_message/error_code_message.h" #include "ur_client_library/primary/robot_message/key_message.h" #include "ur_client_library/primary/robot_message/runtime_exception_message.h" @@ -57,6 +58,7 @@ class PrimaryConsumer : public AbstractPrimaryConsumer { LOG_INFO("Constructing primary consumer"); key_message_worker_.reset(new KeyMessageHandler()); + error_code_message_worker_.reset(new ErrorCodeMessageHandler()); LOG_INFO("Constructed primary consumer"); } virtual ~PrimaryConsumer() = default; @@ -71,11 +73,6 @@ class PrimaryConsumer : public AbstractPrimaryConsumer // LOG_INFO("---RobotState:---\n%s", msg.toString().c_str()); return true; } - virtual bool consume(ErrorCodeMessage& msg) override - { - LOG_INFO("---ErrorCodeMessage---\n%s", msg.toString().c_str()); - return true; - } virtual bool consume(RuntimeExceptionMessage& msg) override { LOG_INFO("---RuntimeExceptionMessage---\n%s", msg.toString().c_str()); @@ -122,6 +119,20 @@ class PrimaryConsumer : public AbstractPrimaryConsumer return false; } + /*! + * \brief Handle a ErrorCodeMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(ErrorCodeMessage& pkg) override + { + if (error_code_message_worker_ != nullptr) + { + error_code_message_worker_->handle(pkg); + return true; + } + return false; + } void setKinematicsInfoHandler(const std::shared_ptr>& handler) { kinematics_info_message_worker_ = handler; @@ -129,6 +140,7 @@ class PrimaryConsumer : public AbstractPrimaryConsumer private: std::shared_ptr> key_message_worker_; + std::shared_ptr> error_code_message_worker_; std::shared_ptr> kinematics_info_message_worker_; }; } // namespace primary_interface diff --git a/include/ur_client_library/primary/robot_message/error_code_message.h b/include/ur_client_library/primary/robot_message/error_code_message.h index df9102333..ca335f289 100644 --- a/include/ur_client_library/primary/robot_message/error_code_message.h +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -34,6 +34,20 @@ namespace urcl { namespace primary_interface { +enum class ReportLevel : int32_t +{ + DEBUG = 0, + INFO = 1, + WARNING = 2, + VIOLATION = 3, + FAULT = 4, + DEVL_DEBUG = 128, + DEVL_INFO = 129, + DEVL_WARNING = 130, + DEVL_VIOLATION = 131, + DEVL_FAULT = 132 +}; + /*! * \brief The ErrorCodeMessage class handles the error code messages sent via the primary UR interface. */ @@ -81,7 +95,7 @@ class ErrorCodeMessage : public RobotMessage int32_t message_code_; int32_t message_argument_; - int32_t report_level_; + ReportLevel report_level_; uint8_t data_type_; uint32_t data_; std::string text_; diff --git a/src/primary/robot_message/error_code_message.cpp b/src/primary/robot_message/error_code_message.cpp index 9395f87e8..074a38b90 100644 --- a/src/primary/robot_message/error_code_message.cpp +++ b/src/primary/robot_message/error_code_message.cpp @@ -40,7 +40,9 @@ bool ErrorCodeMessage::parseWith(comm::BinParser& bp) { bp.parse(message_code_); bp.parse(message_argument_); - bp.parse(report_level_); + int32_t report_level; + bp.parse(report_level); + report_level_ = static_cast(report_level); bp.parse(data_type_); bp.parse(data_); bp.parseRemainder(text_); @@ -58,7 +60,7 @@ std::string ErrorCodeMessage::toString() const std::stringstream ss; ss << "Message code: " << message_code_ << std::endl; ss << "Message argument: " << message_argument_ << std::endl; - ss << "Report level: " << report_level_ << std::endl; + ss << "Report level: " << static_cast(report_level_) << std::endl; ss << "Datatype: " << static_cast(data_type_) << std::endl; ss << "Data: " << data_ << std::endl; ss << "Text: " << text_; From 0fe945632815a52556fa6ef7d70704fefcc13ede Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 15 Sep 2020 08:13:55 +0200 Subject: [PATCH 13/18] fixup! Added TextMessage and KeyMessage --- .../ur_client_library/primary/robot_message/key_message.h | 8 ++++---- .../primary/robot_message/text_message.h | 8 ++++---- src/primary/robot_message/key_message.cpp | 2 +- src/primary/robot_message/text_message.cpp | 2 +- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/include/ur_client_library/primary/robot_message/key_message.h b/include/ur_client_library/primary/robot_message/key_message.h index 3375fe527..aadbcf390 100644 --- a/include/ur_client_library/primary/robot_message/key_message.h +++ b/include/ur_client_library/primary/robot_message/key_message.h @@ -25,8 +25,8 @@ */ //---------------------------------------------------------------------- -#ifndef UR_RTDE_DRIVER_PRIMARY_KEY_MESSAGE_H_INCLUDED -#define UR_RTDE_DRIVER_PRIMARY_KEY_MESSAGE_H_INCLUDED +#ifndef UR_CLIENT_LIBRARY_PRIMARY_KEY_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_KEY_MESSAGE_H_INCLUDED #include "ur_client_library/primary/robot_message.h" @@ -86,6 +86,6 @@ class KeyMessage : public RobotMessage std::string text_; }; } // namespace primary_interface -} // namespace ur_driver +} // namespace urcl -#endif // ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_message/text_message.h b/include/ur_client_library/primary/robot_message/text_message.h index 0f99453e3..a97ab4a85 100644 --- a/include/ur_client_library/primary/robot_message/text_message.h +++ b/include/ur_client_library/primary/robot_message/text_message.h @@ -25,8 +25,8 @@ */ //---------------------------------------------------------------------- -#ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED -#define UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED +#ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED #include "ur_client_library/primary/robot_message.h" @@ -82,6 +82,6 @@ class TextMessage : public RobotMessage std::string text_; }; } // namespace primary_interface -} // namespace ur_driver +} // namespace urcl -#endif // ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/src/primary/robot_message/key_message.cpp b/src/primary/robot_message/key_message.cpp index 2b94e2aa8..4c98bc23a 100644 --- a/src/primary/robot_message/key_message.cpp +++ b/src/primary/robot_message/key_message.cpp @@ -61,4 +61,4 @@ std::string KeyMessage::toString() const return ss.str(); } } // namespace primary_interface -} // namespace ur_driver +} // namespace urcl diff --git a/src/primary/robot_message/text_message.cpp b/src/primary/robot_message/text_message.cpp index 905dafb5c..1e544f10a 100644 --- a/src/primary/robot_message/text_message.cpp +++ b/src/primary/robot_message/text_message.cpp @@ -53,4 +53,4 @@ std::string TextMessage::toString() const return text_; } } // namespace primary_interface -} // namespace ur_driver +} // namespace urcl From c872e027d0bb02eadc5b50d5be858b1162a9d0c5 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 15 Sep 2020 08:14:28 +0200 Subject: [PATCH 14/18] fixup! Added error_code and runtime_exception message --- .../primary/robot_message/error_code_message.h | 8 ++++---- .../primary/robot_message/runtime_exception_message.h | 8 ++++---- src/primary/robot_message/error_code_message.cpp | 2 +- src/primary/robot_message/runtime_exception_message.cpp | 2 +- 4 files changed, 10 insertions(+), 10 deletions(-) diff --git a/include/ur_client_library/primary/robot_message/error_code_message.h b/include/ur_client_library/primary/robot_message/error_code_message.h index ca335f289..d8d8fe30c 100644 --- a/include/ur_client_library/primary/robot_message/error_code_message.h +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -25,8 +25,8 @@ */ //---------------------------------------------------------------------- -#ifndef UR_RTDE_DRIVER_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED -#define UR_RTDE_DRIVER_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED +#ifndef UR_CLIENT_LIBRARY_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED #include "ur_client_library/primary/robot_message.h" @@ -101,6 +101,6 @@ class ErrorCodeMessage : public RobotMessage std::string text_; }; } // namespace primary_interface -} // namespace ur_driver +} // namespace urcl -#endif // ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_message/runtime_exception_message.h b/include/ur_client_library/primary/robot_message/runtime_exception_message.h index 7b150d329..a2cb2a08a 100644 --- a/include/ur_client_library/primary/robot_message/runtime_exception_message.h +++ b/include/ur_client_library/primary/robot_message/runtime_exception_message.h @@ -25,8 +25,8 @@ */ //---------------------------------------------------------------------- -#ifndef UR_RTDE_DRIVER_PRIMARY_RUNTIME_EXCEPTION_MESSAGE_H_INCLUDED -#define UR_RTDE_DRIVER_PRIMARY_RUNTIME_EXCEPTION_MESSAGE_H_INCLUDED +#ifndef UR_CLIENT_LIBRARY_PRIMARY_RUNTIME_EXCEPTION_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_RUNTIME_EXCEPTION_MESSAGE_H_INCLUDED #include "ur_client_library/primary/robot_message.h" @@ -84,6 +84,6 @@ class RuntimeExceptionMessage : public RobotMessage std::string text_; }; } // namespace primary_interface -} // namespace ur_driver +} // namespace urcl -#endif // ifndef UR_RTDE_DRIVER_PRIMARY_TEXT_MESSAGE_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED diff --git a/src/primary/robot_message/error_code_message.cpp b/src/primary/robot_message/error_code_message.cpp index 074a38b90..8370f4480 100644 --- a/src/primary/robot_message/error_code_message.cpp +++ b/src/primary/robot_message/error_code_message.cpp @@ -67,4 +67,4 @@ std::string ErrorCodeMessage::toString() const return ss.str(); } } // namespace primary_interface -} // namespace ur_driver +} // namespace urcl diff --git a/src/primary/robot_message/runtime_exception_message.cpp b/src/primary/robot_message/runtime_exception_message.cpp index 467c475a4..86bfa99ad 100644 --- a/src/primary/robot_message/runtime_exception_message.cpp +++ b/src/primary/robot_message/runtime_exception_message.cpp @@ -59,4 +59,4 @@ std::string RuntimeExceptionMessage::toString() const return ss.str(); } } // namespace primary_interface -} // namespace ur_driver +} // namespace urcl From 9997b84119c57ec468d1896c859d4971065b504e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 15 Sep 2020 08:16:17 +0200 Subject: [PATCH 15/18] fixup! Added a draft version for a primary client --- include/ur_client_library/primary/primary_client.h | 6 +++--- include/ur_client_library/primary/primary_shell_consumer.h | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index 66e63283a..6dd0df6dd 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -24,8 +24,8 @@ * */ //---------------------------------------------------------------------- -#ifndef UR_ROBOT_DRIVER_PRIMARY_CLIENT_H_INCLUDED -#define UR_ROBOT_DRIVER_PRIMARY_CLIENT_H_INCLUDED +#ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED #include #include @@ -76,4 +76,4 @@ class PrimaryClient } // namespace primary_interface } // namespace urcl -#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_CLIENT_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_shell_consumer.h b/include/ur_client_library/primary/primary_shell_consumer.h index 73d78a6ce..329da48ce 100644 --- a/include/ur_client_library/primary/primary_shell_consumer.h +++ b/include/ur_client_library/primary/primary_shell_consumer.h @@ -26,8 +26,8 @@ */ //---------------------------------------------------------------------- -#ifndef UR_ROBOT_DRIVER_PRIMARY_SHELL_CONSUMER_H_INCLUDED -#define UR_ROBOT_DRIVER_PRIMARY_SHELL_CONSUMER_H_INCLUDED +#ifndef UR_CLIENT_LIBRARY_PRIMARY_SHELL_CONSUMER_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_SHELL_CONSUMER_H_INCLUDED #include "ur_client_library/log.h" #include "ur_client_library/primary/abstract_primary_consumer.h" @@ -89,4 +89,4 @@ class PrimaryShellConsumer : public AbstractPrimaryConsumer } // namespace primary_interface } // namespace urcl -#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_SHELL_CONSUMER_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_SHELL_CONSUMER_H_INCLUDED From 5a88fc44108a9335060c1d9b212c1e74c0fd2f2c Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 15 Sep 2020 08:18:55 +0200 Subject: [PATCH 16/18] fixup! WIP: Move primary interface logic to primary client --- include/ur_client_library/primary/key_message_handler.h | 6 +++--- include/ur_client_library/primary/primary_consumer.h | 6 +++--- include/ur_client_library/primary/primary_package_handler.h | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/ur_client_library/primary/key_message_handler.h b/include/ur_client_library/primary/key_message_handler.h index 79122d409..bc8872973 100644 --- a/include/ur_client_library/primary/key_message_handler.h +++ b/include/ur_client_library/primary/key_message_handler.h @@ -25,8 +25,8 @@ */ //---------------------------------------------------------------------- -#ifndef UR_ROBOT_DRIVER_KEY_MESSAGE_HANDLER_H_INCLUDED -#define UR_ROBOT_DRIVER_KEY_MESSAGE_HANDLER_H_INCLUDED +#ifndef UR_CLIENT_LIBRARY_KEY_MESSAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_KEY_MESSAGE_HANDLER_H_INCLUDED #include #include @@ -57,4 +57,4 @@ class KeyMessageHandler : public IPrimaryPackageHandler }; } // namespace primary_interface } // namespace urcl -#endif // ifndef UR_ROBOT_DRIVER_KEY_MESSAGE_HANDLER_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_KEY_MESSAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h index b9d3816d9..dc2c34cbe 100644 --- a/include/ur_client_library/primary/primary_consumer.h +++ b/include/ur_client_library/primary/primary_consumer.h @@ -25,8 +25,8 @@ */ //---------------------------------------------------------------------- -#ifndef UR_ROBOT_DRIVER_PRIMARY_CONSUMER_H_INCLUDED -#define UR_ROBOT_DRIVER_PRIMARY_CONSUMER_H_INCLUDED +#ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED #include "ur_client_library/log.h" #include "ur_client_library/comm/pipeline.h" @@ -146,4 +146,4 @@ class PrimaryConsumer : public AbstractPrimaryConsumer } // namespace primary_interface } // namespace urcl -#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_CONSUMER_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_package_handler.h b/include/ur_client_library/primary/primary_package_handler.h index c1e379120..8943de5e0 100644 --- a/include/ur_client_library/primary/primary_package_handler.h +++ b/include/ur_client_library/primary/primary_package_handler.h @@ -25,8 +25,8 @@ */ //---------------------------------------------------------------------- -#ifndef UR_ROBOT_DRIVER_PRIMARY_PACKAGE_HANDLER_H_INCLUDED -#define UR_ROBOT_DRIVER_PRIMARY_PACKAGE_HANDLER_H_INCLUDED +#ifndef UR_CLIENT_LIBRARY_PRIMARY_PACKAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_PACKAGE_HANDLER_H_INCLUDED namespace urcl { @@ -55,4 +55,4 @@ class IPrimaryPackageHandler }; } // namespace primary_interface } // namespace urcl -#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_PACKAGE_HANDLER_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_PACKAGE_HANDLER_H_INCLUDED From 6f87e4bf17a7255b5eaa0ee21d590096f0b188a5 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 15 Sep 2020 08:19:12 +0200 Subject: [PATCH 17/18] Corrected header guard namings --- .../ur_client_library/primary/abstract_primary_consumer.h | 6 +++--- include/ur_client_library/ur/dashboard_client.h | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/include/ur_client_library/primary/abstract_primary_consumer.h b/include/ur_client_library/primary/abstract_primary_consumer.h index 0b7cb2d54..b9344cfd3 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -25,8 +25,8 @@ */ //---------------------------------------------------------------------- -#ifndef UR_ROBOT_DRIVER_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED -#define UR_ROBOT_DRIVER_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED +#ifndef UR_CLIENT_LIBRARY_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED +#define UR_CLIENT_LIBRARY_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED #include "ur_client_library/log.h" #include "ur_client_library/comm/pipeline.h" @@ -87,4 +87,4 @@ class AbstractPrimaryConsumer : public comm::IConsumer } // namespace primary_interface } // namespace urcl -#endif // ifndef UR_ROBOT_DRIVER_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_ABSTRACT_PRIMARY_CONSUMER_H_INCLUDED diff --git a/include/ur_client_library/ur/dashboard_client.h b/include/ur_client_library/ur/dashboard_client.h index 75f26105b..db5d41255 100644 --- a/include/ur_client_library/ur/dashboard_client.h +++ b/include/ur_client_library/ur/dashboard_client.h @@ -24,8 +24,8 @@ * */ //---------------------------------------------------------------------- -#ifndef UR_ROBOT_DRIVER_DASHBOARD_CLIENT_DASHBOARD_CLIENT_H_INCLUDED -#define UR_ROBOT_DRIVER_DASHBOARD_CLIENT_DASHBOARD_CLIENT_H_INCLUDED +#ifndef UR_CLIENT_LIBRARY_DASHBOARD_CLIENT_DASHBOARD_CLIENT_H_INCLUDED +#define UR_CLIENT_LIBRARY_DASHBOARD_CLIENT_DASHBOARD_CLIENT_H_INCLUDED #include @@ -97,4 +97,4 @@ class DashboardClient : public comm::TCPSocket std::mutex write_mutex_; }; } // namespace urcl -#endif // ifndef UR_ROBOT_DRIVER_DASHBOARD_CLIENT_DASHBOARD_CLIENT_H_INCLUDED +#endif // ifndef UR_CLIENT_LIBRARY_DASHBOARD_CLIENT_DASHBOARD_CLIENT_H_INCLUDED From 3da1c2b7defdb082f9c23d9f21c47af51e2cd1bb Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 21 Feb 2022 16:30:07 +0100 Subject: [PATCH 18/18] WIP ProgramStateMessage --- CMakeLists.txt | 2 + examples/primary_pipeline.cpp | 18 ++- .../primary/abstract_primary_consumer.h | 3 + .../primary/primary_parser.h | 39 +++++++ .../primary/primary_shell_consumer.h | 10 ++ .../primary/program_state_message.h | 108 ++++++++++++++++++ .../global_variables_update_message.h | 90 +++++++++++++++ src/primary/primary_client.cpp | 20 ++-- src/primary/program_state_message.cpp | 56 +++++++++ .../global_variables_update_message.cpp | 65 +++++++++++ 10 files changed, 399 insertions(+), 12 deletions(-) create mode 100644 include/ur_client_library/primary/program_state_message.h create mode 100644 include/ur_client_library/primary/program_state_message/global_variables_update_message.h create mode 100644 src/primary/program_state_message.cpp create mode 100644 src/primary/program_state_message/global_variables_update_message.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 3924cc143..fa43640de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,8 +21,10 @@ add_library(urcl SHARED src/comm/server.cpp src/primary/primary_client.cpp src/primary/primary_package.cpp + src/primary/program_state_message.cpp src/primary/robot_message.cpp src/primary/robot_state.cpp + src/primary/program_state_message/global_variables_update_message.cpp src/primary/robot_message/error_code_message.cpp src/primary/robot_message/runtime_exception_message.cpp src/primary/robot_message/version_message.cpp diff --git a/examples/primary_pipeline.cpp b/examples/primary_pipeline.cpp index 80c49d7e6..171c752d3 100644 --- a/examples/primary_pipeline.cpp +++ b/examples/primary_pipeline.cpp @@ -27,7 +27,7 @@ #include #include -#include +#include #include #ifdef ROS_BUILD @@ -38,7 +38,7 @@ using namespace urcl; // In a real-world example it would be better to get those values from command line parameters / a better configuration // system such as Boost.Program_options -const std::string ROBOT_IP = "192.168.56.101"; +const std::string ROBOT_IP = "172.17.0.2"; int main(int argc, char* argv[]) { @@ -60,7 +60,7 @@ int main(int argc, char* argv[]) // The shell consumer will print the package contents to the shell std::unique_ptr> consumer; - consumer.reset(new comm::ShellConsumer()); + consumer.reset(new primary_interface::PrimaryShellConsumer()); // The notifer will be called at some points during connection setup / loss. This isn't fully // implemented atm. @@ -70,6 +70,18 @@ int main(int argc, char* argv[]) comm::Pipeline pipeline(prod, consumer.get(), "Pipeline", notifier); pipeline.run(); + std::this_thread::sleep_for(std::chrono::seconds(2)); + + std::string script_code = "zero_ftsensor()"; + + auto program_with_newline = script_code + '\n'; + + size_t len = program_with_newline.size(); + const uint8_t* data = reinterpret_cast(program_with_newline.c_str()); + size_t written; + + primary_stream.write(data, len, written); + // Package contents will be printed while not being interrupted // Note: Packages for which the parsing isn't implemented, will only get their raw bytes printed. while (true) diff --git a/include/ur_client_library/primary/abstract_primary_consumer.h b/include/ur_client_library/primary/abstract_primary_consumer.h index b9344cfd3..f68899ce5 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -36,6 +36,7 @@ #include "ur_client_library/primary/robot_message/text_message.h" #include "ur_client_library/primary/robot_message/version_message.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" +#include "ur_client_library/primary/program_state_message/global_variables_update_message.h" namespace urcl { @@ -80,6 +81,8 @@ class AbstractPrimaryConsumer : public comm::IConsumer virtual bool consume(TextMessage& pkg) = 0; virtual bool consume(VersionMessage& pkg) = 0; virtual bool consume(KinematicsInfo& pkg) = 0; + virtual bool consume(ProgramStateMessage& pkg) = 0; + virtual bool consume(GlobalVariablesUpdateMessage& pkg) = 0; private: /* data */ diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index 6fa31ff52..08a5eed12 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -32,6 +32,8 @@ #include "ur_client_library/primary/robot_message/runtime_exception_message.h" #include "ur_client_library/primary/robot_message/text_message.h" #include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/program_state_message/global_variables_update_message.h" + namespace urcl { @@ -138,6 +140,28 @@ class PrimaryParser : public comm::Parser break; } + case RobotPackageType::PROGRAM_STATE_MESSAGE: + { + uint64_t timestamp; + ProgramStateMessageType state_type; + LOG_DEBUG("ProgramStateMessage received"); + + bp.parse(timestamp); + bp.parse(state_type); + + LOG_DEBUG("ProgramStateMessage of type %d received", static_cast(state_type)); + + std::unique_ptr packet(programStateFromType(state_type, timestamp)); + if (!packet->parseWith(bp)) + { + LOG_ERROR("Package parsing of type %d failed!", static_cast(state_type)); + return false; + } + + results.push_back(std::move(packet)); + return true; + } + default: { LOG_DEBUG("Invalid robot package type recieved: %u", static_cast(type)); @@ -190,6 +214,21 @@ class PrimaryParser : public comm::Parser return new RobotMessage(timestamp, source, type); } } + + ProgramStateMessage* programStateFromType(ProgramStateMessageType type, uint64_t timestamp) + { + switch(type) + { + //case ProgramStateMessageType::GLOBAL_VARIABLES_SETUP: + //return new GlobalVariablesSetupMessage(timestamp); + case ProgramStateMessageType::GLOBAL_VARIABLES_UPDATE: + return new GlobalVariablesUpdateMessage(timestamp); + //case ProgramStateMessageType::TYPE_VARIABLES_UPDATE: + //return new TypeVariablesUpdateMessage(timestamp); + default: + return new ProgramStateMessage(timestamp, type); + } + } }; } // namespace primary_interface diff --git a/include/ur_client_library/primary/primary_shell_consumer.h b/include/ur_client_library/primary/primary_shell_consumer.h index 329da48ce..1c875ca72 100644 --- a/include/ur_client_library/primary/primary_shell_consumer.h +++ b/include/ur_client_library/primary/primary_shell_consumer.h @@ -82,6 +82,16 @@ class PrimaryShellConsumer : public AbstractPrimaryConsumer LOG_INFO("%s", msg.toString().c_str()); return true; } + virtual bool consume(ProgramStateMessage& msg) override + { + LOG_INFO("---ProgramStateMessage---%s", msg.toString().c_str()); + return true; + } + virtual bool consume(GlobalVariablesUpdateMessage& msg) override + { + LOG_INFO("---GlobalVariablesUpdateMessage---\n%s", msg.toString().c_str()); + return true; + } private: /* data */ diff --git a/include/ur_client_library/primary/program_state_message.h b/include/ur_client_library/primary/program_state_message.h new file mode 100644 index 000000000..8973fdbb4 --- /dev/null +++ b/include/ur_client_library/primary/program_state_message.h @@ -0,0 +1,108 @@ +// 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 Exner + * \date 2022-02-21 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_PROGRAM_STATE_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PROGRAM_STATE_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/primary_package.h" +#include "ur_client_library/primary/package_header.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief Possible ProgramStateMessage types + */ +enum class ProgramStateMessageType : uint8_t +{ + GLOBAL_VARIABLES_SETUP = 0, + GLOBAL_VARIABLES_UPDATE = 1, + TYPE_VARIABLES_UPDATE = 2, +}; + +/*! + * \brief The ProgramStateMessage class is a parent class for the different received robot messages. + */ +class ProgramStateMessage : public PrimaryPackage +{ +public: + /*! + * \brief Creates a new ProgramStateMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + */ + ProgramStateMessage(const uint64_t timestamp) : timestamp_(timestamp) + { + } + + /*! + * \brief Creates a new ProgramStateMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param message_type The package's message type + */ + ProgramStateMessage(const uint64_t timestamp, const ProgramStateMessageType state_type) + : timestamp_(timestamp), state_type_(state_type) + { + } + virtual ~ProgramStateMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized version of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + uint64_t timestamp_; + ProgramStateMessageType state_type_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif /* UR_CLIENT_LIBRARY_ROBOT_STATE_H_INCLUDED */ diff --git a/include/ur_client_library/primary/program_state_message/global_variables_update_message.h b/include/ur_client_library/primary/program_state_message/global_variables_update_message.h new file mode 100644 index 000000000..f821714d4 --- /dev/null +++ b/include/ur_client_library/primary/program_state_message/global_variables_update_message.h @@ -0,0 +1,90 @@ +// 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, Text 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 Exner exner@fzi.de + * \date 20222-02-21 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_PRIMARY_GLOBAL_VARIABLES_UPDATE_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_GLOBAL_VARIABLES_UPDATE_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/program_state_message.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief The GlobalVariablesUpdateMessage class handles the key messages sent via the primary UR interface. + */ +class GlobalVariablesUpdateMessage : public ProgramStateMessage +{ +public: + GlobalVariablesUpdateMessage() = delete; + /*! + * \brief Creates a new GlobalVariablesUpdateMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + GlobalVariablesUpdateMessage(const uint64_t timestamp) + : ProgramStateMessage(timestamp, ProgramStateMessageType::GLOBAL_VARIABLES_UPDATE) + { + } + virtual ~GlobalVariablesUpdateMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + uint16_t start_index_; + std::string variables_; + +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED + diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index a7838f5e2..be4a74eab 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -41,19 +41,21 @@ PrimaryClient::PrimaryClient(const std::string& robot_ip, const std::string& cal producer_.reset(new comm::URProducer(*stream_, parser_)); producer_->setupProducer(); - consumer_.reset(new PrimaryConsumer()); - std::shared_ptr calibration_checker(new CalibrationChecker(calibration_checksum)); - consumer_->setKinematicsInfoHandler(calibration_checker); + //consumer_.reset(new PrimaryConsumer()); + //std::shared_ptr calibration_checker(new CalibrationChecker(calibration_checksum)); + //consumer_->setKinematicsInfoHandler(calibration_checker); + std::unique_ptr> consumer; + consumer.reset(new primary_interface::PrimaryShellConsumer()); pipeline_.reset(new comm::Pipeline(*producer_, consumer_.get(), "primary pipeline", notifier_)); pipeline_->run(); - calibration_checker->isChecked(); - while (!calibration_checker->isChecked()) - { - std::this_thread::sleep_for(std::chrono::seconds(1)); - } - LOG_DEBUG("Got calibration information from robot."); + //calibration_checker->isChecked(); + //while (!calibration_checker->isChecked()) + //{ + //std::this_thread::sleep_for(std::chrono::seconds(1)); + //} + //LOG_DEBUG("Got calibration information from robot."); } bool PrimaryClient::sendScript(const std::string& script_code) diff --git a/src/primary/program_state_message.cpp b/src/primary/program_state_message.cpp new file mode 100644 index 000000000..728e1ebcc --- /dev/null +++ b/src/primary/program_state_message.cpp @@ -0,0 +1,56 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 FZI Forschungszentrum Informatik (ur_robot_driver) +// +// 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 Exner exner@fzi.de + * \date 2022-02-21 + * + */ +//---------------------------------------------------------------------- +#include + +#include "ur_client_library/primary/program_state_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +bool ProgramStateMessage::parseWith(comm::BinParser& bp) +{ + return true; +} + +bool ProgramStateMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string ProgramStateMessage::toString() const +{ + std::stringstream ss; + ss << "timestamp: " << timestamp_ << std::endl; + ss << "Type: " << static_cast(state_type_) << std::endl; + ss << PrimaryPackage::toString(); + return ss.str(); +} + +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/program_state_message/global_variables_update_message.cpp b/src/primary/program_state_message/global_variables_update_message.cpp new file mode 100644 index 000000000..649bf9785 --- /dev/null +++ b/src/primary/program_state_message/global_variables_update_message.cpp @@ -0,0 +1,65 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// Copyright 2017, 2018 Simon Rasmussen (refactor) +// +// Copyright 2015, 2016 Thomas Timm Andersen (original version) +// +// 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 Exner exner@fzi.de + * \date 2022-02-21 + * + */ +//---------------------------------------------------------------------- + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/program_state_message/global_variables_update_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +bool GlobalVariablesUpdateMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(start_index_); + bp.parseRemainder(variables_); + + return true; // not really possible to check dynamic size packets +} + +bool GlobalVariablesUpdateMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string GlobalVariablesUpdateMessage::toString() const +{ + std::stringstream ss; + ss << "start index: " << start_index_ << std::endl; + ss << "variables: (" << variables_.length() << ")"; + for (const char& c : variables_) + { + ss << std::hex << static_cast(c) << " "; + } + return ss.str(); +} +} // namespace primary_interface +} // namespace urcl +