From a76b9ebff24f1b9701501598e3a9c4883b213980 Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 08:48:58 +0100 Subject: [PATCH 01/14] Added debugging messages and moved when producer checks if it is running. --- include/ur_client_library/comm/pipeline.h | 5 ++++- include/ur_client_library/comm/producer.h | 17 ++++++++++------- 2 files changed, 14 insertions(+), 8 deletions(-) diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h index 5120be36..09d701e7 100644 --- a/include/ur_client_library/comm/pipeline.h +++ b/include/ur_client_library/comm/pipeline.h @@ -303,6 +303,8 @@ class Pipeline return; URCL_LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str()); + URCL_LOG_DEBUG("Producer thread joinable?! <%i>", pThread_.joinable()); + URCL_LOG_DEBUG("Consumer thread joinable?! <%i>", cThread_.joinable()); running_ = false; @@ -315,6 +317,7 @@ class Pipeline { cThread_.join(); } + URCL_LOG_DEBUG("Joined pipeline threads"); notifier_.stopped(name_); } @@ -352,7 +355,7 @@ class Pipeline void runProducer() { - URCL_LOG_DEBUG("Starting up producer"); + URCL_LOG_DEBUG("Starting up producer <%s>", name_.c_str()); std::ifstream realtime_file("/sys/kernel/realtime", std::ios::in); bool has_realtime; realtime_file >> has_realtime; diff --git a/include/ur_client_library/comm/producer.h b/include/ur_client_library/comm/producer.h index 71d36f22..22443efb 100644 --- a/include/ur_client_library/comm/producer.h +++ b/include/ur_client_library/comm/producer.h @@ -42,7 +42,7 @@ class URProducer : public IProducer private: URStream& stream_; Parser& parser_; - std::chrono::seconds timeout_; + std::chrono::milliseconds timeout_; bool running_; @@ -57,6 +57,8 @@ class URProducer : public IProducer { } + virtual ~URProducer() = default; + /*! * \brief Triggers the stream to connect to the robot. */ @@ -83,6 +85,7 @@ class URProducer : public IProducer */ void stopProducer() override { + URCL_LOG_DEBUG("Stopping producer"); running_ = false; } @@ -105,24 +108,24 @@ class URProducer : public IProducer // 4KB should be enough to hold any packet received from UR uint8_t buf[4096]; size_t read = 0; - // expoential backoff reconnects + // exponential backoff reconnects while (true) { + if (!running_) + return true; + if (stream_.read(buf, sizeof(buf), read)) { // reset sleep amount - timeout_ = std::chrono::seconds(1); + timeout_ = std::chrono::milliseconds(100); BinParser bp(buf, read); return parser_.parse(bp, products); } - if (!running_) - return true; - if (stream_.closed()) return false; - URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count()); + URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld milliseconds...", timeout_.count()); std::this_thread::sleep_for(timeout_); if (stream_.connect()) From 56a09d17c8cb3a32bc58cbb97418013d8f38726c Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 08:51:13 +0100 Subject: [PATCH 02/14] Added vector6f_t amd vectoruint8_t as types as they are needed to parse messages for primary client joint data messages. --- include/ur_client_library/comm/bin_parser.h | 13 +++++++++++++ include/ur_client_library/types.h | 2 ++ 2 files changed, 15 insertions(+) diff --git a/include/ur_client_library/comm/bin_parser.h b/include/ur_client_library/comm/bin_parser.h index cad7c632..52846c26 100644 --- a/include/ur_client_library/comm/bin_parser.h +++ b/include/ur_client_library/comm/bin_parser.h @@ -207,6 +207,19 @@ class BinParser } } + /*! + * \brief Parses the next bytes as a vector of 6 floats. + * + * \param val Reference to write the parsed value to + */ + void parse(vector6f_t& val) + { + for (size_t i = 0; i < val.size(); ++i) + { + parse(val[i]); + } + } + /*! * \brief Parses the next bytes as a vector of 6 32 bit integers. * diff --git a/include/ur_client_library/types.h b/include/ur_client_library/types.h index 7a9cbbca..02fa8a9f 100644 --- a/include/ur_client_library/types.h +++ b/include/ur_client_library/types.h @@ -28,6 +28,8 @@ namespace urcl { using vector3d_t = std::array; using vector6d_t = std::array; +using vector6f_t = std::array; +using vector6uint8_t = std::array; using vector6int32_t = std::array; using vector6uint32_t = std::array; From 16aefca0b6325e3cbb95d48febc74435fccafafe Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 08:57:04 +0100 Subject: [PATCH 03/14] Added classes for the different message types received from the primary interface. --- .../primary/program_state_message.h | 108 +++++++++++++++ .../global_variables_setup_message.h | 102 ++++++++++++++ .../global_variables_update_message.h | 99 ++++++++++++++ .../ur_client_library/primary/robot_message.h | 18 ++- .../robot_message/error_code_message.h | 122 +++++++++++++++++ .../primary/robot_message/key_message.h | 106 +++++++++++++++ .../robot_message/runtime_exception_message.h | 102 ++++++++++++++ .../primary/robot_message/text_message.h | 98 ++++++++++++++ .../primary/robot_message/version_message.h | 20 ++- .../ur_client_library/primary/robot_state.h | 4 +- .../primary/robot_state/additional_info.h | 102 ++++++++++++++ .../primary/robot_state/cartesian_info.h | 111 ++++++++++++++++ .../primary/robot_state/force_mode_data.h | 96 ++++++++++++++ .../primary/robot_state/joint_data.h | 113 ++++++++++++++++ .../primary/robot_state/kinematics_info.h | 15 +++ .../primary/robot_state/robot_mode_data.h | 125 ++++++++++++++++++ src/primary/program_state_message.cpp | 73 ++++++++++ .../global_variables_setup_message.cpp | 60 +++++++++ .../global_variables_update_message.cpp | 101 ++++++++++++++ .../robot_message/error_code_message.cpp | 70 ++++++++++ src/primary/robot_message/key_message.cpp | 64 +++++++++ .../runtime_exception_message.cpp | 62 +++++++++ src/primary/robot_message/text_message.cpp | 56 ++++++++ src/primary/robot_message/version_message.cpp | 2 +- src/primary/robot_state.cpp | 63 ++++++++- src/primary/robot_state/additional_info.cpp | 75 +++++++++++ src/primary/robot_state/cartesian_info.cpp | 85 ++++++++++++ src/primary/robot_state/force_mode_data.cpp | 70 ++++++++++ src/primary/robot_state/joint_data.cpp | 89 +++++++++++++ src/primary/robot_state/robot_mode_data.cpp | 93 +++++++++++++ 30 files changed, 2293 insertions(+), 11 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_setup_message.h create mode 100644 include/ur_client_library/primary/program_state_message/global_variables_update_message.h create mode 100644 include/ur_client_library/primary/robot_message/error_code_message.h create mode 100644 include/ur_client_library/primary/robot_message/key_message.h create mode 100644 include/ur_client_library/primary/robot_message/runtime_exception_message.h create mode 100644 include/ur_client_library/primary/robot_message/text_message.h create mode 100644 include/ur_client_library/primary/robot_state/additional_info.h create mode 100644 include/ur_client_library/primary/robot_state/cartesian_info.h create mode 100644 include/ur_client_library/primary/robot_state/force_mode_data.h create mode 100644 include/ur_client_library/primary/robot_state/joint_data.h create mode 100644 include/ur_client_library/primary/robot_state/robot_mode_data.h create mode 100644 src/primary/program_state_message.cpp create mode 100644 src/primary/program_state_message/global_variables_setup_message.cpp create mode 100644 src/primary/program_state_message/global_variables_update_message.cpp create mode 100644 src/primary/robot_message/error_code_message.cpp create mode 100644 src/primary/robot_message/key_message.cpp create mode 100644 src/primary/robot_message/runtime_exception_message.cpp create mode 100644 src/primary/robot_message/text_message.cpp create mode 100644 src/primary/robot_state/additional_info.cpp create mode 100644 src/primary/robot_state/cartesian_info.cpp create mode 100644 src/primary/robot_state/force_mode_data.cpp create mode 100644 src/primary/robot_state/joint_data.cpp create mode 100644 src/primary/robot_state/robot_mode_data.cpp 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 00000000..6e5874a6 --- /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 state_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_setup_message.h b/include/ur_client_library/primary/program_state_message/global_variables_setup_message.h new file mode 100644 index 00000000..30915d22 --- /dev/null +++ b/include/ur_client_library/primary/program_state_message/global_variables_setup_message.h @@ -0,0 +1,102 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_PRIMARY_GLOBAL_VARIABLES_SETUP_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_GLOBAL_VARIABLES_SETUP_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/program_state_message.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief The GlobalVariablesSetupMessage class handles the key messages sent via the primary UR interface. + */ +class GlobalVariablesSetupMessage : public ProgramStateMessage +{ +public: + GlobalVariablesSetupMessage() = delete; + /*! + * \brief Creates a new GlobalVariablesSetupMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + */ + GlobalVariablesSetupMessage(const uint64_t timestamp) + : ProgramStateMessage(timestamp, ProgramStateMessageType::GLOBAL_VARIABLES_SETUP) + { + } + + /*! + * \brief Creates a copy of a GlobalVariablesSetupMessage object. + * + * \param pkg The GlobalVariablesSetupMessage object to be copied + */ + GlobalVariablesSetupMessage(const GlobalVariablesSetupMessage& pkg) + : ProgramStateMessage(pkg.timestamp_, ProgramStateMessageType::GLOBAL_VARIABLES_SETUP) + { + start_index_ = pkg.start_index_; + variable_names_ = pkg.variable_names_; + } + virtual ~GlobalVariablesSetupMessage() = 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 variable_names_; +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_GLOBAL_VARIABLES_SETUP_MESSAGE_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 00000000..62548463 --- /dev/null +++ b/include/ur_client_library/primary/program_state_message/global_variables_update_message.h @@ -0,0 +1,99 @@ +// 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 + */ + GlobalVariablesUpdateMessage(const uint64_t timestamp) + : ProgramStateMessage(timestamp, ProgramStateMessageType::GLOBAL_VARIABLES_UPDATE) + { + } + + /*! + * \brief Creates a copy of a GlobalVariablesUpdateMessage object. + * + * \param pkg The GlobalVariablesUpdateMessage object to be copied + */ + GlobalVariablesUpdateMessage(const GlobalVariablesUpdateMessage& pkg) + : ProgramStateMessage(pkg.timestamp_, ProgramStateMessageType::GLOBAL_VARIABLES_SETUP) + { + start_index_ = pkg.start_index_; + variables_ = pkg.variables_; + } + 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_GLOBAL_VARIABLES_UPDATE_MESSAGE_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_message.h b/include/ur_client_library/primary/robot_message.h index 5ee0c3a2..4f7bb4f5 100644 --- a/include/ur_client_library/primary/robot_message.h +++ b/include/ur_client_library/primary/robot_message.h @@ -38,7 +38,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, @@ -63,7 +63,19 @@ 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) + { + } + + /*! + * \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; @@ -95,7 +107,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/error_code_message.h b/include/ur_client_library/primary/robot_message/error_code_message.h new file mode 100644 index 00000000..f13b016a --- /dev/null +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -0,0 +1,122 @@ +// 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_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" + +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. + */ +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) + { + } + + /*! + * \brief Creates a copy of a ErrorCodeMessage object. + * + * \param pkg The ErrorCodeMessage object to be copied + */ + ErrorCodeMessage(const ErrorCodeMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE) + { + message_code_ = pkg.message_code_; + message_argument_ = pkg.message_argument_; + report_level_ = pkg.report_level_; + data_type_ = pkg.data_type_; + data_ = pkg.data_; + text_ = pkg.text_; + } + 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 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; + + int32_t message_code_; + int32_t message_argument_; + ReportLevel report_level_; + uint8_t data_type_; + uint32_t data_; + std::string text_; +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED 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 00000000..8cbaf328 --- /dev/null +++ b/include/ur_client_library/primary/robot_message/key_message.h @@ -0,0 +1,106 @@ +// 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_CLIENT_LIBRARY_PRIMARY_KEY_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_KEY_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/robot_message.h" + +namespace urcl +{ +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) + { + } + + /*! + * \brief Creates a copy of a KeyMessage object. + * + * \param pkg The KeyMessage object to be copied + */ + KeyMessage(const KeyMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_KEY) + { + message_code_ = pkg.message_code_; + message_argument_ = pkg.message_argument_; + title_size_ = pkg.title_size_; + title_ = pkg.title_; + text_ = pkg.text_; + } + 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 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; + + int32_t message_code_; + int32_t message_argument_; + uint8_t title_size_; + std::string title_; + std::string text_; +}; +} // namespace primary_interface +} // namespace urcl + +#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 new file mode 100644 index 00000000..be2ec287 --- /dev/null +++ b/include/ur_client_library/primary/robot_message/runtime_exception_message.h @@ -0,0 +1,102 @@ +// 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_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" + +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) + { + } + + /*! + * \brief Creates a copy of a RuntimeExceptionMessage object. + * + * \param pkg The RuntimeExceptionMessage object to be copied + */ + RuntimeExceptionMessage(const RuntimeExceptionMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_RUNTIME_EXCEPTION) + { + line_number_ = pkg.line_number_; + column_number_ = pkg.column_number_; + text_ = pkg.text_; + } + 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 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; + + int32_t line_number_; + int32_t column_number_; + std::string text_; +}; +} // namespace primary_interface +} // namespace urcl + +#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 new file mode 100644 index 00000000..1d8413e8 --- /dev/null +++ b/include/ur_client_library/primary/robot_message/text_message.h @@ -0,0 +1,98 @@ +// 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_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/robot_message.h" + +namespace urcl +{ +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) + { + } + + /*! + * \brief Creates a copy of a TextMessage object. + * + * \param pkg The TextMessage object to be copied + */ + TextMessage(const TextMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_TEXT) + { + text_ = pkg.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 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; + + std::string text_; +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED 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 9663691d..84f5a136 100644 --- a/include/ur_client_library/primary/robot_message/version_message.h +++ b/include/ur_client_library/primary/robot_message/version_message.h @@ -48,9 +48,27 @@ 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, RobotMessagePackageType::ROBOT_MESSAGE_VERSION) { } + + /*! + * \brief Creates a copy of a VersionMessage object. + * + * \param pkg The VersionMessage object to be copied + */ + VersionMessage(const VersionMessage& pkg) + : RobotMessage(pkg.timestamp_, pkg.source_, RobotMessagePackageType::ROBOT_MESSAGE_VERSION) + { + project_name_length_ = pkg.project_name_length_; + project_name_ = pkg.project_name_; + major_version_ = pkg.major_version_; + minor_version_ = pkg.minor_version_; + svn_version_ = pkg.svn_version_; + build_number_ = pkg.build_number_; + build_date_ = pkg.build_date_; + } virtual ~VersionMessage() = default; /*! diff --git a/include/ur_client_library/primary/robot_state.h b/include/ur_client_library/primary/robot_state.h index 2e468a60..8e4865df 100644 --- a/include/ur_client_library/primary/robot_state.h +++ b/include/ur_client_library/primary/robot_state.h @@ -50,7 +50,9 @@ enum class RobotStateType : uint8_t CONFIGURATION_DATA = 6, FORCE_MODE_DATA = 7, ADDITIONAL_INFO = 8, - CALIBRATION_DATA = 9 + TOOL_COMMUNICATION_INFO = 11, + TOOL_MODE_INFO = 12, + SINGULARITY_INFO = 13 }; /*! diff --git a/include/ur_client_library/primary/robot_state/additional_info.h b/include/ur_client_library/primary/robot_state/additional_info.h new file mode 100644 index 00000000..819c680d --- /dev/null +++ b/include/ur_client_library/primary/robot_state/additional_info.h @@ -0,0 +1,102 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_ADDITIONAL_INFO_H_INCLUDED +#define UR_CLIENT_LIBRARY_ADDITIONAL_INFO_H_INCLUDED + +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief This messages contains information about the button states and freedrive of the robot. + */ +class AdditionalInfo : public RobotState +{ +public: + AdditionalInfo() = delete; + /*! + * \brief Creates a new AdditionalInfo object. + * + * \param type The type of RobotState message received + */ + AdditionalInfo(const RobotStateType type) : RobotState(type) + { + } + + AdditionalInfo(const AdditionalInfo& pkg) : RobotState(RobotStateType::ADDITIONAL_INFO) + { + tp_button_state_ = pkg.tp_button_state_; + freedrive_button_enabled_ = pkg.freedrive_button_enabled_; + io_enabled_freedrive_ = pkg.io_enabled_freedrive_; + reserved_length_ = pkg.reserved_length_; + reserved_ = pkg.reserved_; + } + virtual ~AdditionalInfo() = 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 specific 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; + + uint8_t tp_button_state_; + bool freedrive_button_enabled_; + bool io_enabled_freedrive_; + uint8_t reserved_length_; + std::string reserved_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_ADDITIONAL_INFO_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_state/cartesian_info.h b/include/ur_client_library/primary/robot_state/cartesian_info.h new file mode 100644 index 00000000..12eebbe7 --- /dev/null +++ b/include/ur_client_library/primary/robot_state/cartesian_info.h @@ -0,0 +1,111 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_CARTESIAN_INFO_H_INCLUDED +#define UR_CLIENT_LIBRARY_CARTESIAN_INFO_H_INCLUDED + +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief This messages contains cartesian information on the robot. The flange_coordinates vector is the flange + * coordinates of the robot and the tcp_offset_coordinates is the coordinates of the tcp offset. Both vectors are x, y, + * z, rx, ry, rz. + */ +class CartesianInfo : public RobotState +{ +public: + CartesianInfo() = delete; + /*! + * \brief Creates a new CartesianInfo object. + * + * \param type The type of RobotState message received + */ + CartesianInfo(const RobotStateType type) : RobotState(type) + { + } + + /*! + * \brief Creates a copy of a CartesianInfo object. + * + * \param pkg The CartesianInfo object to be copied + */ + CartesianInfo(const CartesianInfo& pkg) : RobotState(RobotStateType::CARTESIAN_INFO) + { + flange_coordinates_ = pkg.flange_coordinates_; + tcp_offset_coordinates_ = pkg.tcp_offset_coordinates_; + } + + virtual ~CartesianInfo() = 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 specific 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; + + /*! + * \brief Calculates a hash value of the parameters to allow for identification of a calibration. + * + * \returns A hash value of the parameters + */ + // std::string toHash() const; + + vector6d_t flange_coordinates_; + vector6d_t tcp_offset_coordinates_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_CARTESIAN_INFO_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_state/force_mode_data.h b/include/ur_client_library/primary/robot_state/force_mode_data.h new file mode 100644 index 00000000..edced1b7 --- /dev/null +++ b/include/ur_client_library/primary/robot_state/force_mode_data.h @@ -0,0 +1,96 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_FORCE_MODE_DATA_H_INCLUDED +#define UR_CLIENT_LIBRARY_FORCE_MODE_DATA_H_INCLUDED + +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief This messages contains the wrench applied to the flange?tcp? in base frame?tcp frame? + */ +class ForceModeData : public RobotState +{ +public: + ForceModeData() = delete; + /*! + * \brief Creates a new ForceModeData object. + * + * \param type The type of RobotState message received + */ + ForceModeData(const RobotStateType type) : RobotState(type) + { + } + + ForceModeData(const ForceModeData& pkg) : RobotState(RobotStateType::FORCE_MODE_DATA) + { + wrench_ = pkg.wrench_; + robot_dexterity_ = pkg.robot_dexterity_; + } + virtual ~ForceModeData() = 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 specific 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; + + vector6d_t wrench_; + double robot_dexterity_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_FORCE_MODE_DATA_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_state/joint_data.h b/include/ur_client_library/primary/robot_state/joint_data.h new file mode 100644 index 00000000..68412870 --- /dev/null +++ b/include/ur_client_library/primary/robot_state/joint_data.h @@ -0,0 +1,113 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_JOINT_DATA_H_INCLUDED +#define UR_CLIENT_LIBRARY_JOINT_DATA_H_INCLUDED + +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief This messages contains data about the joints of the robot and the motors of the joints. + */ +class JointData : public RobotState +{ +public: + JointData() = delete; + /*! + * \brief Creates a new JointData object. + * + * \param type The type of RobotState message received + */ + JointData(const RobotStateType type) : RobotState(type) + { + } + + /*! + * \brief Creates a copy of a JointData object. + * + * \param pkg The JointData object to be copied + */ + JointData(const JointData& pkg) : RobotState(RobotStateType::JOINT_DATA) + { + q_actual_ = pkg.q_actual_; + q_target_ = pkg.q_target_; + qd_actual_ = pkg.qd_actual_; + i_actual_ = pkg.i_actual_; + v_actual_ = pkg.v_actual_; + t_motor_ = pkg.t_motor_; + t_micro_ = pkg.t_micro_; + joint_mode_ = pkg.joint_mode_; + } + virtual ~JointData() = 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 specific 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; + + vector6d_t q_actual_; + vector6d_t q_target_; + vector6d_t qd_actual_; + vector6f_t i_actual_; + vector6f_t v_actual_; + vector6f_t t_motor_; + vector6f_t t_micro_; // Deprecated + vector6uint8_t joint_mode_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_JOINT_DATA_H_INCLUDED diff --git a/include/ur_client_library/primary/robot_state/kinematics_info.h b/include/ur_client_library/primary/robot_state/kinematics_info.h index 4549d4a0..e4a3a93c 100644 --- a/include/ur_client_library/primary/robot_state/kinematics_info.h +++ b/include/ur_client_library/primary/robot_state/kinematics_info.h @@ -52,6 +52,21 @@ class KinematicsInfo : public RobotState KinematicsInfo(const RobotStateType type) : RobotState(type) { } + + /*! + * \brief Creates a copy of a KinematicsInfo object. + * + * \param pkg The KinematicsInfo object to be copied + */ + KinematicsInfo(const KinematicsInfo& pkg) : RobotState(RobotStateType::JOINT_DATA) + { + checksum_ = pkg.checksum_; + dh_theta_ = pkg.dh_theta_; + dh_a_ = pkg.dh_a_; + dh_d_ = pkg.dh_d_; + dh_alpha_ = pkg.dh_alpha_; + calibration_status_ = pkg.calibration_status_; + } virtual ~KinematicsInfo() = default; /*! diff --git a/include/ur_client_library/primary/robot_state/robot_mode_data.h b/include/ur_client_library/primary/robot_state/robot_mode_data.h new file mode 100644 index 00000000..46e52ab7 --- /dev/null +++ b/include/ur_client_library/primary/robot_state/robot_mode_data.h @@ -0,0 +1,125 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_H_INCLUDED +#define UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_H_INCLUDED + +#include "ur_client_library/types.h" +#include "ur_client_library/primary/robot_state.h" +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief This messages contains data about the mode of the robot. + */ +class RobotModeData : public RobotState +{ +public: + RobotModeData() = delete; + /*! + * \brief Creates a new RobotModeData object. + * + * \param type The type of RobotState message received + */ + RobotModeData(const RobotStateType type) : RobotState(type) + { + } + + /*! + * \brief Creates a copy of a RobotModeData object. + * + * \param pkg The RobotModeData object to be copied + */ + RobotModeData(const RobotModeData& pkg) : RobotState(RobotStateType::ROBOT_MODE_DATA) + { + timestamp_ = pkg.timestamp_; + is_real_robot_connected_ = pkg.is_real_robot_connected_; + is_real_robot_enabled_ = pkg.is_real_robot_enabled_; + is_robot_power_on_ = pkg.is_robot_power_on_; + is_emergency_stopped_ = pkg.is_emergency_stopped_; + is_protective_stopped_ = pkg.is_protective_stopped_; + is_program_running_ = pkg.is_program_running_; + is_program_paused_ = pkg.is_program_paused_; + robot_mode_ = pkg.robot_mode_; + control_mode_ = pkg.control_mode_; + target_speed_fraction_ = pkg.target_speed_fraction_; + speed_scaling_ = pkg.speed_scaling_; + target_speed_fraction_limit_ = pkg.target_speed_fraction_limit_; + reserved_ = pkg.reserved_; + } + virtual ~RobotModeData() = 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 specific 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_; + bool is_real_robot_connected_; + bool is_real_robot_enabled_; + bool is_robot_power_on_; + bool is_emergency_stopped_; + bool is_protective_stopped_; + bool is_program_running_; + bool is_program_paused_; + uint8_t robot_mode_; + uint8_t control_mode_; + double target_speed_fraction_; + double speed_scaling_; + double target_speed_fraction_limit_; + std::string reserved_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_H_INCLUDED diff --git a/src/primary/program_state_message.cpp b/src/primary/program_state_message.cpp new file mode 100644 index 00000000..bbaba194 --- /dev/null +++ b/src/primary/program_state_message.cpp @@ -0,0 +1,73 @@ +// 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; + switch (state_type_) + { + case ProgramStateMessageType::GLOBAL_VARIABLES_SETUP: + { + ss << GlobalVariablesSetupMessage(timestamp_).toString(); + break; + } + case ProgramStateMessageType::GLOBAL_VARIABLES_UPDATE: + { + ss << GlobalVariablesUpdateMessage(timestamp_).toString(); + break; + } + default: + { + ss << "Unknown RobotStateType: " << static_cast(state_type_) << std::endl << ss.str(); + URCL_LOG_ERROR("%s", ss.str().c_str()); + } + } + return ss.str(); +} + +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/program_state_message/global_variables_setup_message.cpp b/src/primary/program_state_message/global_variables_setup_message.cpp new file mode 100644 index 00000000..ac78c3bb --- /dev/null +++ b/src/primary/program_state_message/global_variables_setup_message.cpp @@ -0,0 +1,60 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/program_state_message/global_variables_setup_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +bool GlobalVariablesSetupMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(start_index_); + bp.parseRemainder(variable_names_); + + return true; // not really possible to check dynamic size packets +} + +bool GlobalVariablesSetupMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string GlobalVariablesSetupMessage::toString() const +{ + std::stringstream ss; + ss << "start index: " << start_index_ << std::endl; + ss << "variable names: " << variable_names_ << std::endl; + 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 00000000..0b23b069 --- /dev/null +++ b/src/primary/program_state_message/global_variables_update_message.cpp @@ -0,0 +1,101 @@ +// 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 +{ + // int value_type; + std::stringstream ss; + ss << "start index: " << start_index_ << std::endl; + ss << "variables: (" << variables_.length() << ")"; + for (const char& c : variables_) + { + // value_type = static_cast(c); + // Parsing might not be as straight forward. Where should the global variables be stores? Maybe make functions for + // parsing different global variable types and leave it at that? + // switch (value_type) + // { + // case 0x00: + // break; + // case 0x03: + // std::cout << "const string" << std::endl; + // break; + // case 0x04: + // std::cout << "var string" << std::endl; + // break; + // case 0x0c: + // std::cout << "pose" << std::endl; + // break; + // case 0x0d: + // std::cout << "bool" << std::endl; + // break; + // case 0x0e: + // std::cout << "num" << std::endl; + // break; + // case 0x0f: + // std::cout << "int" << std::endl; + // break; + // case 0x10: + // std::cout << "float" << std::endl; + // break; + // case 0x11: + // std::cout << "list" << std::endl; + // break; + // case 0x12: + // std::cout << "matrix" << std::endl; + // break; + // } + ss << std::hex << static_cast(c) << " "; + } + ss << std::endl; + return ss.str(); +} +} // namespace primary_interface +} // namespace urcl 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 00000000..8370f448 --- /dev/null +++ b/src/primary/robot_message/error_code_message.cpp @@ -0,0 +1,70 @@ +// 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" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +bool ErrorCodeMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(message_code_); + bp.parse(message_argument_); + int32_t report_level; + bp.parse(report_level); + report_level_ = static_cast(report_level); + bp.parse(data_type_); + bp.parse(data_); + bp.parseRemainder(text_); + + 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; + ss << "Message code: " << message_code_ << std::endl; + ss << "Message argument: " << message_argument_ << 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_; + return ss.str(); +} +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/robot_message/key_message.cpp b/src/primary/robot_message/key_message.cpp new file mode 100644 index 00000000..4c98bc23 --- /dev/null +++ b/src/primary/robot_message/key_message.cpp @@ -0,0 +1,64 @@ +// 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_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 +{ +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 +} + +bool KeyMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +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 urcl 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 00000000..86bfa99a --- /dev/null +++ b/src/primary/robot_message/runtime_exception_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/runtime_exception_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.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 +} + +bool RuntimeExceptionMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +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 urcl diff --git a/src/primary/robot_message/text_message.cpp b/src/primary/robot_message/text_message.cpp new file mode 100644 index 00000000..1e544f10 --- /dev/null +++ b/src/primary/robot_message/text_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 2019-04-08 + * + */ +//---------------------------------------------------------------------- + +#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 +{ +namespace primary_interface +{ +bool TextMessage::parseWith(comm::BinParser& bp) +{ + bp.parseRemainder(text_); + + 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_; +} +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/robot_message/version_message.cpp b/src/primary/robot_message/version_message.cpp index f18e07ab..30b6fd80 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); } diff --git a/src/primary/robot_state.cpp b/src/primary/robot_state.cpp index 15d8a276..57b848ec 100644 --- a/src/primary/robot_state.cpp +++ b/src/primary/robot_state.cpp @@ -46,11 +46,64 @@ bool RobotState::consumeWith(AbstractPrimaryConsumer& consumer) std::string RobotState::toString() const { - std::stringstream ss; - ss << "Type: " << static_cast(state_type_) << std::endl; - ss << PrimaryPackage::toString(); - return ss.str(); + std::stringstream out_ss; + out_ss << "Type: " << static_cast(state_type_) << std::endl; + // ss << PrimaryPackage::toString(); + switch (state_type_) + { + case RobotStateType::ROBOT_MODE_DATA: + { + out_ss << RobotModeData(state_type_).toString(); + break; + } + case RobotStateType::JOINT_DATA: + { + out_ss << JointData(state_type_).toString(); + break; + } + case RobotStateType::TOOL_DATA: + { + URCL_LOG_INFO("TOOL_DATA"); + break; + } + case RobotStateType::MASTERBOARD_DATA: + { + URCL_LOG_INFO("MASTERBOARD_DATA"); + break; + } + case RobotStateType::CARTESIAN_INFO: + { + out_ss << CartesianInfo(state_type_).toString(); + break; + } + case RobotStateType::KINEMATICS_INFO: + { + out_ss << KinematicsInfo(state_type_).toString(); + break; + } + case RobotStateType::CONFIGURATION_DATA: + { + URCL_LOG_INFO("CONFIGURATION_DATA"); + break; + } + case RobotStateType::FORCE_MODE_DATA: + { + out_ss << ForceModeData(state_type_).toString(); + break; + } + case RobotStateType::ADDITIONAL_INFO: + { + out_ss << AdditionalInfo(state_type_).toString(); + break; + } + default: + { + std::stringstream ss; + ss << "Unknown RobotStateType: " << static_cast(state_type_) << std::endl << out_ss.str(); + URCL_LOG_ERROR("%s", ss.str().c_str()); + } + } + return out_ss.str().c_str(); } - } // namespace primary_interface } // namespace urcl diff --git a/src/primary/robot_state/additional_info.cpp b/src/primary/robot_state/additional_info.cpp new file mode 100644 index 00000000..f62330a9 --- /dev/null +++ b/src/primary/robot_state/additional_info.cpp @@ -0,0 +1,75 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_state/additional_info.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include + +namespace urcl +{ +namespace primary_interface +{ +bool AdditionalInfo::parseWith(comm::BinParser& bp) +{ + bp.parse(tp_button_state_); + bp.parse(freedrive_button_enabled_); + bp.parse(io_enabled_freedrive_); + bp.parseRemainder(reserved_); + + return true; +} + +bool AdditionalInfo::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string AdditionalInfo::toString() const +{ + std::stringstream os; + os << "tpButtonState: " << unsigned(tp_button_state_) << std::endl; + os << "freedrive_button_enabled: "; + os << freedrive_button_enabled_ << std::endl; + os << "io_enabled_freedrive: "; + os << io_enabled_freedrive_ << std::endl; + os << "reserved: (" << reserved_.length() << ") "; + for (const char& c : reserved_) + { + os << std::hex << static_cast(c) << " "; + } + os << std::endl; + + return os.str(); +} + +} // namespace primary_interface +} // namespace urcl \ No newline at end of file diff --git a/src/primary/robot_state/cartesian_info.cpp b/src/primary/robot_state/cartesian_info.cpp new file mode 100644 index 00000000..9bc46c43 --- /dev/null +++ b/src/primary/robot_state/cartesian_info.cpp @@ -0,0 +1,85 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_state/cartesian_info.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include + +namespace urcl +{ +namespace primary_interface +{ +bool CartesianInfo::parseWith(comm::BinParser& bp) +{ + bp.parse(flange_coordinates_); + bp.parse(tcp_offset_coordinates_); + + return true; +} + +bool CartesianInfo::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string CartesianInfo::toString() const +{ + std::stringstream os; + os << "flange_coordinates: ["; + for (size_t i = 0; i < flange_coordinates_.size(); ++i) + { + os << std::setprecision(15) << flange_coordinates_[i] << " "; + } + os << "]" << std::endl; + os << "tcp_offset_coordinates: ["; + for (size_t i = 0; i < tcp_offset_coordinates_.size(); ++i) + { + os << std::setprecision(15) << tcp_offset_coordinates_[i] << " "; + } + os << "]" << std::endl; + + return os.str(); +} + +// std::string CartesianInfo::toHash() const +// { +// std::stringstream ss; +// for (size_t i = 0; i < 6; ++i) +// { +// ss << flange_coordinates_[i]; +// ss << tcp_offset_coordinates_[i]; +// } +// std::hash hash_fn; +// return "calib_" + std::to_string(hash_fn(ss.str())); +// } +} // namespace primary_interface +} // namespace urcl \ No newline at end of file diff --git a/src/primary/robot_state/force_mode_data.cpp b/src/primary/robot_state/force_mode_data.cpp new file mode 100644 index 00000000..9aab121d --- /dev/null +++ b/src/primary/robot_state/force_mode_data.cpp @@ -0,0 +1,70 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_state/force_mode_data.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include + +namespace urcl +{ +namespace primary_interface +{ +bool ForceModeData::parseWith(comm::BinParser& bp) +{ + bp.parse(wrench_); + bp.parse(robot_dexterity_); + + return true; +} + +bool ForceModeData::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string ForceModeData::toString() const +{ + std::stringstream os; + os << "wrench: ["; + for (size_t i = 0; i < wrench_.size(); ++i) + { + os << std::setprecision(15) << wrench_[i] << " "; + } + os << "]" << std::endl; + os << "robot_dexterity: "; + os << std::setprecision(15) << robot_dexterity_ << std::endl; + + return os.str(); +} + +} // namespace primary_interface +} // namespace urcl \ No newline at end of file diff --git a/src/primary/robot_state/joint_data.cpp b/src/primary/robot_state/joint_data.cpp new file mode 100644 index 00000000..3d496d4d --- /dev/null +++ b/src/primary/robot_state/joint_data.cpp @@ -0,0 +1,89 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_state/joint_data.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include + +namespace urcl +{ +namespace primary_interface +{ +bool JointData::parseWith(comm::BinParser& bp) +{ + for (size_t i = 0; i < q_actual_.size(); i++) + { + bp.parse(q_actual_.at(i)); + bp.parse(q_target_.at(i)); + bp.parse(qd_actual_.at(i)); + bp.parse(i_actual_.at(i)); + bp.parse(v_actual_.at(i)); + bp.parse(t_motor_.at(i)); + bp.parse(t_micro_.at(i)); // Deprecated + bp.parse(joint_mode_.at(i)); + } + + return true; +} + +bool JointData::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string JointData::toString() const +{ + std::stringstream os; + os << "q_actual: " << q_actual_ << std::endl; + + os << "q_target: " << q_target_ << std::endl; + + os << "qd_actual: " << qd_actual_ << std::endl; + + os << "I_actual: " << i_actual_ << std::endl; + + os << "V_actual: " << v_actual_ << std::endl; + + os << "T_motor: " << t_motor_ << std::endl; + + os << "joint_mode: ["; + for (size_t i = 0; i < joint_mode_.size(); ++i) + { + os << unsigned(joint_mode_[i]) << " "; + } + os << "]" << std::endl; + + return os.str(); +} + +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/robot_state/robot_mode_data.cpp b/src/primary/robot_state/robot_mode_data.cpp new file mode 100644 index 00000000..17f6ec9d --- /dev/null +++ b/src/primary/robot_state/robot_mode_data.cpp @@ -0,0 +1,93 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_state/robot_mode_data.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include + +namespace urcl +{ +namespace primary_interface +{ +bool RobotModeData::parseWith(comm::BinParser& bp) +{ + bp.parse(timestamp_); + bp.parse(is_real_robot_connected_); + bp.parse(is_real_robot_enabled_); + bp.parse(is_robot_power_on_); + bp.parse(is_emergency_stopped_); + bp.parse(is_protective_stopped_); + bp.parse(is_program_running_); + bp.parse(is_program_paused_); + bp.parse(robot_mode_); + bp.parse(control_mode_); + bp.parse(target_speed_fraction_); + bp.parse(speed_scaling_); + bp.parse(target_speed_fraction_limit_); + bp.parseRemainder(reserved_); + + return true; +} + +bool RobotModeData::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string RobotModeData::toString() const +{ + std::stringstream os; + os << "Timestamp: " << timestamp_ << std::endl; + os << "Is real robot connected: " << is_real_robot_connected_ << std::endl; + os << "Is real robot enabled: " << is_real_robot_enabled_ << std::endl; + os << "Is robot power on: " << is_robot_power_on_ << std::endl; + os << "Is emergency stopped: " << is_emergency_stopped_ << std::endl; + os << "Is protective stopped: " << is_protective_stopped_ << std::endl; + os << "Is program running: " << is_program_running_ << std::endl; + os << "Is program paused: " << is_program_paused_ << std::endl; + os << "Robot mode: " << unsigned(robot_mode_) << std::endl; + os << "Control mode: " << unsigned(control_mode_) << std::endl; + os << "Target speed fraction: " << target_speed_fraction_ << std::endl; + os << "Speed scaling: " << speed_scaling_ << std::endl; + os << "Target speed fraction limit: " << target_speed_fraction_limit_ << std::endl; + os << "Reserved: ( " << reserved_.length() << ")"; + for (const char& c : reserved_) + { + os << std::hex << static_cast(c) << ", "; + } + os << std::endl; + + return os.str(); +} + +} // namespace primary_interface +} // namespace urcl \ No newline at end of file From c8cac884c59cf90e2b43069e4d3d8f9d522a97a4 Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 09:04:24 +0100 Subject: [PATCH 04/14] Added message handlers for the messages from the primary interface. These handlers save the data from the latest message of each type, which can then be accessed through the primary client. The handlers also enable the possibility to handle the data differently for each message. --- .../addtional_info_message_handler.h | 76 ++++++++++++ .../cartesian_info_message_handler.h | 76 ++++++++++++ .../error_code_message_handler.h | 108 ++++++++++++++++++ .../force_mode_data_message_handler.h | 76 ++++++++++++ .../global_variables_setup_message_handler.h | 76 ++++++++++++ .../global_variables_update_message_handler.h | 76 ++++++++++++ .../joint_data_message_handler.h | 76 ++++++++++++ .../message_handlers/key_message_handler.h | 73 ++++++++++++ .../robot_mode_data_message_handler.h | 76 ++++++++++++ .../runtime_exception_message_handler.h | 76 ++++++++++++ .../message_handlers/text_message_handler.h | 76 ++++++++++++ .../version_message_handler.h | 76 ++++++++++++ .../primary/primary_package_handler.h | 67 +++++++++++ 13 files changed, 1008 insertions(+) create mode 100644 include/ur_client_library/primary/message_handlers/addtional_info_message_handler.h create mode 100644 include/ur_client_library/primary/message_handlers/cartesian_info_message_handler.h create mode 100644 include/ur_client_library/primary/message_handlers/error_code_message_handler.h create mode 100644 include/ur_client_library/primary/message_handlers/force_mode_data_message_handler.h create mode 100644 include/ur_client_library/primary/message_handlers/global_variables_setup_message_handler.h create mode 100644 include/ur_client_library/primary/message_handlers/global_variables_update_message_handler.h create mode 100644 include/ur_client_library/primary/message_handlers/joint_data_message_handler.h create mode 100644 include/ur_client_library/primary/message_handlers/key_message_handler.h create mode 100644 include/ur_client_library/primary/message_handlers/robot_mode_data_message_handler.h create mode 100644 include/ur_client_library/primary/message_handlers/runtime_exception_message_handler.h create mode 100644 include/ur_client_library/primary/message_handlers/text_message_handler.h create mode 100644 include/ur_client_library/primary/message_handlers/version_message_handler.h create mode 100644 include/ur_client_library/primary/primary_package_handler.h diff --git a/include/ur_client_library/primary/message_handlers/addtional_info_message_handler.h b/include/ur_client_library/primary/message_handlers/addtional_info_message_handler.h new file mode 100644 index 00000000..1cf56c2f --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/addtional_info_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_ADDITIONAL_INFO_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_ADDITIONAL_INFO_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class AdditionalInfoHandler : public IPrimaryPackageHandler +{ +public: + AdditionalInfoHandler() = default; + virtual ~AdditionalInfoHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(AdditionalInfo& pkg) override + { + // URCL_LOG_INFO("---AdditionalInfo---\n%s", pkg.toString().c_str()); + data_.reset(new AdditionalInfo(pkg)); + } + + /*! + * \brief Get latest AdditionalInfo + * + * \return latest AdditionalInfo + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A AdditionalInfo package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_ADDITIONAL_INFO_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/cartesian_info_message_handler.h b/include/ur_client_library/primary/message_handlers/cartesian_info_message_handler.h new file mode 100644 index 00000000..7ca72698 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/cartesian_info_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_CARTESIAN_INFO_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_CARTESIAN_INFO_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class CartesianInfoHandler : public IPrimaryPackageHandler +{ +public: + CartesianInfoHandler() = default; + virtual ~CartesianInfoHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(CartesianInfo& pkg) override + { + // URCL_LOG_INFO("---CartesianInfo---\n%s", pkg.toString().c_str()); + data_.reset(new CartesianInfo(pkg)); + } + + /*! + * \brief Get latest CartesianInfo + * + * \return latest CartesianInfo + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A CartesianInfo package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_CARTESIAN_INFO_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/error_code_message_handler.h b/include/ur_client_library/primary/message_handlers/error_code_message_handler.h new file mode 100644 index 00000000..e5bd596b --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/error_code_message_handler.h @@ -0,0 +1,108 @@ +// 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: + { + URCL_LOG_DEBUG("%s", out_ss.str().c_str()); + break; + } + case ReportLevel::INFO: + case ReportLevel::DEVL_INFO: + { + URCL_LOG_INFO("%s", out_ss.str().c_str()); + break; + } + case ReportLevel::WARNING: + { + URCL_LOG_WARN("%s", out_ss.str().c_str()); + break; + } + case ReportLevel::VIOLATION: + case ReportLevel::FAULT: + case ReportLevel::DEVL_VIOLATION: + case ReportLevel::DEVL_FAULT: + { + URCL_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(); + URCL_LOG_ERROR("%s", ss.str().c_str()); + } + } + data_.reset(new ErrorCodeMessage(pkg)); + } + + /*! + * \brief Get latest ErrorCodeMessage + * + * \return latest ErrorCodeMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A ErrorCodeMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr 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/message_handlers/force_mode_data_message_handler.h b/include/ur_client_library/primary/message_handlers/force_mode_data_message_handler.h new file mode 100644 index 00000000..8f6e02c0 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/force_mode_data_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_FORCE_MODE_DATA_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_FORCE_MODE_DATA_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class ForceModeDataHandler : public IPrimaryPackageHandler +{ +public: + ForceModeDataHandler() = default; + virtual ~ForceModeDataHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(ForceModeData& pkg) override + { + // URCL_LOG_INFO("---ForceModeData---\n%s", pkg.toString().c_str()); + data_.reset(new ForceModeData(pkg)); + } + + /*! + * \brief Get latest ForceModeData + * + * \return latest ForceModeData + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A ForceModeData package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_FORCE_MODE_DATA_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/global_variables_setup_message_handler.h b/include/ur_client_library/primary/message_handlers/global_variables_setup_message_handler.h new file mode 100644 index 00000000..e545fbe2 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/global_variables_setup_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_GLOBAL_VARIABLES_SETUP_MESSAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_GLOBAL_VARIABLES_SETUP_MESSAGE_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class GlobalVariablesSetupMessageHandler : public IPrimaryPackageHandler +{ +public: + GlobalVariablesSetupMessageHandler() = default; + virtual ~GlobalVariablesSetupMessageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(GlobalVariablesSetupMessage& pkg) override + { + // URCL_LOG_INFO("---GlobalVariablesSetupMessage---\n%s", pkg.toString().c_str()); + data_.reset(new GlobalVariablesSetupMessage(pkg)); + } + + /*! + * \brief Get latest GlobalVariablesSetupMessage + * + * \return latest GlobalVariablesSetupMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A GlobalVariablesSetupMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_GLOBAL_VARIABLES_SETUP_MESSAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/global_variables_update_message_handler.h b/include/ur_client_library/primary/message_handlers/global_variables_update_message_handler.h new file mode 100644 index 00000000..5737d420 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/global_variables_update_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_GLOBAL_VARIABLES_UPDATE_MESSAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_GLOBAL_VARIABLES_UPDATE_MESSAGE_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class GlobalVariablesUpdateMessageHandler : public IPrimaryPackageHandler +{ +public: + GlobalVariablesUpdateMessageHandler() = default; + virtual ~GlobalVariablesUpdateMessageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(GlobalVariablesUpdateMessage& pkg) override + { + // URCL_LOG_INFO("---GlobalVariablesUpdateMessage---\n%s", pkg.toString().c_str()); + data_.reset(new GlobalVariablesUpdateMessage(pkg)); + } + + /*! + * \brief Get latest GlobalVariablesUpdateMessage + * + * \return latest GlobalVariablesUpdateMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A GlobalVariablesUpdateMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_GLOBAL_VARIABLES_UPDATE_MESSAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/joint_data_message_handler.h b/include/ur_client_library/primary/message_handlers/joint_data_message_handler.h new file mode 100644 index 00000000..5e756738 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/joint_data_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_JOINT_DATA_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_JOINT_DATA_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class JointDataHandler : public IPrimaryPackageHandler +{ +public: + JointDataHandler() = default; + virtual ~JointDataHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(JointData& pkg) override + { + // URCL_LOG_INFO("---JointData---\n%s", pkg.toString().c_str()); + data_.reset(new JointData(pkg)); + } + + /*! + * \brief Get latest JointData + * + * \return latest JointData + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A JointData package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_JOINT_DATA_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/key_message_handler.h b/include/ur_client_library/primary/message_handlers/key_message_handler.h new file mode 100644 index 00000000..5a4c8a96 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/key_message_handler.h @@ -0,0 +1,73 @@ +// 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_KEY_MESSAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_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 + { + // URCL_LOG_INFO("---KeyMessage---\n%s", pkg.toString().c_str()); + data_.reset(new KeyMessage(pkg)); + } + + /*! + * \brief Get latest KeyMessage + * + * \return latest KeyMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A KeyMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_KEY_MESSAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/robot_mode_data_message_handler.h b/include/ur_client_library/primary/message_handlers/robot_mode_data_message_handler.h new file mode 100644 index 00000000..edfe175a --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/robot_mode_data_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class RobotModeDataHandler : public IPrimaryPackageHandler +{ +public: + RobotModeDataHandler() = default; + virtual ~RobotModeDataHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(RobotModeData& pkg) override + { + // URCL_LOG_INFO("---RobotModeData---\n%s", pkg.toString().c_str()); + data_.reset(new RobotModeData(pkg)); + } + + /*! + * \brief Get latest RobotModeData + * + * \return latest RobotModeData + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A RobotModeData package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_ROBOT_MODE_DATA_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/runtime_exception_message_handler.h b/include/ur_client_library/primary/message_handlers/runtime_exception_message_handler.h new file mode 100644 index 00000000..ea9e45c7 --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/runtime_exception_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_RUNTIME_EXCEPTION_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_RUNTIME_EXCEPTION_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class RuntimeExceptionHandler : public IPrimaryPackageHandler +{ +public: + RuntimeExceptionHandler() = default; + virtual ~RuntimeExceptionHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(RuntimeExceptionMessage& pkg) override + { + // URCL_LOG_INFO("---RuntimeExceptionMessage---\n%s", pkg.toString().c_str()); + data_.reset(new RuntimeExceptionMessage(pkg)); + } + + /*! + * \brief Get latest RuntimeExceptionMessage + * + * \return latest RuntimeExceptionMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A RuntimeExceptionMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_RUNTIME_EXCEPTION_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/text_message_handler.h b/include/ur_client_library/primary/message_handlers/text_message_handler.h new file mode 100644 index 00000000..af6e42bd --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/text_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_TEXT_MESSAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_TEXT_MESSAGE_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class TextMessageHandler : public IPrimaryPackageHandler +{ +public: + TextMessageHandler() = default; + virtual ~TextMessageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(TextMessage& pkg) override + { + // URCL_LOG_INFO("---TextMessage---\n%s", pkg.toString().c_str()); + data_.reset(new TextMessage(pkg)); + } + + /*! + * \brief Get latest TextMessage + * + * \return latest TextMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A TextMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_TEXT_MESSAGE_HANDLER_H_INCLUDED diff --git a/include/ur_client_library/primary/message_handlers/version_message_handler.h b/include/ur_client_library/primary/message_handlers/version_message_handler.h new file mode 100644 index 00000000..0a6567ba --- /dev/null +++ b/include/ur_client_library/primary/message_handlers/version_message_handler.h @@ -0,0 +1,76 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_VERSION_MESSAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_VERSION_MESSAGE_HANDLER_H_INCLUDED + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class VersionMessageHandler : public IPrimaryPackageHandler +{ +public: + VersionMessageHandler() = default; + virtual ~VersionMessageHandler() = default; + + /*! + * \brief Actual worker function + * + * \param pkg package that should be handled + */ + virtual void handle(VersionMessage& pkg) override + { + // URCL_LOG_INFO("---VersionMessage---\n%s", pkg.toString().c_str()); + data_.reset(new VersionMessage(pkg)); + } + + /*! + * \brief Get latest VersionMessage + * + * \return latest VersionMessage + */ + virtual std::shared_ptr getData() + { + if (data_ == nullptr) + throw UrException("A VersionMessage package has not been received yet"); + return data_; + } + +private: + std::shared_ptr data_; +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_VERSION_MESSAGE_HANDLER_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 00000000..cad4c8d7 --- /dev/null +++ b/include/ur_client_library/primary/primary_package_handler.h @@ -0,0 +1,67 @@ +// 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_PRIMARY_PACKAGE_HANDLER_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_PACKAGE_HANDLER_H_INCLUDED + +#include + +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; + + /*! + * \brief Get latest PackageT + * + * \return latest PackageT + */ + virtual std::shared_ptr getData() = 0; + +private: + /* data */ +}; +} // namespace primary_interface +} // namespace urcl +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_PACKAGE_HANDLER_H_INCLUDED From 07186170cfcc931810cb88c869ff6b1229136fb7 Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 09:10:39 +0100 Subject: [PATCH 05/14] Updated primary consumer, so it can consume the new message types. --- .../primary/abstract_primary_consumer.h | 31 +- .../primary/primary_consumer.h | 319 ++++++++++++++++++ 2 files changed, 346 insertions(+), 4 deletions(-) create mode 100644 include/ur_client_library/primary/primary_consumer.h diff --git a/include/ur_client_library/primary/abstract_primary_consumer.h b/include/ur_client_library/primary/abstract_primary_consumer.h index b2c13c6d..c6fb2156 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -26,13 +26,24 @@ */ //---------------------------------------------------------------------- -#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" +#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/robot_mode_data.h" +#include "ur_client_library/primary/robot_state/joint_data.h" +#include "ur_client_library/primary/robot_state/cartesian_info.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" +#include "ur_client_library/primary/robot_state/force_mode_data.h" +#include "ur_client_library/primary/robot_state/additional_info.h" +#include "ur_client_library/primary/program_state_message/global_variables_update_message.h" +#include "ur_client_library/primary/program_state_message/global_variables_setup_message.h" namespace urcl { @@ -51,7 +62,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer virtual ~AbstractPrimaryConsumer() = default; /*! - * \brief This consume method is usally being called by the Pipeline structure. We don't + * \brief This consume method is usually being called by the Pipeline structure. We don't * necessarily need to know the specific package type here, as the packages themselves will take * care to be consumed with the correct function (visitor pattern). * @@ -71,8 +82,20 @@ 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(RobotModeData& pkg) = 0; + virtual bool consume(JointData& pkg) = 0; + virtual bool consume(CartesianInfo& pkg) = 0; virtual bool consume(KinematicsInfo& pkg) = 0; + virtual bool consume(ForceModeData& pkg) = 0; + virtual bool consume(AdditionalInfo& pkg) = 0; + virtual bool consume(ProgramStateMessage& pkg) = 0; + virtual bool consume(GlobalVariablesUpdateMessage& pkg) = 0; + virtual bool consume(GlobalVariablesSetupMessage& pkg) = 0; private: /* data */ @@ -80,4 +103,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/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h new file mode 100644 index 00000000..58b21280 --- /dev/null +++ b/include/ur_client_library/primary/primary_consumer.h @@ -0,0 +1,319 @@ +// 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_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" +#include "ur_client_library/primary/primary_package_handler.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" +#include "ur_client_library/primary/message_handlers/key_message_handler.h" +#include "ur_client_library/primary/message_handlers/version_message_handler.h" +#include "ur_client_library/primary/message_handlers/text_message_handler.h" +#include "ur_client_library/primary/message_handlers/runtime_exception_message_handler.h" +#include "ur_client_library/primary/message_handlers/error_code_message_handler.h" +#include "ur_client_library/primary/message_handlers/robot_mode_data_message_handler.h" +#include "ur_client_library/primary/message_handlers/joint_data_message_handler.h" +#include "ur_client_library/primary/message_handlers/cartesian_info_message_handler.h" +#include "ur_client_library/primary/message_handlers/force_mode_data_message_handler.h" +#include "ur_client_library/primary/message_handlers/addtional_info_message_handler.h" +#include "ur_client_library/primary/message_handlers/global_variables_update_message_handler.h" +#include "ur_client_library/primary/message_handlers/global_variables_setup_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 an 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() + { + URCL_LOG_INFO("Constructing primary consumer"); + key_message_worker_.reset(new KeyMessageHandler()); + text_message_worker_.reset(new TextMessageHandler()); + version_message_worker_.reset(new VersionMessageHandler()); + error_code_message_worker_.reset(new ErrorCodeMessageHandler()); + cartesian_info_message_worker_.reset(new CartesianInfoHandler()); + force_mode_data_message_worker_.reset(new ForceModeDataHandler()); + additional_info_message_worker_.reset(new AdditionalInfoHandler()); + robot_mode_data_message_worker_.reset(new RobotModeDataHandler()); + joint_data_message_worker_.reset(new JointDataHandler()); + runtime_exception_message_worker_.reset(new RuntimeExceptionHandler()); + global_variables_update_message_worker_.reset(new GlobalVariablesUpdateMessageHandler()); + global_variables_setup_message_worker_.reset(new GlobalVariablesSetupMessageHandler()); + URCL_LOG_INFO("Constructed primary consumer"); + } + virtual ~PrimaryConsumer() = default; + + // These need to be overriden + virtual bool consume(RobotMessage& msg) override + { + // URCL_LOG_INFO("---RobotMessage:---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(RobotState& msg) override + { + // URCL_LOG_INFO("---RobotState:---\n%s", msg.toString().c_str()); + return true; + } + virtual bool consume(ProgramStateMessage& msg) override + { + // URCL_LOG_INFO("---ProgramStateMessage---\n%s", msg.toString().c_str()); + return true; + } + + /*! + * \brief Handle a GlobalVariablesSetupMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(GlobalVariablesSetupMessage& pkg) override + { + if (global_variables_setup_message_worker_ != nullptr) + { + global_variables_setup_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a VersionMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(VersionMessage& pkg) override + { + if (version_message_worker_ != nullptr) + { + version_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a RuntimeExceptionMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(RuntimeExceptionMessage& pkg) override + { + if (runtime_exception_message_worker_ != nullptr) + { + runtime_exception_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a TextMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(TextMessage& pkg) override + { + if (text_message_worker_ != nullptr) + { + text_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a GlobalVariablesUpdateMessage + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(GlobalVariablesUpdateMessage& pkg) override + { + if (global_variables_update_message_worker_ != nullptr) + { + global_variables_update_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a RobotModeData + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(RobotModeData& pkg) override + { + if (robot_mode_data_message_worker_ != nullptr) + { + robot_mode_data_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a JointData + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(JointData& pkg) override + { + if (joint_data_message_worker_ != nullptr) + { + joint_data_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a AdditionalInfo + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(AdditionalInfo& pkg) override + { + if (additional_info_message_worker_ != nullptr) + { + additional_info_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a ForceModeData + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(ForceModeData& pkg) override + { + if (force_mode_data_message_worker_ != nullptr) + { + force_mode_data_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \brief Handle a CartesianInfo + * + * \returns True if there's a handler for this message type registered. False otherwise. + */ + virtual bool consume(CartesianInfo& pkg) override + { + if (cartesian_info_message_worker_ != nullptr) + { + cartesian_info_message_worker_->handle(pkg); + return true; + } + return false; + } + + /*! + * \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; + } + + /*! + * \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; + } + + std::shared_ptr> key_message_worker_; + std::shared_ptr> text_message_worker_; + std::shared_ptr> version_message_worker_; + std::shared_ptr> error_code_message_worker_; + std::shared_ptr> cartesian_info_message_worker_; + std::shared_ptr> kinematics_info_message_worker_; + std::shared_ptr> force_mode_data_message_worker_; + std::shared_ptr> additional_info_message_worker_; + std::shared_ptr> robot_mode_data_message_worker_; + std::shared_ptr> joint_data_message_worker_; + std::shared_ptr> runtime_exception_message_worker_; + std::shared_ptr> global_variables_update_message_worker_; + std::shared_ptr> global_variables_setup_message_worker_; +}; +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED From be9a6941339af38ea89118bdfc4996239e40398b Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 09:11:26 +0100 Subject: [PATCH 06/14] Updated primary parser, so it parses the messages based on the type of message received. --- .../primary/primary_parser.h | 84 +++++++++++++++---- 1 file changed, 69 insertions(+), 15 deletions(-) diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index 3dfba100..f4654685 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -26,8 +26,20 @@ #include "ur_client_library/primary/package_header.h" #include "ur_client_library/primary/robot_state.h" #include "ur_client_library/primary/robot_message.h" +#include "ur_client_library/primary/robot_state/robot_mode_data.h" +#include "ur_client_library/primary/robot_state/joint_data.h" +#include "ur_client_library/primary/robot_state/cartesian_info.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" +#include "ur_client_library/primary/robot_state/force_mode_data.h" +#include "ur_client_library/primary/robot_state/additional_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" +#include "ur_client_library/primary/program_state_message/global_variables_update_message.h" +#include "ur_client_library/primary/program_state_message/global_variables_setup_message.h" +#include namespace urcl { @@ -56,6 +68,7 @@ class PrimaryParser : public comm::Parser { int32_t packet_size; RobotPackageType type; + bp.parse(packet_size); bp.parse(type); @@ -115,7 +128,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); @@ -134,9 +147,31 @@ class PrimaryParser : public comm::Parser break; } + case RobotPackageType::PROGRAM_STATE_MESSAGE: + { + uint64_t timestamp; + ProgramStateMessageType state_type; + URCL_LOG_DEBUG("ProgramStateMessage received"); + + bp.parse(timestamp); + bp.parse(state_type); + + URCL_LOG_DEBUG("ProgramStateMessage of type %d received", static_cast(state_type)); + + std::unique_ptr packet(programStateFromType(state_type, timestamp)); + if (!packet->parseWith(bp)) + { + URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(state_type)); + return false; + } + + results.push_back(std::move(packet)); + return true; + } + default: { - URCL_LOG_DEBUG("Invalid robot package type recieved: %u", static_cast(type)); + URCL_LOG_DEBUG("Invalid robot package type received: %u", static_cast(type)); bp.consume(); return true; } @@ -149,14 +184,18 @@ class PrimaryParser : public comm::Parser { switch (type) { - /*case robot_state_type::ROBOT_MODE_DATA: - // SharedRobotModeData* rmd = new SharedRobotModeData(); - - //return new rmd; - case robot_state_type::MASTERBOARD_DATA: - return new MBD;*/ + case RobotStateType::ROBOT_MODE_DATA: + return new RobotModeData(type); + case RobotStateType::JOINT_DATA: + return new JointData(type); + case RobotStateType::CARTESIAN_INFO: + return new CartesianInfo(type); case RobotStateType::KINEMATICS_INFO: return new KinematicsInfo(type); + case RobotStateType::FORCE_MODE_DATA: + return new ForceModeData(type); + case RobotStateType::ADDITIONAL_INFO: + return new AdditionalInfo(type); default: return new RobotState(type); } @@ -166,16 +205,31 @@ class PrimaryParser : public comm::Parser { switch (type) { - /*case robot_state_type::ROBOT_MODE_DATA: - // SharedRobotModeData* rmd = new SharedRobotModeData(); - - //return new rmd; - case robot_state_type::MASTERBOARD_DATA: - return new MBD;*/ case RobotMessagePackageType::ROBOT_MESSAGE_VERSION: return new VersionMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_TEXT: + 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); + } + } + + 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); default: - return new RobotMessage(timestamp, source); + return new ProgramStateMessage(timestamp, type); } } }; From 91b4f27b6544757b4dfaf0850efe6272dd1240a8 Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 09:13:17 +0100 Subject: [PATCH 07/14] Updated calibration checker to double as both the message handler for the kinematics info message type and a calibration checker. It is still possible to add a separate calibration checker, if that is desired. --- .../ur/calibration_checker.h | 63 +++++++++---------- src/ur/calibration_checker.cpp | 26 +++++--- 2 files changed, 46 insertions(+), 43 deletions(-) diff --git a/include/ur_client_library/ur/calibration_checker.h b/include/ur_client_library/ur/calibration_checker.h index d68efed5..69dc4ec7 100644 --- a/include/ur_client_library/ur/calibration_checker.h +++ b/include/ur_client_library/ur/calibration_checker.h @@ -28,18 +28,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: /*! @@ -51,40 +50,15 @@ 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. @@ -107,10 +81,33 @@ class CalibrationChecker : public comm::IConsumer getData() + { + if (data_ == nullptr) + throw UrException("A KinematicsInfo package has not been received yet"); + return data_; + } + + /*! + * \brief Returns whether the calibration check was successful. + * + * \param expected_hash The hash expected to match the hash on the robot + * + * \returns True if the robot's expected_hash matches the one given to the checker. False + * if it doesn't match or the check was not yet performed. + */ + bool checkCalibration(const std::string& expected_hash); + private: std::string expected_hash_; bool checked_; bool matches_; + std::shared_ptr data_; }; } // namespace urcl diff --git a/src/ur/calibration_checker.cpp b/src/ur/calibration_checker.cpp index b7da09ed..870924c7 100644 --- a/src/ur/calibration_checker.cpp +++ b/src/ur/calibration_checker.cpp @@ -34,18 +34,24 @@ CalibrationChecker::CalibrationChecker(const std::string& expected_hash) : expected_hash_(expected_hash), checked_(false), matches_(false) { } -bool CalibrationChecker::consume(std::shared_ptr product) + +void CalibrationChecker::handle(primary_interface::KinematicsInfo& kin_info) +{ + // URCL_LOG_INFO("%s", kin_info.toString().c_str()); + // + matches_ = kin_info.toHash() == expected_hash_; + + checked_ = true; + + data_.reset(new primary_interface::KinematicsInfo(kin_info)); +} + +bool CalibrationChecker::checkCalibration(const std::string& expected_hash) { - auto kin_info = std::dynamic_pointer_cast(product); - if (kin_info != nullptr) - { - // URCL_LOG_INFO("%s", product->toString().c_str()); - // - matches_ = kin_info->toHash() == expected_hash_; + matches_ = expected_hash == expected_hash_; - checked_ = true; - } + checked_ = true; - return true; + return matches_; } } // namespace urcl From 92eb009dd09ad4806e37ee44d1fb6e1419f56fb0 Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 09:21:22 +0100 Subject: [PATCH 08/14] Added the new primary client. The primary client can through its consumer get the message handlers and from the message handlers get the latest message of each type. The primary client checks whether the robot is in remote control or not, as it should not be able to send script if the robot is not in remote control. This is not true if the robot is simulated. Therefore, when constructing a primary client instance it is needed to specify if the robot is simulated or real. I was not able to find a way for the primary client to check whether it was real or simulated automatically. The primary client now automatically checks whether the checksum is correct when starting. --- CMakeLists.txt | 13 + .../primary/primary_client.h | 205 +++++++++++++++ src/primary/primary_client.cpp | 246 ++++++++++++++++++ 3 files changed, 464 insertions(+) create mode 100644 include/ur_client_library/primary/primary_client.h create mode 100644 src/primary/primary_client.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6ebf3264..fe2d2193 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -23,11 +23,24 @@ add_library(urcl SHARED src/control/script_sender.cpp src/control/trajectory_point_interface.cpp src/control/script_command_interface.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/program_state_message/global_variables_setup_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 + src/primary/robot_message/text_message.cpp + src/primary/robot_message/key_message.cpp src/primary/robot_state/kinematics_info.cpp + src/primary/robot_state/robot_mode_data.cpp + src/primary/robot_state/joint_data.cpp + src/primary/robot_state/cartesian_info.cpp + src/primary/robot_state/force_mode_data.cpp + src/primary/robot_state/additional_info.cpp src/rtde/control_package_pause.cpp src/rtde/control_package_setup_inputs.cpp src/rtde/control_package_setup_outputs.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 00000000..7f3962df --- /dev/null +++ b/include/ur_client_library/primary/primary_client.h @@ -0,0 +1,205 @@ +// 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_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class PrimaryClient +{ +public: + PrimaryClient() = delete; + PrimaryClient(const std::string& robot_ip, const std::string& calibration_checksum); + // virtual ~PrimaryClient() = default; + virtual ~PrimaryClient(); + + /*! + * \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 Configures the primary client + * + * Creates a connection to the stream and sets up producer, consumer and pipeline + */ + bool configure(); + + /*! + * \brief Checks if the robot is in local or remote control + * + * Checks for package with error code determining if robot is in remote or local control + */ + void checkRemoteLocalControl(); + + /*! + * \brief Stops the primary client + * + * Closes the thread checking for remote or local control + */ + void stop(); + + /*! + * \brief Returns whether the robot is in remote or local control + * + * Returns whether the robot is in remote or local control + */ + bool isInRemoteControl(); + + /*! + * \brief Returns latest AdditionalInfo message + * + * Returns latest AdditionalInfo message + */ + std::shared_ptr getAdditionalInfo(); + + /*! + * \brief Returns latest CartesianInfo message + * + * Returns latest CartesianInfo message + */ + std::shared_ptr getCartesianInfo(); + + /*! + * \brief Returns latest ForceModeData message + * + * Returns latest ForceModeData message + */ + std::shared_ptr getForceModeData(); + + /*! + * \brief Returns latest JointData message + * + * Returns latest JointData message + */ + std::shared_ptr getJointData(); + + /*! + * \brief Returns latest RobotModeData message + * + * Returns latest RobotModeData message + */ + std::shared_ptr getRobotModeData(); + + /*! + * \brief Returns latest KeyMessage message + * + * Returns latest KeyMessage message + */ + std::shared_ptr getKeyMessage(); + + /*! + * \brief Returns latest ErrorCodeMessage message + * + * Returns latest ErrorCodeMessage message + */ + std::shared_ptr getErrorCodeMessage(); + + /*! + * \brief Returns latest RuntimeExceptionMessage message + * + * Returns latest RuntimeExceptionMessage message + */ + std::shared_ptr getRuntimeExceptionMessage(); + + /*! + * \brief Returns latest TextMessage message + * + * Returns latest TextMessage message + */ + std::shared_ptr getTextMessage(); + + /*! + * \brief Returns latest VersionMessage message + * + * Returns latest VersionMessage message + */ + std::shared_ptr getVersionMessage(); + + /*! + * \brief Returns latest GlobalVariablesSetupMessage message + * + * Returns latest GlobalVariablesSetupMessage message + */ + std::shared_ptr getGlobalVariablesSetupMessage(); + + /*! + * \brief Returns Calibration checker + * + * Returns Calibration checker + */ + std::shared_ptr getCalibrationChecker(); + + /*! + * \brief Set whether the robot is simulated or real + */ + void setSimulated(bool value); + + /*! + * \brief Check if the robot is simulated or real + * + * \returns true if robot is simulated, false otherwise. + */ + bool getSimulated(); + +private: + std::string robot_ip_; + int port_; + PrimaryParser parser_; + comm::INotifier notifier_; + bool connected_; + bool simulated_; + std::atomic running_, in_remote_control_; + std::unique_ptr consumer_; + std::unique_ptr> producer_; + std::unique_ptr> stream_; + std::unique_ptr> pipeline_; + std::unique_ptr dashboard_client_; + std::shared_ptr calibration_checker_; + std::thread rc_thread_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp new file mode 100644 index 00000000..2cc8d9c7 --- /dev/null +++ b/src/primary/primary_client.cpp @@ -0,0 +1,246 @@ +// 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 + +#include + +#include + +namespace urcl +{ +namespace primary_interface +{ +PrimaryClient::PrimaryClient(const std::string& robot_ip, const std::string& calibration_checksum) + : robot_ip_(robot_ip), port_(UR_PRIMARY_PORT), simulated_(false) +{ + stream_.reset(new comm::URStream(robot_ip_, port_)); + producer_.reset(new comm::URProducer(*stream_, parser_)); + producer_->setupProducer(); + + consumer_.reset(new PrimaryConsumer()); + calibration_checker_.reset(new CalibrationChecker(calibration_checksum)); + consumer_->setKinematicsInfoHandler(calibration_checker_); + + pipeline_.reset(new comm::Pipeline(*producer_, consumer_.get(), "primary pipeline", notifier_)); + pipeline_->run(); + in_remote_control_ = false; + running_ = true; + + rc_thread_ = std::thread(&PrimaryClient::checkRemoteLocalControl, this); + + std::chrono::duration time_done(0); + std::chrono::duration timeout = std::chrono::seconds(1); + std::chrono::duration retry_period = std::chrono::milliseconds(10); + while (!calibration_checker_->isChecked() || time_done < timeout) + { + std::this_thread::sleep_for(retry_period); + time_done += retry_period; + } + if (calibration_checker_->checkSuccessful()) + { + URCL_LOG_INFO("Calibration checked successfully."); + } + else + { + URCL_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."); + } +} + +PrimaryClient::~PrimaryClient() +{ + URCL_LOG_DEBUG("Destructing primary client"); + stop(); +} + +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 (!in_remote_control_ && !simulated_) + { + URCL_LOG_ERROR("Not in remote control. Cannot send script whilst in local control"); + return false; + } + + if (stream_->write(data, len, written)) + { + URCL_LOG_INFO("Sent program to robot:\n%s", program_with_newline.c_str()); + return true; + } + URCL_LOG_ERROR("Could not send program to robot"); + return false; +} + +bool PrimaryClient::configure() +{ + URCL_LOG_DEBUG("Reconnecting to stream on %s:%d", robot_ip_.c_str(), port_); + pipeline_->stop(); + + if (stream_->getState() == comm::SocketState::Connected) + stream_->disconnect(); + + bool res = stream_->connect(); + pipeline_->run(); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + return res; +} + +void PrimaryClient::checkRemoteLocalControl() +{ + dashboard_client_.reset(new urcl::DashboardClient(robot_ip_)); + dashboard_client_->connect(); + bool old_val = dashboard_client_->sendAndReceive("is in remote control\n") == "true"; + while (running_) + { + if (dashboard_client_->getState() == comm::SocketState::Connected) + { + std::string answer = dashboard_client_->sendAndReceive("is in remote control\n"); + in_remote_control_ = answer == "true"; + + if (old_val != in_remote_control_) + configure(); + } + else + { + URCL_LOG_INFO("Reconnecting to Dashboard Server"); + dashboard_client_->connect(); + } + old_val = in_remote_control_; + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } +} + +void PrimaryClient::stop() +{ + running_ = false; + if (rc_thread_.joinable()) + { + rc_thread_.join(); + } + pipeline_->stop(); + stream_->disconnect(); +} + +bool PrimaryClient::isInRemoteControl() +{ + return in_remote_control_; +} + +std::shared_ptr PrimaryClient::getAdditionalInfo() +{ + return consumer_->additional_info_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getCartesianInfo() +{ + return consumer_->cartesian_info_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getForceModeData() +{ + return consumer_->force_mode_data_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getJointData() +{ + return consumer_->joint_data_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getRobotModeData() +{ + return consumer_->robot_mode_data_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getKeyMessage() +{ + return consumer_->key_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getErrorCodeMessage() +{ + return consumer_->error_code_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getRuntimeExceptionMessage() +{ + return consumer_->runtime_exception_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getTextMessage() +{ + return consumer_->text_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getVersionMessage() +{ + return consumer_->version_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getGlobalVariablesSetupMessage() +{ + return consumer_->global_variables_setup_message_worker_->getData(); +} + +std::shared_ptr PrimaryClient::getCalibrationChecker() +{ + return calibration_checker_; +} + +void PrimaryClient::setSimulated(bool value) +{ + simulated_ = value; +} + +bool PrimaryClient::getSimulated() +{ + return simulated_; +} +} // namespace primary_interface +} // namespace urcl From 3f00c2cedf0a2f48d6ff6bf1c8384b2a9e7d515d Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 09:26:34 +0100 Subject: [PATCH 09/14] Updated ur_driver to be compatible with new primary client. The ur_driver now also checks if the checksum is correct when an instance is made, as it makes an instance of the primary client, which checks when it is instantiated. Also added debugging message to tcp socket, which specifies which socket is already connected. --- include/ur_client_library/ur/ur_driver.h | 35 ++++++--- src/comm/tcp_socket.cpp | 4 +- src/ur/ur_driver.cpp | 91 ++++-------------------- 3 files changed, 42 insertions(+), 88 deletions(-) diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 16ce45ea..f6091396 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -38,6 +38,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 @@ -112,6 +113,7 @@ class UrDriver * \param tool_comm_setup Configuration for using the tool communication. * \param calibration_checksum Expected checksum of calibration. Will be matched against the * calibration reported by the robot. + * \param simulated Set the robot as simulated or real. * \param reverse_port Port that will be opened by the driver to allow direct communication between the driver * and the robot controller. * \param script_sender_port The driver will offer an interface to receive the program's URScript on this port. If @@ -129,9 +131,10 @@ class UrDriver UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, std::unique_ptr tool_comm_setup, const std::string& calibration_checksum = "", - const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, - double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "", - const uint32_t trajectory_port = 50003, const uint32_t script_command_port = 50004); + const bool& simulated = false, const uint32_t reverse_port = 50001, + const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, + bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003, + const uint32_t script_command_port = 50004); /*! * \brief Constructs a new UrDriver object. * @@ -145,6 +148,7 @@ class UrDriver * \param headless_mode Parameter to control if the driver should be started in headless mode. * \param calibration_checksum Expected checksum of calibration. Will be matched against the * calibration reported by the robot. + * \param simulated Set the robot as simulated or real. * \param reverse_port Port that will be opened by the driver to allow direct communication between the driver * and the robot controller * \param script_sender_port The driver will offer an interface to receive the program's URScript on this port. @@ -161,13 +165,13 @@ class UrDriver */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, - const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001, - const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03, - bool non_blocking_read = false, const std::string& reverse_ip = "", const uint32_t trajectory_port = 50003, - const uint32_t script_command_port = 50004) + const std::string& calibration_checksum = "", const bool& simulated = false, + const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000, + double servoj_lookahead_time = 0.03, bool non_blocking_read = false, const std::string& reverse_ip = "", + const uint32_t trajectory_port = 50003, const uint32_t script_command_port = 50004) : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::unique_ptr{}, calibration_checksum, reverse_port, script_sender_port, servoj_gain, - servoj_lookahead_time, non_blocking_read, reverse_ip) + std::unique_ptr{}, calibration_checksum, simulated, reverse_port, script_sender_port, + servoj_gain, servoj_lookahead_time, non_blocking_read, reverse_ip) { } @@ -351,6 +355,16 @@ class UrDriver trajectory_interface_->setTrajectoryEndCallback(trajectory_done_cb); } + /*! + * \brief Getter for the primary client + * + * \returns The used primary client + */ + std::shared_ptr getPrimaryClient() + { + return primary_client_; + } + private: std::string readScriptFile(const std::string& filename); @@ -361,8 +375,7 @@ class UrDriver std::unique_ptr trajectory_interface_; std::unique_ptr script_command_interface_; std::unique_ptr script_sender_; - std::unique_ptr> primary_stream_; - std::unique_ptr> secondary_stream_; + std::shared_ptr primary_client_; double servoj_time_; uint32_t servoj_gain_; diff --git a/src/comm/tcp_socket.cpp b/src/comm/tcp_socket.cpp index 4421be3c..3fbcea72 100644 --- a/src/comm/tcp_socket.cpp +++ b/src/comm/tcp_socket.cpp @@ -58,7 +58,10 @@ void TCPSocket::setOptions(int socket_fd) bool TCPSocket::setup(std::string& host, int port) { if (state_ == SocketState::Connected) + { + URCL_LOG_DEBUG("Socket already connected on: %s:%d", host.c_str(), port); return false; + } URCL_LOG_DEBUG("Setting up connection: %s:%d", host.c_str(), port); @@ -208,6 +211,5 @@ void TCPSocket::setReceiveTimeout(const timeval& timeout) setOptions(socket_fd_); } } - } // namespace comm } // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index d1036b2d..e4eef0e2 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -53,9 +53,10 @@ static const std::string SCRIPT_COMMAND_PORT_REPLACE("{{SCRIPT_COMMAND_SERVER_PO urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, - std::unique_ptr tool_comm_setup, const uint32_t reverse_port, - const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, - bool non_blocking_read, const std::string& reverse_ip, const uint32_t trajectory_port, + std::unique_ptr tool_comm_setup, const std::string& calibration_checksum, + const bool& simulated, const uint32_t reverse_port, const uint32_t script_sender_port, + int servoj_gain, double servoj_lookahead_time, bool non_blocking_read, + const std::string& reverse_ip, const uint32_t trajectory_port, const uint32_t script_command_port) : servoj_time_(0.008) , servoj_gain_(servoj_gain) @@ -66,12 +67,9 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ URCL_LOG_DEBUG("Initializing urdriver"); URCL_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(); + URCL_LOG_DEBUG("Initializing Primary client"); + primary_client_.reset(new primary_interface::PrimaryClient(robot_ip, calibration_checksum)); + primary_client_->setSimulated(simulated); non_blocking_read_ = non_blocking_read; get_packet_timeout_ = non_blocking_read_ ? 0 : 100; @@ -179,31 +177,14 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file, const std::string& input_recipe_file, std::function handle_program_state, bool headless_mode, - 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, const std::string& reverse_ip, - const uint32_t trajectory_port, const uint32_t script_command_port) + std::unique_ptr tool_comm_setup, const uint32_t reverse_port, + const uint32_t script_sender_port, int servoj_gain, double servoj_lookahead_time, + bool non_blocking_read, const std::string& reverse_ip, const uint32_t trajectory_port, + const uint32_t script_command_port) : UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode, - std::move(tool_comm_setup), reverse_port, script_sender_port, servoj_gain, servoj_lookahead_time, - non_blocking_read, reverse_ip, trajectory_port) + std::move(tool_comm_setup), "", false, reverse_port, script_sender_port, servoj_gain, + servoj_lookahead_time, non_blocking_read, reverse_ip, trajectory_port, script_command_port) { - URCL_LOG_WARN("DEPRECATION NOTICE: Passing the calibration_checksum to the UrDriver's constructor has been " - "deprecated. Instead, use the checkCalibration(calibration_checksum) function separately. This " - "notice is for application developers using this library. If you are only using an application using " - "this library, you can ignore this message."); - if (checkCalibration(calibration_checksum)) - { - URCL_LOG_INFO("Calibration checked successfully."); - } - else - { - URCL_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."); - } } std::unique_ptr urcl::UrDriver::getDataPackage() @@ -339,27 +320,7 @@ std::string UrDriver::readScriptFile(const std::string& filename) bool 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)); - } - URCL_LOG_DEBUG("Got calibration information from robot."); - return consumer.checkSuccessful(); + return primary_client_->getCalibrationChecker()->checkCalibration(checksum); } rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() @@ -369,29 +330,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)) - { - URCL_LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str()); - return true; - } - URCL_LOG_ERROR("Could not send program to robot"); - return false; + return primary_client_->sendScript(program); } bool UrDriver::sendRobotProgram() From 837fd415a2c760065227ecca1216f29887fd74c1 Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 09:28:24 +0100 Subject: [PATCH 10/14] Updated examples to reflect changes to the primary client. --- examples/full_driver.cpp | 15 ++++++-- examples/primary_pipeline.cpp | 70 ++++++++++++++++++----------------- 2 files changed, 48 insertions(+), 37 deletions(-) diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index 3a2135ab..186fe765 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -44,6 +44,8 @@ const std::string SCRIPT_FILE = "resources/external_control.urscript"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; +const bool SIMULATED_ROBOT = true; +bool g_program_running = false; std::unique_ptr g_my_driver; std::unique_ptr g_my_dashboard; @@ -54,6 +56,7 @@ void handleRobotProgramState(bool program_running) { // Print the text in green so we see it better std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; + g_program_running = program_running; } int main(int argc, char* argv[]) @@ -109,12 +112,18 @@ int main(int argc, char* argv[]) std::unique_ptr tool_comm_setup; const bool HEADLESS = true; g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS, - std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); + std::move(tool_comm_setup), CALIBRATION_CHECKSUM, SIMULATED_ROBOT)); // Once RTDE communication is started, we have to make sure to read from the interface buffer, as - // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main + // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main // loop. + // Wait for the program to run on the robot + while (!g_program_running) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + g_my_driver->startRTDECommunication(); double increment = 0.01; @@ -157,7 +166,7 @@ int main(int argc, char* argv[]) URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); return 1; } - URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString()); + URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); } else { diff --git a/examples/primary_pipeline.cpp b/examples/primary_pipeline.cpp index e4390d8b..096f73bb 100644 --- a/examples/primary_pipeline.cpp +++ b/examples/primary_pipeline.cpp @@ -27,18 +27,18 @@ #include #include -#include #include +#include 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 DEFAULT_ROBOT_IP = "192.168.56.101"; +const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; int main(int argc, char* argv[]) { - // Set the loglevel to info get print out the DH parameters urcl::setLogLevel(urcl::LogLevel::INFO); // Parse the ip arguments if given @@ -48,40 +48,42 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } - // Parse how many seconds to run - int second_to_run = -1; - if (argc > 2) - { - second_to_run = std::stoi(argv[2]); - } - - // First of all, we need a stream that connects to the robot - comm::URStream primary_stream(robot_ip, urcl::primary_interface::UR_PRIMARY_PORT); - - // This will parse the primary packages - primary_interface::PrimaryParser parser; - - // The producer needs both, the stream and the parser to fully work - comm::URProducer prod(primary_stream, parser); - prod.setupProducer(); + // First of all, we need to create a primary client that connects to the robot + primary_interface::PrimaryClient primary_client(robot_ip, CALIBRATION_CHECKSUM); - // The shell consumer will print the package contents to the shell - std::unique_ptr> consumer; - consumer.reset(new comm::ShellConsumer()); + // Give time to get the client to connect + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - // The notifer will be called at some points during connection setup / loss. This isn't fully - // implemented atm. - comm::INotifier notifier; - - // Now that we have all components, we can create and start the pipeline to run it all. - comm::Pipeline pipeline(prod, consumer.get(), "Pipeline", notifier); - pipeline.run(); - - // 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. - do + for (int i = 0; i < 10; ++i) { - std::this_thread::sleep_for(std::chrono::seconds(second_to_run)); - } while (second_to_run < 0); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + // Create script program to send through client + std::stringstream cmd; + cmd.imbue(std::locale::classic()); // Make sure, decimal divider is actually '.' + cmd << "sec setup():" << std::endl + << " textmsg(\"Command through primary interface complete " << i++ << "\")" << std::endl + << "end"; + std::string script_code = cmd.str(); + auto program_with_newline = script_code + '\n'; + // Send script + primary_client.sendScript(program_with_newline); + + try + { + URCL_LOG_INFO("Cartesian Information:\n%s", primary_client.getCartesianInfo()->toString().c_str()); + URCL_LOG_INFO("Calibration Hash:\n%s", primary_client.getCalibrationChecker()->getData()->toHash().c_str()); + URCL_LOG_INFO("Build Date:\n%s", primary_client.getVersionMessage()->build_date_.c_str()); + std::cout << primary_client.getJointData()->toString() << std::endl; + std::stringstream os; + os << primary_client.getJointData()->q_actual_; + URCL_LOG_INFO("Joint Angles:\n%s", os.str().c_str()); + // getGlobalVariablesSetupMessage() will throw an exception if a program on the robot has not been started + URCL_LOG_INFO("Global Variables:\n%s", primary_client.getGlobalVariablesSetupMessage()->variable_names_.c_str()); + } + catch (const UrException& e) + { + URCL_LOG_WARN(e.what()); + } + } return 0; } From 1d1367670e3b0a272b0232963e59aa453cff480c Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 09:28:51 +0100 Subject: [PATCH 11/14] Added tests for the primary client and primary parser. --- tests/CMakeLists.txt | 6 + tests/test_primary_client.cpp | 296 ++++++++++++++++++++++++ tests/test_primary_parser.cpp | 418 ++++++++++++++++++++++++++-------- 3 files changed, 622 insertions(+), 98 deletions(-) create mode 100644 tests/test_primary_client.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index ea8bb011..c7e896a2 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -48,6 +48,12 @@ else() message(STATUS "Skipping integration tests.") endif() +add_executable(primary_client_tests test_primary_client.cpp) +target_compile_options(primary_client_tests PRIVATE ${CXX17_FLAG}) +target_include_directories(primary_client_tests PRIVATE ${GTEST_INCLUDE_DIRS}) +target_link_libraries(primary_client_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) +gtest_add_tests(TARGET primary_client_tests +) add_executable(primary_parser_tests test_primary_parser.cpp) target_compile_options(primary_parser_tests PRIVATE ${CXX17_FLAG}) diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp new file mode 100644 index 00000000..21334b07 --- /dev/null +++ b/tests/test_primary_client.cpp @@ -0,0 +1,296 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include + +#include +#define private public +#include + +using namespace urcl; + +std::string ROBOT_IP = "127.0.0.1"; +int PRIMARY_PORT = 30001; +int DASHBOARD_PORT = 29999; +int FAKE_PRIMARY_PORT = 60061; +int FAKE_DASHBOARD_PORT = 60059; + +class PrimaryClientTest : public ::testing::Test +{ +protected: + void SetUp() + { + in_remote_control_ = true; + } + + void TearDown() + { + dashboard_server_.reset(); + primary_server_.reset(); + client_.reset(); + } + + void run() + { + unsigned char message[] = { 0x00, 0x00, 0x00, 0x50, 0x19, 0x00, 0x00, 0x00, + 0x00, 0x56, 0x76, 0xd3, 0xa0, 0x00, 0x00, 0x00 }; // Empty GlobalVariablesSetupMessage + size_t len = sizeof(message); + size_t written; + while (running_) + { + primary_server_->write(client_fd_, message, len, written); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + } + + void stopThread() + { + URCL_LOG_DEBUG("Shutting down thread"); + running_ = false; + if (server_thread_.joinable()) + { + server_thread_.join(); + } + } + + void connectionCallback(const int filedescriptor) + { + std::lock_guard lk(connect_mutex_); + client_fd_ = filedescriptor; + connect_cv_.notify_one(); + connection_callback_ = true; + } + + void connectionCallbackDB(const int filedescriptor) + { + std::lock_guard lk(connect_mutex_db_); + client_fd_db_ = filedescriptor; + + unsigned char message[] = { + 0x43, 0x6f, 0x6e, 0x6e, 0x65, 0x63, 0x74, 0x65, 0x64, 0x3a, 0x20, 0x55, 0x6e, 0x69, 0x76, 0x65, 0x72, 0x73, 0x61, + 0x6c, 0x20, 0x52, 0x6f, 0x62, 0x6f, 0x74, 0x73, 0x20, 0x46, 0x61, 0x6b, 0x65, 0x20, 0x54, 0x65, 0x73, 0x74, 0x20, + 0x44, 0x61, 0x73, 0x68, 0x62, 0x6f, 0x61, 0x72, 0x64, 0x20, 0x53, 0x65, 0x72, 0x76, 0x65, 0x72, 0x0a + }; // "Connected: Universal Robots Fake Test Dashboard Server\n" + size_t len = sizeof(message); + size_t written; + dashboard_server_->write(client_fd_db_, message, len, written); + connect_cv_db_.notify_one(); + connection_callback_db_ = true; + } + + void messageCallback(const int filedescriptor, char* buffer) + { + std::lock_guard lk(message_mutex_); + message_ = std::string(buffer); + message_cv_.notify_one(); + message_callback_ = true; + } + + void messageCallbackDB(const int filedescriptor, char* buffer) + { + std::lock_guard lk(message_mutex_db_); + message_db_ = std::string(buffer); + if (message_db_ == "is in remote control\n") + { + unsigned char message_true[] = { 0x74, 0x72, 0x75, 0x65, 0x0a }; // "true\n" + unsigned char message_false[] = { 0x66, 0x61, 0x6c, 0x73, 0x65, 0x0a }; // "false\n" + + size_t len = in_remote_control_ ? sizeof(message_true) : sizeof(message_false); + size_t written; + dashboard_server_->write(client_fd_db_, in_remote_control_ ? message_true : message_false, len, written); + } + if (message_db_ == "PolyscopeVersion\n") + { + unsigned char message_pv[] = { + 0x55, 0x52, 0x53, 0x6f, 0x66, 0x74, 0x77, 0x61, 0x72, 0x65, 0x20, 0x35, 0x2e, 0x31, + 0x32, 0x2e, 0x32, 0x2e, 0x31, 0x31, 0x30, 0x31, 0x35, 0x33, 0x34, 0x20, 0x28, 0x4a, + 0x75, 0x6c, 0x20, 0x30, 0x36, 0x20, 0x32, 0x30, 0x32, 0x32, 0x29, 0x0a + }; // URSoftware 5.12.2.1101534 (Jul 06 2022) + size_t len = sizeof(message_pv); + size_t written; + dashboard_server_->write(client_fd_db_, message_pv, len, written); + } + + message_cv_db_.notify_one(); + message_callback_db_ = true; + } + + bool waitForConnectionCallback(int milliseconds = 100) + { + std::unique_lock lk(connect_mutex_); + if (connect_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + connection_callback_ == true) + { + connection_callback_ = false; + return true; + } + else + { + return false; + } + } + + bool waitForMessageCallback(int milliseconds = 100) + { + std::unique_lock lk(message_mutex_); + if (message_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + message_callback_ == true) + { + message_callback_ = false; + return true; + } + else + { + return false; + } + } + + bool waitForMessageCallbackDB(int milliseconds = 100) + { + std::unique_lock lk(message_mutex_db_); + if (message_cv_db_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + message_callback_db_ == true) + { + message_callback_db_ = false; + return true; + } + else + { + return false; + } + } + + std::string message_ = ""; + std::string message_db_ = ""; + int client_fd_ = -1; + int client_fd_db_ = -1; + + std::atomic running_; + std::thread server_thread_; + std::unique_ptr client_; + std::unique_ptr primary_server_; + std::unique_ptr dashboard_server_; + + std::condition_variable connect_cv_, connect_cv_db_; + std::mutex connect_mutex_, connect_mutex_db_; + + std::condition_variable message_cv_, message_cv_db_; + std::mutex message_mutex_, message_mutex_db_; + + bool connection_callback_ = false; + bool connection_callback_db_ = false; + bool message_callback_ = false; + bool message_callback_db_ = false; + + bool in_remote_control_; +}; + +TEST_F(PrimaryClientTest, check_remote_control) +{ + dashboard_server_.reset(new comm::TCPServer(FAKE_DASHBOARD_PORT)); + primary_server_.reset(new comm::TCPServer(FAKE_PRIMARY_PORT)); + primary_server_->setMessageCallback(std::bind(&PrimaryClientTest_check_remote_control_Test::messageCallback, this, + std::placeholders::_1, std::placeholders::_2)); + dashboard_server_->setMessageCallback(std::bind(&PrimaryClientTest_check_remote_control_Test::messageCallbackDB, this, + std::placeholders::_1, std::placeholders::_2)); + primary_server_->setConnectCallback( + std::bind(&PrimaryClientTest_check_remote_control_Test::connectionCallback, this, std::placeholders::_1)); + dashboard_server_->setConnectCallback( + std::bind(&PrimaryClientTest_check_remote_control_Test::connectionCallbackDB, this, std::placeholders::_1)); + dashboard_server_->start(); + primary_server_->start(); + server_thread_ = std::thread(&PrimaryClientTest_check_remote_control_Test::run, this); + + std::unique_ptr temp_client; + client_.reset(new primary_interface::PrimaryClient(ROBOT_IP, "")); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // Let connections set up + + // Disconnect from URSim servers and connect to fake servers + client_->pipeline_->stop(); + client_->stream_->disconnect(); + client_->dashboard_client_->disconnect(); + client_->dashboard_client_->port_ = FAKE_DASHBOARD_PORT; + client_->stream_->port_ = FAKE_PRIMARY_PORT; + client_->stream_->connect(); + client_->dashboard_client_->connect(); + client_->pipeline_->run(); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // let connections set up + + // When in_remote_control_ is true the primary client should be able to send script + in_remote_control_ = true; + client_->sendScript("true\n"); + EXPECT_TRUE(waitForMessageCallback(1000)); + + // Make sure thread sets in_remote_control_ to false and primary client has time to reconnect + in_remote_control_ = false; + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + // When in_remote_control_ is false the primary client NOT should be able to send script + client_->sendScript("false\n"); + EXPECT_FALSE(waitForMessageCallback(1000)); + + stopThread(); +} + +TEST_F(PrimaryClientTest, send_script) +{ + client_.reset(new primary_interface::PrimaryClient(ROBOT_IP, "")); + std::stringstream cmd; + cmd.imbue(std::locale::classic()); // Make sure, decimal divider is actually '.' + cmd << "sec setup():" << std::endl + << " textmsg(\"Command through primary interface complete \")" << std::endl + << "end"; + + std::string script_code = cmd.str(); + auto program_with_newline = script_code + '\n'; + // Should always return false in pipeline as robot will never be in remote control + EXPECT_FALSE(client_->sendScript(program_with_newline)); +} + +TEST_F(PrimaryClientTest, get_data) +{ + client_.reset(new primary_interface::PrimaryClient(ROBOT_IP, "")); + EXPECT_EQ(client_->getVersionMessage()->build_number_, 0); + vector6d_t zero_array = { 0 }; + EXPECT_EQ(client_->getCartesianInfo()->tcp_offset_coordinates_, zero_array); + EXPECT_EQ(client_->getForceModeData()->wrench_, zero_array); + // getGlobalVariablesSetupMessage() will throw an exception since a program on the robot has not been started while + // this client has been connected + EXPECT_THROW(client_->getGlobalVariablesSetupMessage()->variable_names_, UrException); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/tests/test_primary_parser.cpp b/tests/test_primary_parser.cpp index cba6d99a..4666b9db 100644 --- a/tests/test_primary_parser.cpp +++ b/tests/test_primary_parser.cpp @@ -33,116 +33,338 @@ using namespace urcl; +unsigned char raw_data[] = { + 0x00, 0x00, 0x05, 0xbf, 0x10, 0x00, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x04, 0x9c, 0x87, 0x7f, 0xf8, 0x01, + 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfb, 0x01, + 0x40, 0x15, 0x89, 0xb9, 0xa0, 0x00, 0x00, 0x00, 0x40, 0x15, 0x89, 0xb7, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0xbd, 0x57, 0x18, 0x34, 0x42, 0x3e, 0x74, 0x53, 0x41, 0xed, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xfd, 0xbf, 0xf7, 0x97, 0xb5, 0x8c, 0x52, 0x45, 0x50, 0xbf, 0xf7, 0x97, 0xbd, 0x58, 0x52, 0x45, 0x50, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x27, 0xa5, 0x09, 0x42, 0x3e, 0x65, 0xa5, 0x41, 0xf4, 0x80, + 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xc0, 0x05, 0xc9, 0x52, 0x60, 0x00, 0x00, 0x00, 0xc0, 0x05, 0xc9, 0x59, 0x60, + 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x85, 0xc5, 0x81, 0x42, 0x3e, 0x83, 0x00, + 0x41, 0xf5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0xf3, 0x14, 0x8c, 0xcc, 0x52, 0x45, 0x50, 0xbf, 0xf3, + 0x14, 0xa4, 0x6c, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x77, 0x02, 0x3b, 0x42, + 0x3e, 0x65, 0xa5, 0x42, 0x0f, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0x96, 0xed, 0xd4, 0x44, 0x2d, 0x18, + 0x00, 0xbf, 0x96, 0xe5, 0x34, 0x44, 0x2d, 0x18, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbe, 0x37, + 0x2a, 0x27, 0x42, 0x3e, 0x83, 0x00, 0x42, 0x1d, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x40, 0x08, 0x13, 0x72, + 0x40, 0x00, 0x00, 0x00, 0x40, 0x08, 0x13, 0x72, 0xc0, 0x00, 0x00, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x3c, 0xbd, 0x2b, 0x48, 0x42, 0x3e, 0x39, 0x9e, 0x42, 0x22, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00, + 0x00, 0x00, 0x65, 0x04, 0xbf, 0xa3, 0x56, 0x0a, 0xd8, 0xab, 0xe1, 0xc1, 0xbf, 0xd4, 0xdf, 0x01, 0x9c, 0x33, 0x4b, + 0xe9, 0x3f, 0xc7, 0x82, 0xfd, 0x1e, 0xfc, 0x1f, 0xe5, 0xbf, 0xf9, 0x19, 0xa6, 0xcb, 0xa7, 0xcf, 0x17, 0xbf, 0xf7, + 0x4b, 0x73, 0x19, 0x28, 0x8c, 0xfc, 0x40, 0x01, 0x6a, 0xf6, 0xae, 0x78, 0xab, 0xf9, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe1, 0x05, 0xad, 0x33, 0xdc, 0x43, 0xad, 0x52, 0x81, 0xf1, 0x2e, + 0x6a, 0x72, 0x7e, 0xad, 0xe9, 0x54, 0x2d, 0xad, 0xe8, 0xf5, 0x9e, 0xad, 0xeb, 0x88, 0x89, 0x3e, 0x58, 0xb7, 0x52, + 0x37, 0x00, 0x00, 0x00, 0x3f, 0xda, 0x3d, 0xdc, 0x79, 0x9e, 0xb9, 0xec, 0x40, 0x14, 0xed, 0x48, 0x93, 0xe2, 0x13, + 0x60, 0x3f, 0xe4, 0x86, 0xb0, 0xe7, 0xf1, 0xca, 0x9e, 0x3e, 0x84, 0x41, 0x1e, 0x96, 0x7e, 0x00, 0x00, 0x3e, 0xa0, + 0x82, 0x46, 0x96, 0x5a, 0x00, 0x00, 0x3f, 0x26, 0x18, 0x3b, 0x6b, 0xe3, 0xed, 0x33, 0xbf, 0xd8, 0xf7, 0x98, 0xba, + 0x58, 0x2a, 0x97, 0xbf, 0xd4, 0x19, 0xd6, 0x44, 0x6b, 0x34, 0x85, 0x3f, 0x1f, 0xaa, 0x89, 0x47, 0x6c, 0x84, 0x7a, + 0xbe, 0xef, 0x01, 0x0f, 0xfb, 0xb4, 0x20, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xc1, + 0x5f, 0x95, 0x34, 0x38, 0x6a, 0x40, 0x64, 0x04, 0xe3, 0x36, 0x9b, 0xf9, 0x53, 0xc0, 0x6b, 0xab, 0x59, 0xb7, 0xa6, + 0xa2, 0xec, 0x40, 0x4e, 0xaa, 0xec, 0x6f, 0x63, 0x01, 0xfd, 0x3f, 0xb9, 0x83, 0xdc, 0xec, 0xb9, 0x10, 0x9a, 0x3f, + 0xb9, 0x87, 0x6a, 0x0b, 0xb9, 0xb4, 0xfc, 0x3f, 0xf9, 0x21, 0x17, 0x86, 0x80, 0x32, 0xe0, 0x3f, 0x51, 0x58, 0xdb, + 0xb7, 0x00, 0x50, 0xb6, 0x3f, 0x6f, 0x65, 0x0e, 0x77, 0x42, 0xe7, 0x3e, 0x3f, 0xf9, 0x1a, 0xa7, 0x3b, 0xc0, 0xb4, + 0xeb, 0xbf, 0xf9, 0x2a, 0x45, 0x93, 0x44, 0x14, 0xb9, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x35, 0x09, 0x3f, 0xe2, 0x52, 0x62, 0x73, 0x85, 0xeb, 0x0c, 0x3f, 0xfa, 0x55, 0xb0, + 0x75, 0xda, 0xac, 0x08, 0x3f, 0xfc, 0x18, 0x09, 0x2b, 0xcd, 0x55, 0x80, 0x3f, 0xd4, 0x53, 0x0f, 0x7a, 0xa0, 0x7f, + 0xcc, 0x3f, 0xcd, 0xf4, 0x17, 0x0c, 0xcd, 0xec, 0xc8, 0xbf, 0xc7, 0x8e, 0x59, 0x16, 0xa2, 0xec, 0x24, 0x00, 0x00, + 0x00, 0x4b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x3f, 0xbc, 0x8d, 0x9c, 0x00, 0x00, + 0x00, 0x00, 0x3f, 0xc3, 0x77, 0x16, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, + 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, 0x00, 0x42, 0x08, 0xc0, 0x00, 0x42, 0x3e, 0xd9, 0x0d, 0x3f, 0x0c, + 0x14, 0xc3, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x65, 0xf5, 0xbb, 0x2b, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, + 0x25, 0x02, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x3c, 0x58, 0xc4, 0x40, 0x42, 0x1f, 0x80, 0x00, 0xfd, 0x00, 0x00, 0x01, 0xbd, + 0x06, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, + 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, + 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, + 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, + 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, + 0x2f, 0x63, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, + 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, + 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, + 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x3f, 0xf0, 0xc1, 0x52, 0x38, 0x2d, 0x73, 0x65, 0x3f, 0xf6, 0x57, 0x18, 0x4a, 0xe7, 0x44, 0x87, + 0x3f, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf3, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x3f, 0xd0, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33, 0x33, + 0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc, 0xcc, + 0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f, 0xb9, + 0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, + 0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, + 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x3d, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x3e, 0x80, 0x4a, 0xad, 0x81, 0xf7, 0x98, 0x78, 0x00, 0x00, 0x00, 0x09, 0x08, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, + 0x00, 0x2b, 0x0a, 0x65, 0xf5, 0xbb, 0x2b, 0x00, 0x01, 0xbf, 0xb9, 0x24, 0x07, 0xe2, 0x9d, 0x71, 0xb2, 0xbf, 0x9a, + 0x90, 0xa7, 0x16, 0xd3, 0x2a, 0x62, 0x3f, 0xe2, 0xbc, 0xae, 0xd2, 0xd2, 0xce, 0x76, 0x3f, 0xc0, 0xa3, 0xd7, 0x0a, + 0x3d, 0x70, 0xa4, 0x00, 0x00, 0x00, 0x1a, 0x0b, 0x01, 0x00, 0x0f, 0x42, 0x40, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, + 0x00, 0x01, 0x3f, 0xc0, 0x00, 0x00, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x0c, 0x00, 0x01, 0x01, 0x00, + 0x00, 0x00, 0x55, 0x0e, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x59, + 0x37, 0x08, 0xaa, 0xc9, 0x6c, 0xc6, 0x3f, 0x59, 0x37, 0x08, 0xaa, 0xc9, 0x6c, 0xc6, 0x3f, 0x59, 0x37, 0x08, 0xaa, + 0xc9, 0x6c, 0xc6, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 +}; + +/* First RobotState of UR10e from real robot 5.11 + * + * This package contains: + * - ROBOT_MODE_DATA, + * - JOINT_DATA, + * - CARTESIAN_INFO, + * - KINEMATICS_INFO, + * - CALIBRATION_DATA + * - MASTERBOARD_DATA, + * - TOOL_DATA, + * - CONFIGURATION_DATA, + * - FORCE_MODE_DATA, + * - ADDITIONAL_INFO, + * - SAFETY_DATA + * - TOOL_COMMUNICATION_INFO, + * - TOOL_MODE_INFO, + * - SINGULARITY_INFO + * - TYPE_14 + */ + +TEST(primary_parser, parse_robot_mode_data) +{ + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + primary_interface::PrimaryParser parser; + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 14); + + if (primary_interface::RobotModeData* data = dynamic_cast(products[0].get())) + { + EXPECT_EQ(data->timestamp_, 19805995000); + EXPECT_EQ(data->is_real_robot_connected_, true); + EXPECT_EQ(data->is_real_robot_enabled_, true); + EXPECT_EQ(data->is_robot_power_on_, true); + EXPECT_EQ(data->is_emergency_stopped_, false); + EXPECT_EQ(data->is_protective_stopped_, false); + EXPECT_EQ(data->is_program_running_, false); + EXPECT_EQ(data->is_program_paused_, false); + EXPECT_EQ(unsigned(data->robot_mode_), 7); // Robot mode running + EXPECT_EQ(unsigned(data->control_mode_), 0); // Control mode position + EXPECT_EQ(data->target_speed_fraction_, 1); + EXPECT_EQ(data->speed_scaling_, 0); + EXPECT_EQ(data->target_speed_fraction_limit_, 1); + EXPECT_EQ(static_cast(data->reserved_[0]), 0); + } + else + { + std::cout << "Failed to get robot mode data" << std::endl; + GTEST_FAIL(); + } +} + +TEST(primary_parser, parse_joint_data) +{ + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + primary_interface::PrimaryParser parser; + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 14); + + if (primary_interface::JointData* data = dynamic_cast(products[1].get())) + { + vector6d_t q_actual = { 5.384497166, -1.474538372, -2.723301649, -1.192517088, -0.02239162127, 3.009495258 }; + for (size_t i = 0; i < data->q_actual_.size(); ++i) + { + EXPECT_NEAR(data->q_actual_[i], q_actual[i], std::abs(q_actual[i] * 1.0e-5)); + } + vector6d_t q_target = { 5.384488583, -1.474545808, -2.723315001, -1.192539619, -0.0223587195, 3.009496212 }; + for (size_t i = 0; i < data->q_target_.size(); ++i) + { + EXPECT_NEAR(data->q_target_[i], q_target[i], std::abs(q_target[i] * 1.0e-5)); + } + EXPECT_EQ(data->qd_actual_, vector6d_t({ 0, 0, 0, 0, 0, 0 })); + vector6d_t i_actual = { -0.05251331627, 0.6548619866, 1.045089841, 0.2412194461, -0.1788717359, 0.02309192717 }; + for (size_t i = 0; i < data->i_actual_.size(); ++i) + { + EXPECT_NEAR(data->i_actual_[i], i_actual[i], std::abs(i_actual[i] * 1.0e-5)); + } + vector6d_t v_actual = { 47.61359787, 47.59926224, 47.62792969, 47.59926224, 47.62792969, 47.55626678 }; + for (size_t i = 0; i < data->v_actual_.size(); ++i) + { + EXPECT_NEAR(data->v_actual_[i], v_actual[i], std::abs(v_actual[i] * 1.0e-5)); + } + EXPECT_EQ(data->t_motor_, vector6f_t({ 29.625, 30.5625, 30.625, 35.875, 39.3125, 40.625 })); + for (size_t i = 0; i < data->joint_mode_.size(); ++i) + { + EXPECT_EQ(unsigned(data->joint_mode_[i]), 253); + } + } + else + { + std::cout << "Failed to get joint data" << std::endl; + GTEST_FAIL(); + } +} + +TEST(primary_parser, parse_cartesian_info) +{ + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + primary_interface::PrimaryParser parser; + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 14); + + if (primary_interface::CartesianInfo* data = dynamic_cast(products[2].get())) + { + vector6d_t flange_coordinates = { -0.03776582618, -0.3261112237, 0.1836849595, + -1.568762584, -1.455920313, 2.17722832 }; + for (size_t i = 0; i < data->flange_coordinates_.size(); ++i) + { + EXPECT_NEAR(data->flange_coordinates_[i], flange_coordinates[i], std::abs(flange_coordinates[i] * 1.0e-5)); + } + vector6d_t tcp_offset_coordinates = { 0, 0, 0, 0, 0, 0 }; + for (size_t i = 0; i < data->tcp_offset_coordinates_.size(); ++i) + { + EXPECT_NEAR(data->tcp_offset_coordinates_[i], tcp_offset_coordinates[i], + std::abs(tcp_offset_coordinates[i] * 1.0e-5)); + } + } + else + { + std::cout << "Failed to get cartesian info" << std::endl; + GTEST_FAIL(); + } +} + TEST(primary_parser, parse_calibration_data) { - /* First RobotState of UR5e from URSim v5.8 - * - * This package contains: - * - ROBOT_MODE_DATA - * - JOINT_DATA - * - CARTESIAN_INFO - * - KINEMATICS_INFO - * - NEEDED_FOR_CALIB_DATA - * - MASTERBOARD_DATA - * - TOOL_DATA - * - CONFIGURATION_DATA - * - FORCE_MODE_DATA - * - ADDITIONAL_INFO - * - SAFETY_DATA - * - TOOL_COMM_INFO - * - TOOL_MODE_INFO - */ - unsigned char raw_data[] = { - 0x00, 0x00, 0x05, 0x6a, 0x10, 0x00, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x05, 0xf8, 0x7d, 0x17, 0x40, 0x01, - 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x3f, 0xef, 0x0a, 0x3d, 0x70, 0xa3, 0xd7, 0x0a, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfb, 0x01, - 0xbf, 0xf9, 0x9c, 0x77, 0x9a, 0x6b, 0x50, 0xb0, 0xbf, 0xf9, 0x9c, 0x77, 0x9a, 0x6b, 0x50, 0xb0, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0xa3, 0xa8, 0x79, 0x38, 0x00, 0x00, 0x00, 0x00, 0x41, 0xcc, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xfd, 0xbf, 0xfb, 0xa2, 0x33, 0x9c, 0x0e, 0xbe, 0xe0, 0xbf, 0xfb, 0xa2, 0x33, 0x9c, 0x0e, 0xbe, 0xe0, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x09, 0x06, 0x5a, 0x00, 0x00, 0x00, 0x00, 0x41, 0xc8, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xc0, 0x01, 0x9f, 0xbe, 0x76, 0xc8, 0xb4, 0x38, 0xc0, 0x01, 0x9f, 0xbe, 0x76, - 0xc8, 0xb4, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xbf, 0x0f, 0xf7, 0x00, 0x00, 0x00, 0x00, - 0x41, 0xc4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0xe9, 0xdb, 0x22, 0xd0, 0xe5, 0x60, 0x40, 0xbf, 0xe9, - 0xdb, 0x22, 0xd0, 0xe5, 0x60, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x6e, 0xbb, 0xe2, 0x00, - 0x00, 0x00, 0x00, 0x41, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x3f, 0xf9, 0x85, 0x87, 0x93, 0xdd, 0x97, - 0xf6, 0x3f, 0xf9, 0x85, 0x87, 0x93, 0xdd, 0x97, 0xf6, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3a, 0xe6, - 0x05, 0x69, 0x00, 0x00, 0x00, 0x00, 0x41, 0xbc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0xbf, 0x9f, 0xbe, 0x76, - 0xc8, 0xb4, 0x39, 0x00, 0xbf, 0x9f, 0xbe, 0x76, 0xc8, 0xb4, 0x39, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x41, 0xbc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00, - 0x00, 0x00, 0x65, 0x04, 0xbf, 0xc2, 0x6d, 0x90, 0xa0, 0x1d, 0xe7, 0x77, 0xbf, 0xdb, 0xe1, 0x32, 0xf6, 0xa8, 0x66, - 0x44, 0x3f, 0xc9, 0xdc, 0x1e, 0xb0, 0x03, 0x31, 0xe6, 0xbf, 0x54, 0x02, 0xc0, 0xf8, 0xe6, 0xe6, 0x79, 0x40, 0x08, - 0xee, 0x22, 0x63, 0x78, 0xfa, 0xe0, 0x3f, 0xa3, 0xe9, 0xa4, 0x23, 0x7a, 0x7b, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe1, 0x05, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, - 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33, - 0x33, 0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc, - 0xcc, 0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f, - 0xb9, 0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, - 0x50, 0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x35, 0x09, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x4b, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, - 0x00, 0x3f, 0x70, 0x62, 0x4d, 0xe0, 0x00, 0x00, 0x00, 0x41, 0xc0, 0x00, 0x00, 0x42, 0x40, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x57, 0xf4, 0x28, 0x5b, 0x01, 0x01, 0x01, 0x00, 0x00, 0x00, - 0x25, 0x02, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xfd, 0x00, 0x00, 0x01, 0xbd, - 0x06, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, - 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, - 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, - 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, - 0x99, 0x28, 0x2c, 0x2f, 0x63, 0xc0, 0x19, 0x57, 0x99, 0x28, 0x2c, 0x2f, 0x63, 0x40, 0x19, 0x57, 0x99, 0x28, 0x2c, - 0x2f, 0x63, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, - 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, - 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, - 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x0a, 0xbb, 0x94, 0xed, 0xdd, 0xc6, 0xb1, 0x40, 0x44, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x3f, 0xf0, 0xc1, 0x52, 0x38, 0x2d, 0x73, 0x65, 0x3f, 0xf6, 0x57, 0x18, 0x4a, 0xe7, 0x44, 0x87, - 0x3f, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf3, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x3f, 0xd0, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xbf, 0xdb, 0x33, 0x33, 0x33, 0x33, - 0x33, 0x33, 0xbf, 0xd9, 0x19, 0xce, 0x07, 0x5f, 0x6f, 0xd2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xc4, 0xcc, 0xcc, - 0xcc, 0xcc, 0xcc, 0xcd, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x3f, 0xc1, 0x0f, 0xf9, 0x72, 0x47, 0x45, 0x39, 0x3f, 0xb9, 0x85, 0xf0, 0x6f, 0x69, 0x44, 0x67, 0x3f, 0xb9, - 0x7f, 0x62, 0xb6, 0xae, 0x7d, 0x56, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, - 0xbf, 0xf9, 0x21, 0xfb, 0x54, 0x52, 0x45, 0x50, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x01, - 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x3d, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x3f, 0x6c, 0xf5, 0xac, 0x1d, 0xb9, 0xa1, 0x08, 0x00, 0x00, 0x00, 0x09, 0x08, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, - 0x00, 0x2b, 0x0a, 0x57, 0xf4, 0x28, 0x5b, 0x01, 0x01, 0xbf, 0xb8, 0x4d, 0xc2, 0x84, 0x9f, 0xed, 0xcf, 0xbf, 0xb0, - 0x37, 0x9e, 0xd0, 0x87, 0xba, 0x97, 0x3f, 0xe2, 0xa2, 0x5b, 0x78, 0xc3, 0x9f, 0x6c, 0x3f, 0xc0, 0xa3, 0xd7, 0x0a, - 0x3d, 0x70, 0xa4, 0x00, 0x00, 0x00, 0x1a, 0x0b, 0x00, 0x00, 0x01, 0xc2, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x01, 0x3f, 0xc0, 0x00, 0x00, 0x40, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x0c, 0x00, 0x01, 0x01 - }; comm::BinParser bp(raw_data, sizeof(raw_data)); std::vector> products; primary_interface::PrimaryParser parser; parser.parse(bp, products); - EXPECT_EQ(products.size(), 13); + EXPECT_EQ(products.size(), 14); if (primary_interface::KinematicsInfo* data = dynamic_cast(products[3].get())) { EXPECT_EQ(data->checksum_, - vector6uint32_t({ 4294967295, 4294967295, 4294967295, 4294967295, 4294967295, 4294967295 })); - EXPECT_EQ(data->dh_theta_, vector6d_t({ 0, 0, 0, 0, 0, 0 })); - EXPECT_EQ(data->dh_a_, vector6d_t({ 0, -0.425, -0.3922, 0, 0, 0 })); - EXPECT_EQ(data->dh_d_, vector6d_t({ 0.1625, 0, 0, 0.1333, 0.0997, 0.0996 })); - EXPECT_EQ(data->dh_alpha_, vector6d_t({ 1.570796327, 0, 0, 1.570796327, -1.570796327, 0 })); + vector6uint32_t({ 2905857091, 2907865585, 778728062, 2917749805, 2917725598, 2917894281 })); + vector6d_t dh_theta = { 2.30187e-08, 0.410026, 5.23172, 0.641442, 1.50907e-07, 4.92003e-07 }; + for (size_t i = 0; i < data->dh_theta_.size(); ++i) + { + EXPECT_NEAR(data->dh_theta_[i], dh_theta[i], std::abs(dh_theta[i] * 1.0e-5)); + } + vector6d_t dh_a = { 0.000168569, -0.390112, -0.314077, 0.000120797, -1.47839e-05, 0 }; + for (size_t i = 0; i < data->dh_a_.size(); ++i) + { + EXPECT_NEAR(data->dh_a_[i], dh_a[i], std::abs(dh_a[i] * -1.0e-5)); + } + vector6d_t dh_d = { 0.162151, 160.153, -221.355, 61.3353, 0.0996683, 0.0997225 }; + for (size_t i = 0; i < data->dh_d_.size(); ++i) + { + EXPECT_NEAR(data->dh_d_[i], dh_d[i], std::abs(dh_d[i] * 1.0e-5)); + } + vector6d_t dh_alpha = { 1.57058, 0.00105878, 0.00383237, 1.56901, -1.57282, 0 }; + for (size_t i = 0; i < data->dh_alpha_.size(); ++i) + { + EXPECT_NEAR(data->dh_alpha_[i], dh_alpha[i], std::abs(dh_alpha[i] * 1.0e-5)); + } + } + else + { + std::cout << "Failed to get kinematics info" << std::endl; + GTEST_FAIL(); + } +} + +TEST(primary_parser, parse_force_mode_data) +{ + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + primary_interface::PrimaryParser parser; + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 14); + + if (primary_interface::ForceModeData* data = dynamic_cast(products[8].get())) + { + vector6d_t wrench = { 0, 0, 0, 0, 0, 0 }; + for (size_t i = 0; i < data->wrench_.size(); ++i) + { + EXPECT_NEAR(data->wrench_[i], wrench[i], std::abs(wrench[i] * 1.0e-5)); + } + double robot_dexterity = 1.213826985e-07; + EXPECT_NEAR(data->robot_dexterity_, robot_dexterity, std::abs(robot_dexterity * 1.0e-5)); + } + else + { + std::cout << "Failed to get force mode data" << std::endl; + GTEST_FAIL(); + } +} + +TEST(primary_parser, parse_additional_info) +{ + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector> products; + primary_interface::PrimaryParser parser; + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 14); + + if (primary_interface::AdditionalInfo* data = dynamic_cast(products[9].get())) + { + EXPECT_EQ(unsigned(data->tp_button_state_), false); + EXPECT_EQ(data->freedrive_button_enabled_, false); + EXPECT_EQ(data->io_enabled_freedrive_, false); + } + else + { + std::cout << "Failed to get additional info" << std::endl; + GTEST_FAIL(); + } +} + +TEST(primary_parser, parse_global_variables_setup_message) +{ + unsigned char raw_program_state_msg_data[] = { 0x00, 0x00, 0x00, 0x50, 0x19, 0x00, 0x00, 0x00, 0x00, 0x56, 0x76, 0xd3, + 0xa0, 0x00, 0x00, 0x00, 0x6c, 0x6f, 0x77, 0x0a, 0x66, 0x61, 0x72, 0x0a, + 0x6e, 0x5f, 0x72, 0x65, 0x70, 0x65, 0x61, 0x74, 0x73, 0x0a, 0x6d, 0x65, + 0x64, 0x69, 0x75, 0x6d, 0x0a, 0x72, 0x65, 0x73, 0x65, 0x74, 0x5f, 0x76, + 0x61, 0x0a, 0x62, 0x6c, 0x65, 0x6e, 0x64, 0x69, 0x6e, 0x67, 0x0a, 0x68, + 0x69, 0x67, 0x68, 0x0a, 0x63, 0x6c, 0x6f, 0x73, 0x65, 0x0a, 0x6f, 0x6e, + 0x5f, 0x72, 0x65, 0x74, 0x75, 0x72, 0x6e, 0x0a }; + comm::BinParser bp(raw_program_state_msg_data, sizeof(raw_program_state_msg_data)); + + std::vector> products; + primary_interface::PrimaryParser parser; + parser.parse(bp, products); + + EXPECT_EQ(products.size(), 1); + + if (primary_interface::GlobalVariablesSetupMessage* data = + dynamic_cast(products[0].get())) + { + EXPECT_EQ(data->start_index_, 0); + EXPECT_EQ(data->variable_names_, "low\nfar\nn_repeats\nmedium\nreset_va\nblending\nhigh\nclose\non_return\n"); + } + else + { + std::cout << "Failed to get Global Variables Update Message info" << std::endl; + GTEST_FAIL(); } } From 89df591753f5627edfb1951b9ce7eff8438955c6 Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 11:47:40 +0100 Subject: [PATCH 12/14] Now checks if robot is e-series or cb3. If robot is cb3 in_remote_control_ will always be true. The thread to check if robot is in remote control or not will only start if robot is e-series. --- examples/primary_pipeline.cpp | 3 +-- include/ur_client_library/primary/primary_client.h | 1 + src/primary/primary_client.cpp | 9 ++++++--- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/examples/primary_pipeline.cpp b/examples/primary_pipeline.cpp index 096f73bb..2c587c79 100644 --- a/examples/primary_pipeline.cpp +++ b/examples/primary_pipeline.cpp @@ -61,7 +61,7 @@ int main(int argc, char* argv[]) std::stringstream cmd; cmd.imbue(std::locale::classic()); // Make sure, decimal divider is actually '.' cmd << "sec setup():" << std::endl - << " textmsg(\"Command through primary interface complete " << i++ << "\")" << std::endl + << " textmsg(\"Command through primary interface complete " << i << "\")" << std::endl << "end"; std::string script_code = cmd.str(); auto program_with_newline = script_code + '\n'; @@ -73,7 +73,6 @@ int main(int argc, char* argv[]) URCL_LOG_INFO("Cartesian Information:\n%s", primary_client.getCartesianInfo()->toString().c_str()); URCL_LOG_INFO("Calibration Hash:\n%s", primary_client.getCalibrationChecker()->getData()->toHash().c_str()); URCL_LOG_INFO("Build Date:\n%s", primary_client.getVersionMessage()->build_date_.c_str()); - std::cout << primary_client.getJointData()->toString() << std::endl; std::stringstream os; os << primary_client.getJointData()->q_actual_; URCL_LOG_INFO("Joint Angles:\n%s", os.str().c_str()); diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index 7f3962df..feffc5fe 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -189,6 +189,7 @@ class PrimaryClient comm::INotifier notifier_; bool connected_; bool simulated_; + bool e_series_; std::atomic running_, in_remote_control_; std::unique_ptr consumer_; std::unique_ptr> producer_; diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index 2cc8d9c7..d86b9a0f 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -49,11 +49,9 @@ PrimaryClient::PrimaryClient(const std::string& robot_ip, const std::string& cal pipeline_.reset(new comm::Pipeline(*producer_, consumer_.get(), "primary pipeline", notifier_)); pipeline_->run(); - in_remote_control_ = false; + in_remote_control_ = true; running_ = true; - rc_thread_ = std::thread(&PrimaryClient::checkRemoteLocalControl, this); - std::chrono::duration time_done(0); std::chrono::duration timeout = std::chrono::seconds(1); std::chrono::duration retry_period = std::chrono::milliseconds(10); @@ -75,6 +73,11 @@ PrimaryClient::PrimaryClient(const std::string& robot_ip, const std::string& cal "[https://github.com/UniversalRobots/Universal_Robots_ROS_Driver#extract-calibration-information] " "for details."); } + + // Check robot version + e_series_ = getVersionMessage()->major_version_ > 3; + if (e_series_) + rc_thread_ = std::thread(&PrimaryClient::checkRemoteLocalControl, this); } PrimaryClient::~PrimaryClient() From 438eb848d5476893fea2bf5d455947cf5d240daf Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 12:46:31 +0100 Subject: [PATCH 13/14] Updated primary client test to reflect changes from adding functionality to determine if robot is e-series or cb3. --- tests/test_primary_client.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp index 21334b07..2e8c60a6 100644 --- a/tests/test_primary_client.cpp +++ b/tests/test_primary_client.cpp @@ -264,6 +264,7 @@ TEST_F(PrimaryClientTest, check_remote_control) TEST_F(PrimaryClientTest, send_script) { client_.reset(new primary_interface::PrimaryClient(ROBOT_IP, "")); + client_->setSimulated(true); std::stringstream cmd; cmd.imbue(std::locale::classic()); // Make sure, decimal divider is actually '.' cmd << "sec setup():" << std::endl @@ -272,13 +273,14 @@ TEST_F(PrimaryClientTest, send_script) std::string script_code = cmd.str(); auto program_with_newline = script_code + '\n'; - // Should always return false in pipeline as robot will never be in remote control - EXPECT_FALSE(client_->sendScript(program_with_newline)); + // Should always return true in pipeline as robot is simulated + EXPECT_TRUE(client_->sendScript(program_with_newline)); } TEST_F(PrimaryClientTest, get_data) { client_.reset(new primary_interface::PrimaryClient(ROBOT_IP, "")); + client_->setSimulated(true); EXPECT_EQ(client_->getVersionMessage()->build_number_, 0); vector6d_t zero_array = { 0 }; EXPECT_EQ(client_->getCartesianInfo()->tcp_offset_coordinates_, zero_array); From 3cd736af926200e132d7f2742d1624946f984101 Mon Sep 17 00:00:00 2001 From: urmarp Date: Fri, 6 Jan 2023 13:37:11 +0100 Subject: [PATCH 14/14] Updated test to reflect changes to primary client --- .../primary/primary_client.h | 7 ++++++ src/primary/primary_client.cpp | 23 +++++++++++++++---- tests/CMakeLists.txt | 14 ++++++----- tests/test_primary_client.cpp | 5 ++++ 4 files changed, 39 insertions(+), 10 deletions(-) diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index feffc5fe..34993bb8 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -86,6 +86,13 @@ class PrimaryClient */ bool isInRemoteControl(); + /*! + * \brief Starts the checkRemoteLocalControl thread. + * + * Starts the thread checking for remote or local control + */ + void startCheckRemoteControlThread(); + /*! * \brief Returns latest AdditionalInfo message * diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index d86b9a0f..c40426a2 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -74,10 +74,20 @@ PrimaryClient::PrimaryClient(const std::string& robot_ip, const std::string& cal "for details."); } - // Check robot version - e_series_ = getVersionMessage()->major_version_ > 3; - if (e_series_) - rc_thread_ = std::thread(&PrimaryClient::checkRemoteLocalControl, this); + try + { + // Check if robot is e-series or not + e_series_ = getVersionMessage()->major_version_ > 3; + if (e_series_) + startCheckRemoteControlThread(); + } + catch (const UrException& e) + { + URCL_LOG_WARN(e.what()); + URCL_LOG_WARN("This message is only relevant if the robot is an e-series:\nThe thread checking if the robot is in " + "remote control or not has not been started. It needs to be started manually by calling the " + "startCheckRemoteControlThread() function"); + } } PrimaryClient::~PrimaryClient() @@ -171,6 +181,11 @@ void PrimaryClient::stop() stream_->disconnect(); } +void PrimaryClient::startCheckRemoteControlThread() +{ + rc_thread_ = std::thread(&PrimaryClient::checkRemoteLocalControl, this); +} + bool PrimaryClient::isInRemoteControl() { return in_remote_control_; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index c7e896a2..c36cf460 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -44,16 +44,18 @@ if (INTEGRATION_TESTS) target_link_libraries(dashboard_client_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) gtest_add_tests(TARGET dashboard_client_tests ) + + add_executable(primary_client_tests test_primary_client.cpp) + target_compile_options(primary_client_tests PRIVATE ${CXX17_FLAG}) + target_include_directories(primary_client_tests PRIVATE ${GTEST_INCLUDE_DIRS}) + target_link_libraries(primary_client_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) + gtest_add_tests(TARGET primary_client_tests + ) else() message(STATUS "Skipping integration tests.") endif() -add_executable(primary_client_tests test_primary_client.cpp) -target_compile_options(primary_client_tests PRIVATE ${CXX17_FLAG}) -target_include_directories(primary_client_tests PRIVATE ${GTEST_INCLUDE_DIRS}) -target_link_libraries(primary_client_tests PRIVATE ur_client_library::urcl ${GTEST_LIBRARIES}) -gtest_add_tests(TARGET primary_client_tests -) + add_executable(primary_parser_tests test_primary_parser.cpp) target_compile_options(primary_parser_tests PRIVATE ${CXX17_FLAG}) diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp index 2e8c60a6..2fc19296 100644 --- a/tests/test_primary_client.cpp +++ b/tests/test_primary_client.cpp @@ -234,6 +234,11 @@ TEST_F(PrimaryClientTest, check_remote_control) client_.reset(new primary_interface::PrimaryClient(ROBOT_IP, "")); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // Let connections set up + if (!client_->e_series_) + { + client_->startCheckRemoteControlThread(); + } + // Disconnect from URSim servers and connect to fake servers client_->pipeline_->stop(); client_->stream_->disconnect();