From 4c114efc91ea669e217192915935ebc8396d7e8c Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2020 07:50:41 +0200 Subject: [PATCH 01/11] source and message types should be signed --- .../include/ur_robot_driver/primary/primary_parser.h | 2 +- .../include/ur_robot_driver/primary/robot_message.h | 6 +++--- .../ur_robot_driver/primary/robot_message/version_message.h | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h index c609c0f88..f1c91513e 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h +++ b/ur_robot_driver/include/ur_robot_driver/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/ur_robot_driver/include/ur_robot_driver/primary/robot_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message.h index a27f9cc69..93c00bf10 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/robot_message.h +++ b/ur_robot_driver/include/ur_robot_driver/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/ur_robot_driver/include/ur_robot_driver/primary/robot_message/version_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/version_message.h index 73abc09ad..545a12c46 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/version_message.h +++ b/ur_robot_driver/include/ur_robot_driver/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 6230e6c0c270fdea1ff3c3f074113c01aca1f76a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2020 07:51:44 +0200 Subject: [PATCH 02/11] Added constructor with package type for RobotMessage --- .../include/ur_robot_driver/primary/primary_parser.h | 2 +- .../include/ur_robot_driver/primary/robot_message.h | 12 ++++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h index f1c91513e..92f55c965 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h +++ b/ur_robot_driver/include/ur_robot_driver/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/ur_robot_driver/include/ur_robot_driver/primary/robot_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message.h index 93c00bf10..a1224df76 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/robot_message.h +++ b/ur_robot_driver/include/ur_robot_driver/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 7ff7b75980cfd0c624d7a314aa7fa6b2f153ae44 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2020 07:53:43 +0200 Subject: [PATCH 03/11] Initialize message_type in VersionMessage --- .../ur_robot_driver/primary/robot_message/version_message.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/version_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/version_message.h index 545a12c46..e166cbd6a 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/version_message.h +++ b/ur_robot_driver/include/ur_robot_driver/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 502530aedfa4899cf060eb77101012e819f64034 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2020 08:04:55 +0200 Subject: [PATCH 04/11] Added virtual destructor for URProducer --- ur_robot_driver/include/ur_robot_driver/comm/producer.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ur_robot_driver/include/ur_robot_driver/comm/producer.h b/ur_robot_driver/include/ur_robot_driver/comm/producer.h index 32ba2ed3d..050080fed 100644 --- a/ur_robot_driver/include/ur_robot_driver/comm/producer.h +++ b/ur_robot_driver/include/ur_robot_driver/comm/producer.h @@ -57,6 +57,8 @@ class URProducer : public IProducer { } + virtual ~URProducer() = default; + /*! * \brief Triggers the stream to connect to the robot. */ From b9dc0d2387f52c8bc758ab83d264106435136253 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Apr 2020 08:09:08 +0200 Subject: [PATCH 05/11] Added TextMessage and KeyMessage --- ur_robot_driver/CMakeLists.txt | 2 + .../ur_robot_driver/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 +++++++++++++ .../primary/robot_message/text_message.cpp | 50 +++++++++++ 6 files changed, 276 insertions(+) create mode 100644 ur_robot_driver/include/ur_robot_driver/primary/robot_message/key_message.h create mode 100644 ur_robot_driver/include/ur_robot_driver/primary/robot_message/text_message.h create mode 100644 ur_robot_driver/src/primary/robot_message/key_message.cpp create mode 100644 ur_robot_driver/src/primary/robot_message/text_message.cpp diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index eb89d7cf5..7bb093317 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -92,6 +92,8 @@ add_library(ur_robot_driver 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/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h index 92f55c965..6bd13662e 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h +++ b/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h @@ -27,6 +27,8 @@ #include "ur_robot_driver/primary/robot_state.h" #include "ur_robot_driver/primary/robot_message.h" #include "ur_robot_driver/primary/robot_state/kinematics_info.h" +#include "ur_robot_driver/primary/robot_message/key_message.h" +#include "ur_robot_driver/primary/robot_message/text_message.h" #include "ur_robot_driver/primary/robot_message/version_message.h" namespace ur_driver @@ -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/ur_robot_driver/include/ur_robot_driver/primary/robot_message/key_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/key_message.h new file mode 100644 index 000000000..50e092076 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/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/ur_robot_driver/include/ur_robot_driver/primary/robot_message/text_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/text_message.h new file mode 100644 index 000000000..531bf8365 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/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/ur_robot_driver/src/primary/robot_message/key_message.cpp b/ur_robot_driver/src/primary/robot_message/key_message.cpp new file mode 100644 index 000000000..890d0a064 --- /dev/null +++ b/ur_robot_driver/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/ur_robot_driver/src/primary/robot_message/text_message.cpp b/ur_robot_driver/src/primary/robot_message/text_message.cpp new file mode 100644 index 000000000..4927372b8 --- /dev/null +++ b/ur_robot_driver/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 a992fa3c8ebbbf2470055adad7ade13d5d756d3d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 30 Apr 2020 16:01:29 +0200 Subject: [PATCH 06/11] Added error_code and runtime_exception message Note: The error_code message changed between versions 5.1 and 5.2 --- ur_robot_driver/CMakeLists.txt | 2 + .../ur_robot_driver/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 ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h create mode 100644 ur_robot_driver/include/ur_robot_driver/primary/robot_message/runtime_exception_message.h create mode 100644 ur_robot_driver/src/primary/robot_message/error_code_message.cpp create mode 100644 ur_robot_driver/src/primary/robot_message/runtime_exception_message.cpp diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 7bb093317..9097f970a 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -91,6 +91,8 @@ add_library(ur_robot_driver 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/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h index 6bd13662e..ec0b6158a 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h +++ b/ur_robot_driver/include/ur_robot_driver/primary/primary_parser.h @@ -28,6 +28,8 @@ #include "ur_robot_driver/primary/robot_message.h" #include "ur_robot_driver/primary/robot_state/kinematics_info.h" #include "ur_robot_driver/primary/robot_message/key_message.h" +#include "ur_robot_driver/primary/robot_message/error_code_message.h" +#include "ur_robot_driver/primary/robot_message/runtime_exception_message.h" #include "ur_robot_driver/primary/robot_message/text_message.h" #include "ur_robot_driver/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/ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h new file mode 100644 index 000000000..90c1117d2 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/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_robot_driver/primary/robot_message.h" + +namespace ur_driver +{ +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/ur_robot_driver/include/ur_robot_driver/primary/robot_message/runtime_exception_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/runtime_exception_message.h new file mode 100644 index 000000000..a97ac6a53 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/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_robot_driver/primary/robot_message.h" + +namespace ur_driver +{ +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/ur_robot_driver/src/primary/robot_message/error_code_message.cpp b/ur_robot_driver/src/primary/robot_message/error_code_message.cpp new file mode 100644 index 000000000..491c35038 --- /dev/null +++ b/ur_robot_driver/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_robot_driver/log.h" +#include "ur_robot_driver/primary/robot_message/error_code_message.h" + +namespace ur_driver +{ +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/ur_robot_driver/src/primary/robot_message/runtime_exception_message.cpp b/ur_robot_driver/src/primary/robot_message/runtime_exception_message.cpp new file mode 100644 index 000000000..5912b319f --- /dev/null +++ b/ur_robot_driver/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_robot_driver/log.h" +#include "ur_robot_driver/primary/robot_message/runtime_exception_message.h" + +namespace ur_driver +{ +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 9bbba8e132f472c3c00e57310998036afebbc04e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 1 May 2020 11:16:56 +0200 Subject: [PATCH 07/11] Added consumer functions to new message types --- .../ur_robot_driver/primary/abstract_primary_consumer.h | 8 ++++++++ .../primary/robot_message/error_code_message.h | 9 +++++++++ .../ur_robot_driver/primary/robot_message/key_message.h | 9 +++++++++ .../primary/robot_message/runtime_exception_message.h | 9 +++++++++ .../ur_robot_driver/primary/robot_message/text_message.h | 9 +++++++++ .../src/primary/robot_message/error_code_message.cpp | 6 ++++++ .../src/primary/robot_message/key_message.cpp | 6 ++++++ .../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/ur_robot_driver/include/ur_robot_driver/primary/abstract_primary_consumer.h b/ur_robot_driver/include/ur_robot_driver/primary/abstract_primary_consumer.h index 39cc2cb84..7dbe5323d 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/abstract_primary_consumer.h +++ b/ur_robot_driver/include/ur_robot_driver/primary/abstract_primary_consumer.h @@ -30,6 +30,10 @@ #include "ur_robot_driver/log.h" #include "ur_robot_driver/comm/pipeline.h" +#include "ur_robot_driver/primary/robot_message/error_code_message.h" +#include "ur_robot_driver/primary/robot_message/key_message.h" +#include "ur_robot_driver/primary/robot_message/runtime_exception_message.h" +#include "ur_robot_driver/primary/robot_message/text_message.h" #include "ur_robot_driver/primary/robot_message/version_message.h" #include "ur_robot_driver/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/ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h index 90c1117d2..e311fd4e0 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h +++ b/ur_robot_driver/include/ur_robot_driver/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/ur_robot_driver/include/ur_robot_driver/primary/robot_message/key_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/key_message.h index 50e092076..590f8abeb 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/key_message.h +++ b/ur_robot_driver/include/ur_robot_driver/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/ur_robot_driver/include/ur_robot_driver/primary/robot_message/runtime_exception_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/runtime_exception_message.h index a97ac6a53..d03295b26 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/runtime_exception_message.h +++ b/ur_robot_driver/include/ur_robot_driver/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/ur_robot_driver/include/ur_robot_driver/primary/robot_message/text_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/text_message.h index 531bf8365..70e264712 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/text_message.h +++ b/ur_robot_driver/include/ur_robot_driver/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/ur_robot_driver/src/primary/robot_message/error_code_message.cpp b/ur_robot_driver/src/primary/robot_message/error_code_message.cpp index 491c35038..9c5d7f096 100644 --- a/ur_robot_driver/src/primary/robot_message/error_code_message.cpp +++ b/ur_robot_driver/src/primary/robot_message/error_code_message.cpp @@ -30,6 +30,7 @@ #include "ur_robot_driver/log.h" #include "ur_robot_driver/primary/robot_message/error_code_message.h" +#include "ur_robot_driver/primary/abstract_primary_consumer.h" namespace ur_driver { @@ -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/ur_robot_driver/src/primary/robot_message/key_message.cpp b/ur_robot_driver/src/primary/robot_message/key_message.cpp index 890d0a064..bf9f58920 100644 --- a/ur_robot_driver/src/primary/robot_message/key_message.cpp +++ b/ur_robot_driver/src/primary/robot_message/key_message.cpp @@ -30,6 +30,7 @@ #include "ur_robot_driver/log.h" #include "ur_robot_driver/primary/robot_message/key_message.h" +#include "ur_robot_driver/primary/abstract_primary_consumer.h" namespace ur_driver { @@ -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/ur_robot_driver/src/primary/robot_message/runtime_exception_message.cpp b/ur_robot_driver/src/primary/robot_message/runtime_exception_message.cpp index 5912b319f..ecf3b6bbd 100644 --- a/ur_robot_driver/src/primary/robot_message/runtime_exception_message.cpp +++ b/ur_robot_driver/src/primary/robot_message/runtime_exception_message.cpp @@ -30,6 +30,7 @@ #include "ur_robot_driver/log.h" #include "ur_robot_driver/primary/robot_message/runtime_exception_message.h" +#include "ur_robot_driver/primary/abstract_primary_consumer.h" namespace ur_driver { @@ -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/ur_robot_driver/src/primary/robot_message/text_message.cpp b/ur_robot_driver/src/primary/robot_message/text_message.cpp index 4927372b8..b05e4719d 100644 --- a/ur_robot_driver/src/primary/robot_message/text_message.cpp +++ b/ur_robot_driver/src/primary/robot_message/text_message.cpp @@ -30,6 +30,7 @@ #include "ur_robot_driver/log.h" #include "ur_robot_driver/primary/robot_message/text_message.h" +#include "ur_robot_driver/primary/abstract_primary_consumer.h" namespace ur_driver { @@ -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/ur_robot_driver/src/primary/robot_message/version_message.cpp b/ur_robot_driver/src/primary/robot_message/version_message.cpp index f7aa80685..8b11bbbb4 100644 --- a/ur_robot_driver/src/primary/robot_message/version_message.cpp +++ b/ur_robot_driver/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 efab63211ebbf9403be7d35b709643e218e66b4f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 1 May 2020 11:33:39 +0200 Subject: [PATCH 08/11] Added a draft version for a primary client --- ur_robot_driver/CMakeLists.txt | 6 +- .../ur_robot_driver/primary/primary_client.h | 59 ++++++++++++ .../primary/primary_shell_consumer.h | 92 +++++++++++++++++++ .../src/primary/primary_client.cpp | 47 ++++++++++ 4 files changed, 199 insertions(+), 5 deletions(-) create mode 100644 ur_robot_driver/include/ur_robot_driver/primary/primary_client.h create mode 100644 ur_robot_driver/include/ur_robot_driver/primary/primary_shell_consumer.h create mode 100644 ur_robot_driver/src/primary/primary_client.cpp diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 9097f970a..a1153220c 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -83,11 +83,7 @@ include_directories( add_library(ur_robot_driver src/comm/tcp_socket.cpp src/comm/server.cpp - #src/ros/service_stopper.cpp - #src/ur/commander.cpp - #src/ur/master_board.cpp - #src/ur/messages.cpp - #src/ur/robot_mode.cpp + src/primary/primary_client.cpp src/primary/primary_package.cpp src/primary/robot_message.cpp src/primary/robot_state.cpp diff --git a/ur_robot_driver/include/ur_robot_driver/primary/primary_client.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_client.h new file mode 100644 index 000000000..c78a2af39 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/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 ur_driver +{ +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/ur_robot_driver/include/ur_robot_driver/primary/primary_shell_consumer.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_shell_consumer.h new file mode 100644 index 000000000..571cb6301 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/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_robot_driver/log.h" +#include "ur_robot_driver/primary/abstract_primary_consumer.h" + +namespace ur_driver +{ +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/ur_robot_driver/src/primary/primary_client.cpp b/ur_robot_driver/src/primary/primary_client.cpp new file mode 100644 index 000000000..0eff6982e --- /dev/null +++ b/ur_robot_driver/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 ur_driver +{ +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 db508c1bc2bf1592addfbc03d6fe48035ea09543 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 4 May 2020 18:15:33 +0200 Subject: [PATCH 09/11] WIP: Move primary interface logic to primary client --- .../primary/key_message_handler.h | 60 ++++++++ .../ur_robot_driver/primary/primary_client.h | 24 ++- .../primary/primary_consumer.h | 137 ++++++++++++++++++ .../primary/primary_package_handler.h | 58 ++++++++ .../primary/primary_shell_consumer.h | 2 +- .../ur_robot_driver/ur/calibration_checker.h | 38 +---- .../include/ur_robot_driver/ur/ur_driver.h | 9 +- .../src/primary/primary_client.cpp | 56 ++++++- .../src/ur/calibration_checker.cpp | 35 ++--- ur_robot_driver/src/ur/ur_driver.cpp | 65 ++------- 10 files changed, 369 insertions(+), 115 deletions(-) create mode 100644 ur_robot_driver/include/ur_robot_driver/primary/key_message_handler.h create mode 100644 ur_robot_driver/include/ur_robot_driver/primary/primary_consumer.h create mode 100644 ur_robot_driver/include/ur_robot_driver/primary/primary_package_handler.h diff --git a/ur_robot_driver/include/ur_robot_driver/primary/key_message_handler.h b/ur_robot_driver/include/ur_robot_driver/primary/key_message_handler.h new file mode 100644 index 000000000..8362b99d1 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/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 ur_driver +{ +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 ur_driver +#endif // ifndef UR_ROBOT_DRIVER_KEY_MESSAGE_HANDLER_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/primary/primary_client.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_client.h index c78a2af39..d01f7cd71 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/primary_client.h +++ b/ur_robot_driver/include/ur_robot_driver/primary/primary_client.h @@ -31,6 +31,8 @@ #include #include #include +#include +#include namespace ur_driver { @@ -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_; diff --git a/ur_robot_driver/include/ur_robot_driver/primary/primary_consumer.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_consumer.h new file mode 100644 index 000000000..5fe366557 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/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_robot_driver/log.h" +#include "ur_robot_driver/comm/pipeline.h" +#include "ur_robot_driver/primary/primary_package_handler.h" +#include "ur_robot_driver/primary/abstract_primary_consumer.h" +#include "ur_robot_driver/primary/key_message_handler.h" +#include "ur_robot_driver/primary/robot_message/error_code_message.h" +#include "ur_robot_driver/primary/robot_message/key_message.h" +#include "ur_robot_driver/primary/robot_message/runtime_exception_message.h" +#include "ur_robot_driver/primary/robot_message/text_message.h" +#include "ur_robot_driver/primary/robot_message/version_message.h" +#include "ur_robot_driver/primary/robot_state/kinematics_info.h" + +namespace ur_driver +{ +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 ur_driver + +#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_CONSUMER_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/primary/primary_package_handler.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_package_handler.h new file mode 100644 index 000000000..25b3fb624 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/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 ur_driver +{ +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 ur_driver +#endif // ifndef UR_ROBOT_DRIVER_PRIMARY_PACKAGE_HANDLER_H_INCLUDED diff --git a/ur_robot_driver/include/ur_robot_driver/primary/primary_shell_consumer.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_shell_consumer.h index 571cb6301..f829437e0 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/primary_shell_consumer.h +++ b/ur_robot_driver/include/ur_robot_driver/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 diff --git a/ur_robot_driver/include/ur_robot_driver/ur/calibration_checker.h b/ur_robot_driver/include/ur_robot_driver/ur/calibration_checker.h index 559f0f624..f1e49077f 100644 --- a/ur_robot_driver/include/ur_robot_driver/ur/calibration_checker.h +++ b/ur_robot_driver/include/ur_robot_driver/ur/calibration_checker.h @@ -27,18 +27,17 @@ #ifndef UR_RTDE_DRIVER_UR_CALIBRATION_CHECKER_H_INCLUDED #define UR_RTDE_DRIVER_UR_CALIBRATION_CHECKER_H_INCLUDED -#include - +#include #include namespace ur_driver { /*! - * \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/ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h b/ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h index 2bd06449a..2e991a5df 100644 --- a/ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h +++ b/ur_robot_driver/include/ur_robot_driver/ur/ur_driver.h @@ -33,6 +33,7 @@ #include "ur_robot_driver/ur/tool_communication.h" #include "ur_robot_driver/ur/version_information.h" #include "ur_robot_driver/primary/robot_message/version_message.h" +#include "ur_robot_driver/primary/primary_client.h" #include "ur_robot_driver/rtde/rtde_writer.h" namespace ur_driver @@ -165,7 +166,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. @@ -211,8 +212,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/ur_robot_driver/src/primary/primary_client.cpp b/ur_robot_driver/src/primary/primary_client.cpp index 0eff6982e..c7e7679da 100644 --- a/ur_robot_driver/src/primary/primary_client.cpp +++ b/ur_robot_driver/src/primary/primary_client.cpp @@ -25,6 +25,9 @@ */ //---------------------------------------------------------------------- +#include +#include + #include #include @@ -32,16 +35,65 @@ namespace ur_driver { 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 diff --git a/ur_robot_driver/src/ur/calibration_checker.cpp b/ur_robot_driver/src/ur/calibration_checker.cpp index a6dcaee93..b1268024a 100644 --- a/ur_robot_driver/src/ur/calibration_checker.cpp +++ b/ur_robot_driver/src/ur/calibration_checker.cpp @@ -20,30 +20,23 @@ 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 ur_driver diff --git a/ur_robot_driver/src/ur/ur_driver.cpp b/ur_robot_driver/src/ur/ur_driver.cpp index 216a0dd4c..d4a23ea98 100644 --- a/ur_robot_driver/src/ur/ur_driver.cpp +++ b/ur_robot_driver/src/ur/ur_driver.cpp @@ -53,7 +53,8 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc 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 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc 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_, 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); + // 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 82f99e208139522c25f664d188a556fa2be847d6 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 16 Jun 2020 17:58:38 +0200 Subject: [PATCH 10/11] Print title over key message --- .../include/ur_robot_driver/primary/key_message_handler.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_robot_driver/include/ur_robot_driver/primary/key_message_handler.h b/ur_robot_driver/include/ur_robot_driver/primary/key_message_handler.h index 8362b99d1..0e48846ee 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/key_message_handler.h +++ b/ur_robot_driver/include/ur_robot_driver/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 9c3d99282362096fdc43b52d7c18d3f9ac026140 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 16 Jun 2020 17:59:11 +0200 Subject: [PATCH 11/11] Added handler for ErrorCode messages --- .../primary/primary_consumer.h | 22 ++++++++++++++----- .../robot_message/error_code_message.h | 16 +++++++++++++- .../robot_message/error_code_message.cpp | 6 +++-- 3 files changed, 36 insertions(+), 8 deletions(-) diff --git a/ur_robot_driver/include/ur_robot_driver/primary/primary_consumer.h b/ur_robot_driver/include/ur_robot_driver/primary/primary_consumer.h index 5fe366557..fafe07501 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/primary_consumer.h +++ b/ur_robot_driver/include/ur_robot_driver/primary/primary_consumer.h @@ -33,6 +33,7 @@ #include "ur_robot_driver/primary/primary_package_handler.h" #include "ur_robot_driver/primary/abstract_primary_consumer.h" #include "ur_robot_driver/primary/key_message_handler.h" +#include "ur_robot_driver/primary/error_code_message_handler.h" #include "ur_robot_driver/primary/robot_message/error_code_message.h" #include "ur_robot_driver/primary/robot_message/key_message.h" #include "ur_robot_driver/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/ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h index e311fd4e0..f2ca2b6e3 100644 --- a/ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h +++ b/ur_robot_driver/include/ur_robot_driver/primary/robot_message/error_code_message.h @@ -34,6 +34,20 @@ namespace ur_driver { 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/ur_robot_driver/src/primary/robot_message/error_code_message.cpp b/ur_robot_driver/src/primary/robot_message/error_code_message.cpp index 9c5d7f096..11668f31f 100644 --- a/ur_robot_driver/src/primary/robot_message/error_code_message.cpp +++ b/ur_robot_driver/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_;