From c221aac3a16d1a9fb08b6d243d4f27e50ccd5e06 Mon Sep 17 00:00:00 2001 From: urmahp Date: Fri, 29 Sep 2023 15:05:35 +0200 Subject: [PATCH 1/8] Added support for adding and removing consumers in multi consumer interface This gives more flexibility as all the consumers doesn't have to be added when the object is created. --- include/ur_client_library/comm/pipeline.h | 38 +++++++++++++++++++++-- 1 file changed, 36 insertions(+), 2 deletions(-) diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h index 7756aca4..0059be13 100644 --- a/include/ur_client_library/comm/pipeline.h +++ b/include/ur_client_library/comm/pipeline.h @@ -29,6 +29,8 @@ #include #include #include +#include +#include namespace urcl { @@ -89,7 +91,7 @@ template class MultiConsumer : public IConsumer { private: - std::vector*> consumers_; + std::vector>> consumers_; public: /*! @@ -97,10 +99,38 @@ class MultiConsumer : public IConsumer * * \param consumers The list of consumers that should all consume given products */ - MultiConsumer(std::vector*> consumers) : consumers_(consumers) + MultiConsumer(std::vector>> consumers) : consumers_(consumers) { } + /*! + * \brief Adds a new consumer to the list of consumers + * + * \param consumer Consumer that should be added to the list + */ + void addConsumer(std::shared_ptr> consumer) + { + std::lock_guard lk(consumer_list); + consumers_.push_back(consumer); + } + + /*! + * \brief Remove a consumer from the list of consumers + * + * \param consumer Consumer that should be removed from the list + */ + void removeConsumer(std::shared_ptr> consumer) + { + std::lock_guard lk(consumer_list); + auto it = std::find(consumers_.begin(), consumers_.end(), consumer); + if (it == consumers_.end()) + { + URCL_LOG_ERROR("Unable to remove consumer as it is not part of the consumer list"); + return; + } + consumers_.erase(it); + } + /*! * \brief Sets up all registered consumers. */ @@ -151,6 +181,7 @@ class MultiConsumer : public IConsumer */ bool consume(std::shared_ptr product) { + std::lock_guard lk(consumer_list); bool res = true; for (auto& con : consumers_) { @@ -159,6 +190,9 @@ class MultiConsumer : public IConsumer } return res; } + +private: + std::mutex consumer_list; }; /*! From 70d0739c12406445fca3fda091eec65a62831d15 Mon Sep 17 00:00:00 2001 From: urmahp Date: Fri, 29 Sep 2023 15:14:18 +0200 Subject: [PATCH 2/8] Added new classes for handling more of the packages received over the primary interface This includes: * ProgramStateMessage *ErrorCodeMessage *KeyMessage *RuntimeExceptionMessage *TextMessage *RobotModeData Added constructor to robot message class --- CMakeLists.txt | 6 + .../primary/program_state_message.h | 108 +++++++++++++++ .../ur_client_library/primary/robot_message.h | 13 ++ .../robot_message/error_code_message.h | 114 ++++++++++++++++ .../primary/robot_message/key_message.h | 99 ++++++++++++++ .../robot_message/runtime_exception_message.h | 97 +++++++++++++ .../primary/robot_message/text_message.h | 98 ++++++++++++++ .../primary/robot_state/robot_mode_data.h | 128 ++++++++++++++++++ src/primary/program_state_message.cpp | 57 ++++++++ .../robot_message/error_code_message.cpp | 80 +++++++++++ src/primary/robot_message/key_message.cpp | 73 ++++++++++ .../runtime_exception_message.cpp | 69 ++++++++++ src/primary/robot_message/text_message.cpp | 55 ++++++++ src/primary/robot_state/robot_mode_data.cpp | 110 +++++++++++++++ 14 files changed, 1107 insertions(+) create mode 100644 include/ur_client_library/primary/program_state_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/robot_mode_data.h create mode 100644 src/primary/program_state_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/robot_mode_data.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 878ffede..ed4161e2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,10 +25,16 @@ add_library(urcl SHARED src/control/trajectory_point_interface.cpp src/control/script_command_interface.cpp src/primary/primary_package.cpp + src/primary/program_state_message.cpp src/primary/robot_message.cpp src/primary/robot_state.cpp src/primary/robot_message/version_message.cpp + src/primary/robot_message/key_message.cpp + src/primary/robot_message/error_code_message.cpp + src/primary/robot_message/runtime_exception_message.cpp + src/primary/robot_message/text_message.cpp src/primary/robot_state/kinematics_info.cpp + src/primary/robot_state/robot_mode_data.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/program_state_message.h b/include/ur_client_library/primary/program_state_message.h new file mode 100644 index 00000000..8973fdbb --- /dev/null +++ b/include/ur_client_library/primary/program_state_message.h @@ -0,0 +1,108 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner + * \date 2022-02-21 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CLIENT_LIBRARY_PROGRAM_STATE_MESSAGE_H_INCLUDED +#define UR_CLIENT_LIBRARY_PROGRAM_STATE_MESSAGE_H_INCLUDED + +#include "ur_client_library/primary/primary_package.h" +#include "ur_client_library/primary/package_header.h" + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief Possible ProgramStateMessage types + */ +enum class ProgramStateMessageType : uint8_t +{ + GLOBAL_VARIABLES_SETUP = 0, + GLOBAL_VARIABLES_UPDATE = 1, + TYPE_VARIABLES_UPDATE = 2, +}; + +/*! + * \brief The ProgramStateMessage class is a parent class for the different received robot messages. + */ +class ProgramStateMessage : public PrimaryPackage +{ +public: + /*! + * \brief Creates a new ProgramStateMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + */ + ProgramStateMessage(const uint64_t timestamp) : timestamp_(timestamp) + { + } + + /*! + * \brief Creates a new ProgramStateMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param message_type The package's message type + */ + ProgramStateMessage(const uint64_t timestamp, const ProgramStateMessageType state_type) + : timestamp_(timestamp), state_type_(state_type) + { + } + virtual ~ProgramStateMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized version of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief Consume this package with a specific consumer. + * + * \param consumer Placeholder for the consumer calling this + * + * \returns true on success + */ + virtual bool consumeWith(AbstractPrimaryConsumer& consumer); + + /*! + * \brief Produces a human readable representation of the package object. + * + * \returns A string representing the object + */ + virtual std::string toString() const; + + uint64_t timestamp_; + ProgramStateMessageType state_type_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif /* UR_CLIENT_LIBRARY_ROBOT_STATE_H_INCLUDED */ diff --git a/include/ur_client_library/primary/robot_message.h b/include/ur_client_library/primary/robot_message.h index 5ee0c3a2..eeaa4f56 100644 --- a/include/ur_client_library/primary/robot_message.h +++ b/include/ur_client_library/primary/robot_message.h @@ -66,6 +66,19 @@ class RobotMessage : public PrimaryPackage RobotMessage(const uint64_t timestamp, const uint8_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; /*! 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..c7681aad --- /dev/null +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -0,0 +1,114 @@ +// 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); + + 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..3e0891d8 --- /dev/null +++ b/include/ur_client_library/primary/robot_message/key_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 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); + + 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..25b25e54 --- /dev/null +++ b/include/ur_client_library/primary/robot_message/runtime_exception_message.h @@ -0,0 +1,97 @@ +// 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); + + 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_state/robot_mode_data.h b/include/ur_client_library/primary/robot_state/robot_mode_data.h new file mode 100644 index 00000000..d6174323 --- /dev/null +++ b/include/ur_client_library/primary/robot_state/robot_mode_data.h @@ -0,0 +1,128 @@ +// -- 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 Possible robot modes + */ +enum class RobotMode : int8_t +{ + ROBOT_MODE_NO_CONTROLLER = -1, + ROBOT_MODE_DISCONNECTED = 0, + ROBOT_MODE_CONFIRM_SAFETY = 1, + ROBOT_MODE_BOOTING = 2, + ROBOT_MODE_POWER_OFF = 3, + ROBOT_MODE_POWER_ON = 4, + ROBOT_MODE_IDLE = 5, + ROBOT_MODE_BACKDRIVE = 6, + ROBOT_MODE_RUNNING = 7, + ROBOT_MODE_UPDATING_FIRMWARE = 8 +}; + +/*! + * \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); + + 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_; + int8_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..93939d58 --- /dev/null +++ b/src/primary/program_state_message.cpp @@ -0,0 +1,57 @@ +// 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); +} + +// TODO have a look at the toString method +std::string ProgramStateMessage::toString() const +{ + std::stringstream ss; + ss << "timestamp: " << timestamp_ << std::endl; + ss << "Type: " << static_cast(state_type_) << std::endl; + ss << PrimaryPackage::toString(); + return ss.str(); +} + +} // namespace primary_interface +} // namespace urcl diff --git a/src/primary/robot_message/error_code_message.cpp b/src/primary/robot_message/error_code_message.cpp new file mode 100644 index 00000000..e34ebd5e --- /dev/null +++ b/src/primary/robot_message/error_code_message.cpp @@ -0,0 +1,80 @@ +// 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/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +ErrorCodeMessage::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_; +} + +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..f3e1823b --- /dev/null +++ b/src/primary/robot_message/key_message.cpp @@ -0,0 +1,73 @@ +// 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/primary/robot_message/key_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +KeyMessage::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_; +} + +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..ac10e417 --- /dev/null +++ b/src/primary/robot_message/runtime_exception_message.cpp @@ -0,0 +1,69 @@ +// 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/primary/robot_message/runtime_exception_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +RuntimeExceptionMessage::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_; +} + +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..148fee37 --- /dev/null +++ b/src/primary/robot_message/text_message.cpp @@ -0,0 +1,55 @@ +// 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/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_state/robot_mode_data.cpp b/src/primary/robot_state/robot_mode_data.cpp new file mode 100644 index 00000000..060a71b0 --- /dev/null +++ b/src/primary/robot_state/robot_mode_data.cpp @@ -0,0 +1,110 @@ +// -- 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/primary/robot_state/robot_mode_data.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +#include + +namespace urcl +{ +namespace primary_interface +{ +RobotModeData::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_; +} + +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 fe034b378c032de341e4426a2f2225e89d4e6e1e Mon Sep 17 00:00:00 2001 From: urmahp Date: Fri, 29 Sep 2023 15:15:22 +0200 Subject: [PATCH 3/8] Updated primary parser to handle parsing the newly added packages --- .../primary/primary_parser.h | 67 +++++++++++++++---- 1 file changed, 53 insertions(+), 14 deletions(-) diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index 3dfba100..d6dccb22 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -26,8 +26,14 @@ #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/program_state_message.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" +#include "ur_client_library/primary/robot_state/robot_mode_data.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/version_message.h" +#include "ur_client_library/primary/robot_message/key_message.h" +#include "ur_client_library/primary/robot_message/text_message.h" namespace urcl { @@ -115,7 +121,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,6 +140,28 @@ 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)); @@ -149,12 +177,8 @@ 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::KINEMATICS_INFO: return new KinematicsInfo(type); default: @@ -166,16 +190,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_KEY: + return new KeyMessage(timestamp, source); case RobotMessagePackageType::ROBOT_MESSAGE_VERSION: return new VersionMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE: + return new ErrorCodeMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_RUNTIME_EXCEPTION: + return new RuntimeExceptionMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_TEXT: + return new TextMessage(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::TYPE_VARIABLES_UPDATE: + // return new TypeVariablesUpdateMessage(timestamp); default: - return new RobotMessage(timestamp, source); + return new ProgramStateMessage(timestamp, type); } } }; From eaa9d790a698494b94be0d46d52576b60e83e4e0 Mon Sep 17 00:00:00 2001 From: urmahp Date: Fri, 29 Sep 2023 15:17:47 +0200 Subject: [PATCH 4/8] Added primary consumer to consume the packages received Updated abstract primary consumer with the new packages. The primary consumer logs relevant information to the shell. It also adds support for calibration check and feedback on script execution. --- .../primary/abstract_primary_consumer.h | 14 +- .../primary/primary_consumer.h | 345 ++++++++++++++++++ 2 files changed, 358 insertions(+), 1 deletion(-) 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..0508bc6a 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -31,8 +31,14 @@ #include "ur_client_library/log.h" #include "ur_client_library/comm/pipeline.h" +#include "ur_client_library/primary/program_state_message.h" +#include "ur_client_library/primary/robot_message/key_message.h" #include "ur_client_library/primary/robot_message/version_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_state/kinematics_info.h" +#include "ur_client_library/primary/robot_state/robot_mode_data.h" namespace urcl { @@ -51,7 +57,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 +77,14 @@ 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(ProgramStateMessage& pkg) = 0; virtual bool consume(VersionMessage& pkg) = 0; virtual bool consume(KinematicsInfo& pkg) = 0; + virtual bool consume(ErrorCodeMessage& pkg) = 0; + virtual bool consume(RuntimeExceptionMessage& pkg) = 0; + virtual bool consume(KeyMessage& pkg) = 0; + virtual bool consume(RobotModeData& pkg) = 0; + virtual bool consume(TextMessage& pkg) = 0; private: /* data */ 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..12b78ccc --- /dev/null +++ b/include/ur_client_library/primary/primary_consumer.h @@ -0,0 +1,345 @@ +// 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/primary/abstract_primary_consumer.h" + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +/*! + * \brief Primary consumer implementation + * + * This class implements am AbstractPrimaryConsumer such that it can consume all incoming primary + * messages. The actual work for each package will be done in this class. + */ +class PrimaryConsumer : public AbstractPrimaryConsumer +{ +public: + PrimaryConsumer() : calibration_package_received_(false), robot_mode_data_received_(false) + { + } + virtual ~PrimaryConsumer() = default; + + /*! + * \brief Consume a RobotMessage + * + * \param pkg Robot message + * + * \returns True + */ + virtual bool consume(RobotMessage& msg) override + { + return true; + } + + /*! + * \brief Consume a RobotState + * + * \param pkg Robot state + * + * \returns True + */ + virtual bool consume(RobotState& msg) override + { + return true; + } + + /*! + * \brief Consume a ProgramStateMessage + * + * \param pkg Program state message + * + * \returns True + */ + virtual bool consume(ProgramStateMessage& msg) override + { + return true; + } + + /*! + * \brief Handle a VersionMessage + * + * \param pkg VersionMessage + * + * \returns True + */ + virtual bool consume(VersionMessage& pkg) override + { + URCL_LOG_ERROR("---VersionMessage---\n %s", pkg.toString().c_str()); + return true; + } + + /*! + * \brief Handle a KinematicsInfo + * + * \param pkg KinematicsInfo + * + * \returns True + */ + virtual bool consume(KinematicsInfo& pkg) override + { + URCL_LOG_INFO("---KinematicsInfo---\n %s", pkg.toString().c_str()); + + std::unique_lock lk(calibration_data_mutex_); + expected_hash_ = pkg.toHash(); + calibration_package_received_ = true; + return true; + } + + /*! + * \brief Handle an ErrorCodeMessage + * + * \param pkg ErrorCodeMessage + * + * \returns True + */ + virtual bool consume(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()); + } + } + + if (error_code_message_callback_ != nullptr) + { + error_code_message_callback_(pkg); + } + return true; + } + + /*! + * \brief Handle a RuntimeExceptionMessage + * + * \param pkg RuntimeExceptionMessage + * + * \returns True. + */ + virtual bool consume(RuntimeExceptionMessage& pkg) override + { + URCL_LOG_ERROR("---RuntimeExceptionMessage---\n %s", pkg.toString().c_str()); + + if (runtime_exception_message_callback_ != nullptr) + { + runtime_exception_message_callback_(pkg); + } + return true; + } + + /*! + * \brief Handle a KeyMessage + * + * \param pkg keyMessage + * + * \returns True. + */ + virtual bool consume(KeyMessage& pkg) override + { + URCL_LOG_INFO("---KeyMessage---\n %s", pkg.toString().c_str()); + + if (key_message_callback_ != nullptr) + { + key_message_callback_(pkg); + } + return true; + } + + /*! + * \brief Handle a RobotModeData package + * + * \param pkg RobotModeData + * + * \returns True + */ + virtual bool consume(RobotModeData& pkg) override + { + std::lock_guard lk(robot_mode_data_mutex_); + robot_mode_data_.reset(new RobotModeData(pkg)); + robot_mode_data_received_ = true; + robot_mode_data_cv_.notify_one(); + return true; + } + + /*! + * \brief Handle a TextMessage + * + * \param pkg TextMessage + * + * \returns True + */ + virtual bool consume(TextMessage& pkg) override + { + URCL_LOG_INFO("---TextMessage---\n %s", pkg.toString().c_str()); + return true; + } + + /*! + * \brief Checks if the kinematics hash from the robot matches the expected kinematics hash + * + * \param expected_hash The expected kinematic hash + * + * \returns True if the robot's calibration checksum matches the one given to the checker. False + * if it doesn't match + */ + bool checkCalibration(const std::string& expected_hash) + { + while (!calibration_package_received_) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + std::lock_guard lk(calibration_data_mutex_); + return expected_hash_ == expected_hash; + } + + /*! + * \brief Get the robot mode data package + * + * \param wait_for_new_package if true the function will wait until a new robot mode data package is received over the + * primary interface, if false it will return the latest package received + * + * \returns RobotModeData + */ + std::shared_ptr getRobotModeData(bool wait_for_new_package = false) + { + // Make sure we have received at least one package if we are not waiting for a package + while (!robot_mode_data_received_ && wait_for_new_package == false) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + if (wait_for_new_package) + { + // Robot mode data is published with 10 Hz, so 500 milliseconds, should be plenty of time to wait for a new + // package + std::chrono::milliseconds timeout(500); + std::unique_lock lk(robot_mode_data_mutex_); + if (robot_mode_data_cv_.wait_for(lk, timeout) == std::cv_status::no_timeout) + { + return robot_mode_data_; + } + URCL_LOG_WARN("Failed to receive new robot mode data package within the timeout, are you still connected to the " + "robot? Returning the last received package"); + } + + std::lock_guard lk(robot_mode_data_mutex_); + return robot_mode_data_; + } + + /*! + * \brief Set callback function which will be triggered whenever runtime exception messages are received + * + * \param callback_function Function handling the event information. The runtime exception message received is passed + * to the function. + */ + void setRuntimeExceptionMessageCallback(std::function callback_function) + { + runtime_exception_message_callback_ = callback_function; + } + + /*! + * \brief Set callback function which will be triggered whenever key messages are received + * + * \param callback_function Function handling the event information. The key message received is passed to the + * function. + */ + void setKeyMessageCallback(std::function callback_function) + { + key_message_callback_ = callback_function; + } + + /*! + * \brief Set callback function which will be triggered whenever error code messages are received + * + * \param callback_function Function handling the event information. The error code message received is passed to the + * function. + */ + void setErrorCodeMessageCallback(std::function callback_function) + { + error_code_message_callback_ = callback_function; + } + +private: + std::function runtime_exception_message_callback_; + std::function key_message_callback_; + std::function error_code_message_callback_; + + std::string expected_hash_; + std::mutex calibration_data_mutex_; + + std::shared_ptr robot_mode_data_; + std::mutex robot_mode_data_mutex_; + std::condition_variable robot_mode_data_cv_; + + // Variables used to ensure that a package of the type has been received + bool calibration_package_received_; + bool robot_mode_data_received_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED From 24f413e216fb61e8ab1f4edc162fa0071e114746 Mon Sep 17 00:00:00 2001 From: urmahp Date: Fri, 29 Sep 2023 15:22:43 +0200 Subject: [PATCH 5/8] Added a primary client class --- CMakeLists.txt | 1 + .../primary/primary_client.h | 161 +++++++++++ src/primary/primary_client.cpp | 264 ++++++++++++++++++ 3 files changed, 426 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 ed4161e2..4b4fe759 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,6 +35,7 @@ add_library(urcl SHARED src/primary/robot_message/text_message.cpp src/primary/robot_state/kinematics_info.cpp src/primary/robot_state/robot_mode_data.cpp + src/primary/primary_client.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..e07acbb0 --- /dev/null +++ b/include/ur_client_library/primary/primary_client.h @@ -0,0 +1,161 @@ +// 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 +#include + +namespace urcl +{ +namespace primary_interface +{ +class PrimaryClient +{ +public: + PrimaryClient() = delete; + PrimaryClient(const std::string& robot_ip); + virtual ~PrimaryClient() = default; + + /*! + * \brief Checks if the kinematics information in the used model fits the actual robot. + * + * \param checksum Hash of the used kinematics information + * + * \returns True if the robot's calibration checksum matches the one given to the checker. False + * if it doesn't match + */ + bool checkCalibration(const std::string& checksum) const; + + /*! + * \brief Sends a custom script program to the robot. + * + * This function will wait until feedback is received from the robot. This could be either error feedback or that the + * script has started running. + * + * The given code must be valid according the UR Scripting Manual. + * + * \param script_code URScript code that shall be executed by the robot. + * \param timeout Time to wait for feedback from the robot + * + * \returns true if the scripts starts running successfully, false otherwise. + */ + bool sendScript(const std::string& script_code, + const std::chrono::milliseconds timeout = std::chrono::milliseconds(500)); + + /*! + * \brief Sends a secondary custom script program to the robot. + * + * The UR robot supports executing secondary script programs alongside a running program. This function is + * for executing secondary script programs alongside a running program and it will wait for error feedback + * if any is received, but it will not wait for the script to start running. + * + * The given code must be valid according the UR Scripting Manual. + * + * \param script_code URScript code that shall be executed by the robot. + * \param timeout Time to wait for feedback from the robot + * + * \returns true if no error feedback is received within the timeout, false otherwise. + */ + bool sendSecondaryScript(const std::string& script_code, + const std::chrono::milliseconds timeout = std::chrono::milliseconds(100)); + + /*! + * \brief Reconnect to the primary interface, this is necessary if you switch from local to remote control, in order + * to be able to send script code to the robot + */ + void reconnect() const; + + /*! + * \brief Adds a primary consumer to the list of consumers + * + * \param primary_consumer Primary consumer that should be added to the list + */ + void addPrimaryConsumer(std::shared_ptr primary_consumer); + + /*! + * \brief Remove a primary consumer from the list of consumers + * + * \param primary_consumer Primary consumer that should be removed from the list + */ + void removePrimaryConsumer(std::shared_ptr primary_consumer); + +private: + // Used internally to determine the type of feedback received from the robot, when executing script code + enum class RobotFeedbackType : uint8_t + { + ErrorMessage = 0, + RuntimeException = 1, + KeyMessage = 2 + }; + + // The function is called whenever a key message is received + void keyMessageCallback(KeyMessage& msg); + + // The function is called whenever an error code message is received + void errorMessageCallback(ErrorCodeMessage& msg); + + // The function is called whenever a runtime exception message is received + void runtimeExceptionMessageCallback(RuntimeExceptionMessage& msg); + + // Wait for feedback from the robot, returns true if the program starts running, false if an error is received or if + // the function times out + bool waitForRobotFeedback(const std::chrono::milliseconds timeout); + + // Wait for error feedback from the robot, returns true if no error is received within the timeout, false if an error + // is received + bool waitForRobotErrorFeedback(const std::chrono::milliseconds timeout); + + std::string robot_ip_; + PrimaryParser parser_; + + std::shared_ptr consumer_; + std::unique_ptr> multi_consumer_; + + comm::INotifier notifier_; + std::unique_ptr> producer_; + std::unique_ptr> stream_; + std::unique_ptr> pipeline_; + + std::mutex robot_feedback_mutex_; + std::condition_variable robot_feedback_cv_; + RobotFeedbackType robot_feedback_type_; + std::unique_ptr key_message_; + std::unique_ptr error_code_message_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED \ No newline at end of file diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp new file mode 100644 index 00000000..a08e9baa --- /dev/null +++ b/src/primary/primary_client.cpp @@ -0,0 +1,264 @@ +// 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 + +namespace urcl +{ +namespace primary_interface +{ +PrimaryClient::PrimaryClient(const std::string& robot_ip) : robot_ip_(robot_ip) +{ + stream_.reset(new comm::URStream(robot_ip_, UR_PRIMARY_PORT)); + producer_.reset(new comm::URProducer(*stream_, parser_)); + producer_->setupProducer(); + + // Configure consumer + consumer_.reset(new PrimaryConsumer()); + consumer_->setKeyMessageCallback(std::bind(&PrimaryClient::keyMessageCallback, this, std::placeholders::_1)); + consumer_->setErrorCodeMessageCallback(std::bind(&PrimaryClient::errorMessageCallback, this, std::placeholders::_1)); + consumer_->setRuntimeExceptionMessageCallback( + std::bind(&PrimaryClient::runtimeExceptionMessageCallback, this, std::placeholders::_1)); + + // Configure multi consumer even though we only have one consumer as default, as this enables the user to add more + // consumers after the object has been created + std::vector>> consumers; + consumers.push_back(consumer_); + multi_consumer_.reset(new comm::MultiConsumer(consumers)); + + pipeline_.reset(new comm::Pipeline(*producer_, multi_consumer_.get(), "primary pipeline", notifier_)); + pipeline_->run(); +} + +bool PrimaryClient::checkCalibration(const std::string& checksum) const +{ + return consumer_->checkCalibration(checksum); +} + +bool PrimaryClient::sendScript(const std::string& script_code, const std::chrono::milliseconds timeout) +{ + if (stream_ == nullptr) + { + throw std::runtime_error("Sending script to robot requested while there is no primary interface established. This " + "should not happen."); + } + + std::shared_ptr robot_mode_data = consumer_->getRobotModeData(); + if (robot_mode_data->robot_mode_ != toUnderlying(RobotMode::ROBOT_MODE_RUNNING)) + { + URCL_LOG_ERROR("The robot should be running in order to send script commands to the robot"); + return false; + } + + // urscripts (snippets) must end with a newline, or otherwise the controller's runtime will + // not execute them. To avoid problems, we always just append a newline here, even if + // there may already be one. + auto program_with_newline = script_code + '\n'; + + size_t len = program_with_newline.size(); + const uint8_t* data = reinterpret_cast(program_with_newline.c_str()); + size_t written; + + if (!stream_->write(data, len, written)) + { + URCL_LOG_ERROR("Could not send program to robot"); + return false; + } + URCL_LOG_INFO("Sent program to robot:\n%s", program_with_newline.c_str()); + return waitForRobotFeedback(std::chrono::milliseconds(timeout)); +} + +bool PrimaryClient::sendSecondaryScript(const std::string& script_code, const std::chrono::milliseconds timeout) +{ + if (stream_ == nullptr) + { + throw std::runtime_error("Sending script to robot requested while there is no primary interface established. This " + "should not happen."); + } + + std::shared_ptr robot_mode_data = consumer_->getRobotModeData(); + if (robot_mode_data->robot_mode_ != toUnderlying(RobotMode::ROBOT_MODE_RUNNING)) + { + URCL_LOG_ERROR("The robot should be running in order to send script commands to the robot"); + return false; + } + + // urscripts (snippets) must end with a newline, or otherwise the controller's runtime will + // not execute them. To avoid problems, we always just append a newline here, even if + // there may already be one. + auto program_with_newline = script_code + '\n'; + + size_t len = program_with_newline.size(); + const uint8_t* data = reinterpret_cast(program_with_newline.c_str()); + size_t written; + + if (!stream_->write(data, len, written)) + { + URCL_LOG_ERROR("Could not send program to robot"); + return false; + } + URCL_LOG_INFO("Sent secondary program to robot:\n%s", program_with_newline.c_str()); + return waitForRobotErrorFeedback(std::chrono::milliseconds(timeout)); +} + +void PrimaryClient::reconnect() const +{ + pipeline_->stop(); + + if (stream_->getState() == comm::SocketState::Connected) + { + stream_->disconnect(); + } + + producer_->setupProducer(); + pipeline_->run(); +} + +void PrimaryClient::addPrimaryConsumer(std::shared_ptr primary_consumer) +{ + multi_consumer_->addConsumer(primary_consumer); +} + +void PrimaryClient::removePrimaryConsumer(std::shared_ptr primary_consumer) +{ + multi_consumer_->removeConsumer(primary_consumer); +} + +void PrimaryClient::keyMessageCallback(KeyMessage& msg) +{ + std::lock_guard lk(robot_feedback_mutex_); + robot_feedback_type_ = RobotFeedbackType::KeyMessage; + key_message_.reset(new KeyMessage(msg)); + robot_feedback_cv_.notify_one(); +} + +void PrimaryClient::errorMessageCallback(ErrorCodeMessage& msg) +{ + std::lock_guard lk(robot_feedback_mutex_); + error_code_message_.reset(new ErrorCodeMessage(msg)); + robot_feedback_type_ = RobotFeedbackType::ErrorMessage; + robot_feedback_cv_.notify_one(); +} + +void PrimaryClient::runtimeExceptionMessageCallback(RuntimeExceptionMessage& msg) +{ + std::lock_guard lk(robot_feedback_mutex_); + robot_feedback_type_ = RobotFeedbackType::RuntimeException; + robot_feedback_cv_.notify_one(); +} + +bool PrimaryClient::waitForRobotFeedback(const std::chrono::milliseconds timeout) +{ + std::chrono::duration time_left(timeout.count() / 1000.0); + std::chrono::time_point start = std::chrono::steady_clock::now(); + while ((std::chrono::steady_clock::now() - start) < timeout) + { + std::unique_lock lk(robot_feedback_mutex_); + if (robot_feedback_cv_.wait_for(lk, time_left) == std::cv_status::no_timeout) + { + switch (robot_feedback_type_) + { + case RobotFeedbackType::RuntimeException: + URCL_LOG_ERROR("Above runtime exception message was received from the robot, after executing script."); + return false; + + case RobotFeedbackType::ErrorMessage: + // Robot is not in remote control + if (error_code_message_->message_code_ == 210) + { + URCL_LOG_ERROR("The robot is in local control and it is therefore not possible to send script commands to " + "the robot.\nIf the robot is in remote control, you have connected to the robot while " + "it was in local control. In this case it is necessary to reconnect to the primary " + "interface, for that you can use the function reconnect()."); + } + else + { + URCL_LOG_ERROR("Above error code message was received from the robot, after executing script."); + } + return false; + + case RobotFeedbackType::KeyMessage: + if (key_message_->title_ == "PROGRAM_XXX_STARTED") + { + return true; + } + break; + default: + // This shouldn't happen, but in case it does we just continue the loop + break; + } + } + time_left = timeout - (std::chrono::steady_clock::now() - start); + } + URCL_LOG_ERROR("Timed out waiting for feedback from the robot, are you still connected to the robot?"); + return false; +} + +bool PrimaryClient::waitForRobotErrorFeedback(const std::chrono::milliseconds timeout) +{ + std::chrono::duration time_left(timeout.count() / 1000.0); + std::chrono::time_point start = std::chrono::steady_clock::now(); + while ((std::chrono::steady_clock::now() - start) < timeout) + { + std::unique_lock lk(robot_feedback_mutex_); + if (robot_feedback_cv_.wait_for(lk, time_left) == std::cv_status::no_timeout) + { + switch (robot_feedback_type_) + { + case RobotFeedbackType::RuntimeException: + URCL_LOG_ERROR("Above runtime exception message was received from the robot, after executing script."); + return false; + + case RobotFeedbackType::ErrorMessage: + // Robot is not in remote control + if (error_code_message_->message_code_ == 210) + { + URCL_LOG_ERROR("The robot is in local control and it is therefore not possible to send script commands to " + "the robot.\nIf the robot is in remote control, you have connected to the robot while " + "it was in local control. In this case it is necessary to reconnect to the primary " + "interface, for that you can use the function reconnect()."); + } + else + { + URCL_LOG_ERROR("Above error code message was received from the robot, after executing script."); + } + return false; + + case RobotFeedbackType::KeyMessage: + break; + default: + // This shouldn't happen, but in case it does we just continue the loop + break; + } + } + time_left = timeout - (std::chrono::steady_clock::now() - start); + } + return true; +} + +} // namespace primary_interface +} // namespace urcl \ No newline at end of file From adbca507efe9ca9759e1fb732c5f4ed9065b3640 Mon Sep 17 00:00:00 2001 From: urmahp Date: Fri, 29 Sep 2023 15:23:07 +0200 Subject: [PATCH 6/8] Updated UrDriver to use the primary client class --- include/ur_client_library/ur/ur_driver.h | 30 ++++++++++-- src/ur/ur_driver.cpp | 61 +++++++----------------- 2 files changed, 45 insertions(+), 46 deletions(-) diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index c4970e3c..30026a51 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -39,6 +39,7 @@ #include "ur_client_library/ur/version_information.h" #include "ur_client_library/ur/robot_receive_timeout.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 @@ -419,13 +420,35 @@ class UrDriver /*! * \brief Sends a custom script program to the robot. * + * This function will wait until feedback is received from the robot. This could be either error feedback or that the + * script has started running. + * + * The given code must be valid according the UR Scripting Manual. + * + * \param script_code URScript code that shall be executed by the robot. + * \param timeout Time to wait for feedback from the robot + * + * \returns true if the scripts starts running successfully, false otherwise. + */ + bool sendScript(const std::string& program, + const std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) const; + + /*! + * \brief Sends a secondary custom script program to the robot. + * + * The UR robot supports executing secondary script programs alongside a running program. This function is + * for executing secondary script programs alongside a running program and it will wait for error feedback + * if any is received, but it will not wait for the script to start running. + * * The given code must be valid according the UR Scripting Manual. * - * \param program URScript code that shall be executed by the robot. + * \param script_code URScript code that shall be executed by the robot. + * \param timeout Time to wait for feedback from the robot * - * \returns true on successful upload, false otherwise. + * \returns true if no error feedback is received within the timeout, false otherwise. */ - bool sendScript(const std::string& program); + bool sendSecondaryScript(const std::string& script_code, + const std::chrono::milliseconds timeout = std::chrono::milliseconds(100)) const; /*! * \brief Sends the external control program to the robot. @@ -499,6 +522,7 @@ class UrDriver std::unique_ptr script_sender_; std::unique_ptr> primary_stream_; std::unique_ptr> secondary_stream_; + std::unique_ptr primary_client_; uint32_t servoj_gain_; double servoj_lookahead_time_; diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 9292681b..99223b9f 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -37,8 +37,6 @@ #include #include -#include - namespace urcl { static const std::string BEGIN_REPLACE("{{BEGIN_REPLACE}}"); @@ -69,11 +67,7 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ 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(); + primary_client_.reset(new primary_interface::PrimaryClient(robot_ip_)); non_blocking_read_ = non_blocking_read; get_packet_timeout_ = non_blocking_read_ ? 0 : 100; @@ -323,7 +317,7 @@ bool UrDriver::zeroFTSensor() "only work, if the robot is in remote_control mode."); std::stringstream cmd; cmd << "sec tareSetup():" << std::endl << " zero_ftsensor()" << std::endl << "end"; - return sendScript(cmd.str()); + return sendSecondaryScript(cmd.str()); } } } @@ -343,7 +337,7 @@ bool UrDriver::setPayload(const float mass, const vector3d_t& cog) cmd << "sec setup():" << std::endl << " set_payload(" << mass << ", [" << cog[0] << ", " << cog[1] << ", " << cog[2] << "])" << std::endl << "end"; - return sendScript(cmd.str()); + return sendSecondaryScript(cmd.str()); } } @@ -375,7 +369,7 @@ bool UrDriver::setToolVoltage(const ToolVoltage voltage) "robots this will only work, if the robot is in remote_control mode."); std::stringstream cmd; cmd << "sec setup():" << std::endl << " set_tool_voltage(" << toUnderlying(voltage) << ")" << std::endl << "end"; - return sendScript(cmd.str()); + return sendSecondaryScript(cmd.str()); } } @@ -510,27 +504,11 @@ std::string UrDriver::readScriptFile(const std::string& filename) bool UrDriver::checkCalibration(const std::string& checksum) { - if (primary_stream_ == nullptr) + if (primary_client_ == 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_->checkCalibration(checksum); } rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() @@ -538,31 +516,28 @@ rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() return rtde_client_->getWriter(); } -bool UrDriver::sendScript(const std::string& program) +bool UrDriver::sendScript(const std::string& program, const std::chrono::milliseconds timeout) const { - if (secondary_stream_ == nullptr) + if (primary_client_ == 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; + return primary_client_->sendScript(program, timeout); +} - if (secondary_stream_->write(data, len, written)) +bool UrDriver::sendSecondaryScript(const std::string& program, const std::chrono::milliseconds timeout) const +{ + if (primary_client_ == nullptr) { - URCL_LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str()); - return true; + throw std::runtime_error("Sending script to robot requested while there is no primary interface established. " + "This " + "should not happen."); } - URCL_LOG_ERROR("Could not send program to robot"); - return false; + + return primary_client_->sendSecondaryScript(program, timeout); } bool UrDriver::sendRobotProgram() From d9645876d20117eac066e625dfa9aa1dba45bdf7 Mon Sep 17 00:00:00 2001 From: urmahp Date: Fri, 29 Sep 2023 15:23:23 +0200 Subject: [PATCH 7/8] Updated primary pipeline examples --- examples/primary_pipeline.cpp | 77 +++++++++++++----- examples/primary_pipeline_calibration.cpp | 96 ++++++++++++++--------- 2 files changed, 117 insertions(+), 56 deletions(-) diff --git a/examples/primary_pipeline.cpp b/examples/primary_pipeline.cpp index e4390d8b..ca53c31e 100644 --- a/examples/primary_pipeline.cpp +++ b/examples/primary_pipeline.cpp @@ -25,10 +25,8 @@ */ //---------------------------------------------------------------------- -#include -#include -#include -#include +#include +#include using namespace urcl; @@ -55,30 +53,67 @@ int main(int argc, char* argv[]) 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); + // The robot should be running in order to send script code to it + // Connect the the robot Dashboard + std::unique_ptr dashboard_client; + dashboard_client.reset(new DashboardClient(robot_ip)); + if (!dashboard_client->connect()) + { + URCL_LOG_ERROR("Could not connect to dashboard"); + return 1; + } + + // Stop program, if there is one running + if (!dashboard_client->commandStop()) + { + URCL_LOG_ERROR("Could not send stop program command"); + return 1; + } + + // Release the brakes + if (!dashboard_client->commandBrakeRelease()) + { + URCL_LOG_ERROR("Could not send BrakeRelease command"); + return 1; + } - // This will parse the primary packages - primary_interface::PrimaryParser parser; + primary_interface::PrimaryClient primary_client(robot_ip); - // The producer needs both, the stream and the parser to fully work - comm::URProducer prod(primary_stream, parser); - prod.setupProducer(); + // Check that the calibration checksum matches the one provided from the robot + const std::string calibration_check_sum = ""; + bool check_calibration_result = primary_client.checkCalibration(calibration_check_sum); + std::string calibration_check_sum_matches = check_calibration_result ? "true" : "false"; + URCL_LOG_INFO("calibration check sum matches: %s", calibration_check_sum_matches.c_str()); - // The shell consumer will print the package contents to the shell - std::unique_ptr> consumer; - consumer.reset(new comm::ShellConsumer()); + // Send a script program to the robot + std::stringstream cmd; + cmd.imbue(std::locale::classic()); // Make sure, decimal divider is actually '.' + cmd << "def test():" << std::endl << "textmsg(\"Hello from script program\")" << std::endl << "end"; - // The notifer will be called at some points during connection setup / loss. This isn't fully - // implemented atm. - comm::INotifier notifier; + if (primary_client.sendScript(cmd.str())) + { + URCL_LOG_INFO("Script program was successfully sent to the robot"); + } + else + { + URCL_LOG_ERROR("Script program wasn't send successfully to the robot"); + return 1; + } - // 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(); + // Send a secondary script program to the robot + cmd.str(""); + cmd << "sec setup():" << std::endl << "textmsg(\"Hello from secondary program\")" << std::endl << "end"; + if (primary_client.sendSecondaryScript(cmd.str())) + { + URCL_LOG_INFO("Secondary script program was successfully sent to the robot"); + } + else + { + URCL_LOG_ERROR("Secondary script program wasn't send successfully to the robot"); + return 1; + } // 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 { std::this_thread::sleep_for(std::chrono::seconds(second_to_run)); diff --git a/examples/primary_pipeline_calibration.cpp b/examples/primary_pipeline_calibration.cpp index 76a3094d..035e5a94 100644 --- a/examples/primary_pipeline_calibration.cpp +++ b/examples/primary_pipeline_calibration.cpp @@ -16,14 +16,12 @@ // limitations under the License. // -- END LICENSE BLOCK ------------------------------------------------ -#include -#include -#include -#include +#include using namespace urcl; -class CalibrationConsumer : public urcl::comm::IConsumer +// Create a primary consumer for logging calibration data +class CalibrationConsumer : public primary_interface::AbstractPrimaryConsumer { public: CalibrationConsumer() : calibrated_(0), have_received_data(false) @@ -31,15 +29,49 @@ class CalibrationConsumer : public urcl::comm::IConsumer product) + // We should consume all primary packages supported, in this example we just ignore the messages + virtual bool consume(primary_interface::RobotMessage& pkg) override + { + return true; + } + virtual bool consume(primary_interface::RobotState& pkg) override + { + return true; + } + virtual bool consume(primary_interface::ProgramStateMessage& pkg) override + { + return true; + } + virtual bool consume(primary_interface::VersionMessage& pkg) override + { + return true; + } + virtual bool consume(primary_interface::ErrorCodeMessage& pkg) override + { + return true; + } + virtual bool consume(primary_interface::RuntimeExceptionMessage& pkg) override + { + return true; + } + virtual bool consume(primary_interface::KeyMessage& pkg) override + { + return true; + } + virtual bool consume(primary_interface::RobotModeData& pkg) override + { + return true; + } + virtual bool consume(primary_interface::TextMessage& pkg) override { - auto kin_info = std::dynamic_pointer_cast(product); - if (kin_info != nullptr) - { - URCL_LOG_INFO("%s", product->toString().c_str()); - calibrated_ = kin_info->calibration_status_; - have_received_data = true; - } + return true; + } + + // The kinematics info stores the calibration data + virtual bool consume(primary_interface::KinematicsInfo& pkg) override + { + calibrated_ = pkg.calibration_status_; + have_received_data = true; return true; } @@ -63,9 +95,11 @@ class CalibrationConsumer : public urcl::comm::IConsumer primary_stream(robot_ip, urcl::primary_interface::UR_PRIMARY_PORT); + // Create a primary client + primary_interface::PrimaryClient primary_client(robot_ip); - // This will parse the primary packages - primary_interface::PrimaryParser parser; + std::shared_ptr calibration_consumer; + calibration_consumer.reset(new CalibrationConsumer()); - // The producer needs both, the stream and the parser to fully work - comm::URProducer prod(primary_stream, parser); - prod.setupProducer(); + // Add the calibration consumer to the primary consumers + primary_client.addPrimaryConsumer(calibration_consumer); - // The calibration consumer will print the package contents to the shell - CalibrationConsumer calib_consumer; + // Kinematics info is only send when you connect to the primary interface, so triggering a reconnect + primary_client.reconnect(); - // 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 calib_pipeline(prod, &calib_consumer, "Pipeline", notifier); - calib_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. - while (!calib_consumer.calibrationStatusReceived()) + while (!calibration_consumer->calibrationStatusReceived()) { std::this_thread::sleep_for(std::chrono::seconds(1)); } - if (calib_consumer.isCalibrated()) + if (calibration_consumer->isCalibrated()) { printf("The robot on IP: %s is calibrated\n", robot_ip.c_str()); } @@ -113,5 +136,8 @@ int main(int argc, char* argv[]) printf("Remeber to turn on the robot to get calibration stored on the robot!\n"); } + // We can remove the consumer again once we are done using it + primary_client.removePrimaryConsumer(calibration_consumer); + return 0; } From 1780e1dfff3cc1cb09040e547fa73051f2058d28 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 28 May 2024 12:11:11 +0200 Subject: [PATCH 8/8] Fixed docstrings to match code --- include/ur_client_library/primary/primary_consumer.h | 6 +++--- include/ur_client_library/primary/program_state_message.h | 2 +- include/ur_client_library/ur/ur_driver.h | 2 +- src/ur/ur_driver.cpp | 8 ++++---- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h index 12b78ccc..9b3ed82b 100644 --- a/include/ur_client_library/primary/primary_consumer.h +++ b/include/ur_client_library/primary/primary_consumer.h @@ -55,7 +55,7 @@ class PrimaryConsumer : public AbstractPrimaryConsumer /*! * \brief Consume a RobotMessage * - * \param pkg Robot message + * \param msg Robot message * * \returns True */ @@ -67,7 +67,7 @@ class PrimaryConsumer : public AbstractPrimaryConsumer /*! * \brief Consume a RobotState * - * \param pkg Robot state + * \param msg Robot state * * \returns True */ @@ -79,7 +79,7 @@ class PrimaryConsumer : public AbstractPrimaryConsumer /*! * \brief Consume a ProgramStateMessage * - * \param pkg Program state message + * \param msg Program state message * * \returns True */ diff --git a/include/ur_client_library/primary/program_state_message.h b/include/ur_client_library/primary/program_state_message.h index 8973fdbb..8c205d7a 100644 --- a/include/ur_client_library/primary/program_state_message.h +++ b/include/ur_client_library/primary/program_state_message.h @@ -64,7 +64,7 @@ class ProgramStateMessage : public PrimaryPackage * \brief Creates a new ProgramStateMessage object to be filled from a package. * * \param timestamp Timestamp of the package - * \param message_type The package's message type + * \param state_type The package's state type */ ProgramStateMessage(const uint64_t timestamp, const ProgramStateMessageType state_type) : timestamp_(timestamp), state_type_(state_type) diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 30026a51..c8c7a90e 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -430,7 +430,7 @@ class UrDriver * * \returns true if the scripts starts running successfully, false otherwise. */ - bool sendScript(const std::string& program, + bool sendScript(const std::string& script_code, const std::chrono::milliseconds timeout = std::chrono::milliseconds(1000)) const; /*! diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 99223b9f..4b1aafc3 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -516,7 +516,7 @@ rtde_interface::RTDEWriter& UrDriver::getRTDEWriter() return rtde_client_->getWriter(); } -bool UrDriver::sendScript(const std::string& program, const std::chrono::milliseconds timeout) const +bool UrDriver::sendScript(const std::string& script_code, const std::chrono::milliseconds timeout) const { if (primary_client_ == nullptr) { @@ -525,10 +525,10 @@ bool UrDriver::sendScript(const std::string& program, const std::chrono::millise "should not happen."); } - return primary_client_->sendScript(program, timeout); + return primary_client_->sendScript(script_code, timeout); } -bool UrDriver::sendSecondaryScript(const std::string& program, const std::chrono::milliseconds timeout) const +bool UrDriver::sendSecondaryScript(const std::string& script_code, const std::chrono::milliseconds timeout) const { if (primary_client_ == nullptr) { @@ -537,7 +537,7 @@ bool UrDriver::sendSecondaryScript(const std::string& program, const std::chrono "should not happen."); } - return primary_client_->sendSecondaryScript(program, timeout); + return primary_client_->sendSecondaryScript(script_code, timeout); } bool UrDriver::sendRobotProgram()