diff --git a/CMakeLists.txt b/CMakeLists.txt index 306df182d..d1d5338cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,9 +16,11 @@ add_library(urcl SHARED src/control/trajectory_point_interface.cpp src/control/script_command_interface.cpp src/primary/primary_package.cpp + src/primary/primary_client.cpp src/primary/robot_message.cpp src/primary/robot_state.cpp src/primary/robot_message/version_message.cpp + src/primary/robot_message/error_code_message.cpp src/primary/robot_state/kinematics_info.cpp src/rtde/control_package_pause.cpp src/rtde/control_package_setup_inputs.cpp diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h index afd9100bf..f18f17410 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 { @@ -83,7 +85,7 @@ class IConsumer /*! * \brief Consumer, that allows one product to be consumed by multiple arbitrary - * conusmers. + * consumers. * * @tparam T Type of the consumed products */ @@ -91,7 +93,7 @@ template class MultiConsumer : public IConsumer { private: - std::vector*> consumers_; + std::vector>> consumers_; public: /*! @@ -99,10 +101,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. */ @@ -153,6 +183,7 @@ class MultiConsumer : public IConsumer */ bool consume(std::shared_ptr product) { + std::lock_guard lk(consumer_list_); bool res = true; for (auto& con : consumers_) { @@ -161,6 +192,9 @@ class MultiConsumer : public IConsumer } return res; } + +private: + std::mutex consumer_list_; }; /*! @@ -234,7 +268,7 @@ class INotifier }; /*! - * \brief The Pipepline manages the production and optionally consumption of packages. Cyclically + * \brief The Pipeline manages the production and optionally consumption of packages. Cyclically * the producer is called and returned packages are saved in a queue. This queue is then either also * cyclically utilized by the registered consumer or can be externally used. * diff --git a/include/ur_client_library/primary/abstract_primary_consumer.h b/include/ur_client_library/primary/abstract_primary_consumer.h index b2c13c6d6..3ed464e4e 100644 --- a/include/ur_client_library/primary/abstract_primary_consumer.h +++ b/include/ur_client_library/primary/abstract_primary_consumer.h @@ -32,6 +32,7 @@ #include "ur_client_library/log.h" #include "ur_client_library/comm/pipeline.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_state/kinematics_info.h" namespace urcl @@ -51,7 +52,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). * @@ -73,6 +74,7 @@ class AbstractPrimaryConsumer : public comm::IConsumer virtual bool consume(RobotState& pkg) = 0; virtual bool consume(VersionMessage& pkg) = 0; virtual bool consume(KinematicsInfo& pkg) = 0; + virtual bool consume(ErrorCodeMessage& pkg) = 0; private: /* data */ diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h new file mode 100644 index 000000000..d7fd7d58c --- /dev/null +++ b/include/ur_client_library/primary/primary_client.h @@ -0,0 +1,98 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright © 2024-2025 Ocado Group +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +class PrimaryClient +{ +public: + PrimaryClient() = delete; + PrimaryClient(const std::string& robot_ip, comm::INotifier& notifier); + ~PrimaryClient(); + + /*! + * \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); + void start(); + + /*! + * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be + * deleted. + */ + std::deque getErrorCodes(); + +private: + // The function is called whenever an error code message is received + void errorMessageCallback(ErrorCode& code); + + PrimaryParser parser_; + std::shared_ptr consumer_; + std::unique_ptr> multi_consumer_; + + comm::INotifier notifier_; + + comm::URStream stream_; + std::unique_ptr> prod_; + std::unique_ptr> pipeline_; + + std::mutex error_code_queue_mutex_; + std::deque error_code_queue_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h new file mode 100644 index 000000000..cdb84057a --- /dev/null +++ b/include/ur_client_library/primary/primary_consumer.h @@ -0,0 +1,172 @@ +// 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 an 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() + { + } + virtual ~PrimaryConsumer() = default; + + /*! + * \brief Consume a RobotMessage + * + * \param msg Robot message + * + * \returns True + */ + virtual bool consume(RobotMessage& msg) override + { + return true; + } + + /*! + * \brief Consume a RobotState + * + * \param msg Robot state + * + * \returns True + */ + virtual bool consume(RobotState& msg) override + { + return true; + } + + /*! + * \brief Handle a VersionMessage + * + * \param pkg VersionMessage + * + * \returns True + */ + virtual bool consume(VersionMessage& pkg) override + { + return true; + } + + /*! + * \brief Handle a KinematicsInfo + * + * \param pkg KinematicsInfo + * + * \returns True + */ + virtual bool consume(KinematicsInfo& pkg) override + { + return true; + } + + /*! + * \brief Handle an ErrorCodeMessage + * + * \param pkg ErrorCodeMessage + * + * \returns True + */ + virtual bool consume(ErrorCodeMessage& pkg) override + { + urcl::primary_interface::ErrorCode code; + code.message_code = pkg.message_code_; + code.message_argument = pkg.message_argument_; + code.report_level = pkg.report_level_; + code.data_type = pkg.data_type_; + code.data = pkg.data_; + code.text = pkg.text_; + code.timestamp = pkg.timestamp_; + code.to_string = pkg.toString(); + + const auto log_contents = "Logging an ErrorCodeMessage from the UR Controller Box: " + pkg.toString(); + + switch (code.report_level) + { + case urcl::primary_interface::ReportLevel::DEBUG: + case urcl::primary_interface::ReportLevel::DEVL_DEBUG: + case urcl::primary_interface::ReportLevel::DEVL_INFO: + case urcl::primary_interface::ReportLevel::DEVL_WARNING: + case urcl::primary_interface::ReportLevel::DEVL_VIOLATION: + case urcl::primary_interface::ReportLevel::DEVL_FAULT: + URCL_LOG_DEBUG(log_contents.c_str()); + break; + case urcl::primary_interface::ReportLevel::INFO: + URCL_LOG_INFO(log_contents.c_str()); + break; + case urcl::primary_interface::ReportLevel::WARNING: + URCL_LOG_WARN(log_contents.c_str()); + break; + default: + // urcl::primary_interface::ReportLevel::VIOLATION: + // urcl::primary_interface::ReportLevel::FAULT: + URCL_LOG_ERROR(log_contents.c_str()); + break; + } + + if (error_code_message_callback_ != nullptr) + { + error_code_message_callback_(code); + } + return true; + } + + /*! + * \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 error_code_message_callback_; +}; + +} // namespace primary_interface +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index 3dfba100f..74d423a85 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -28,6 +28,7 @@ #include "ur_client_library/primary/robot_message.h" #include "ur_client_library/primary/robot_state/kinematics_info.h" #include "ur_client_library/primary/robot_message/version_message.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" namespace urcl { @@ -174,6 +175,8 @@ class PrimaryParser : public comm::Parser return new MBD;*/ case RobotMessagePackageType::ROBOT_MESSAGE_VERSION: return new VersionMessage(timestamp, source); + case RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE: + return new ErrorCodeMessage(timestamp, source); default: return new RobotMessage(timestamp, source); } diff --git a/include/ur_client_library/primary/robot_message.h b/include/ur_client_library/primary/robot_message.h index 5ee0c3a2c..120a37e26 100644 --- a/include/ur_client_library/primary/robot_message.h +++ b/include/ur_client_library/primary/robot_message.h @@ -66,6 +66,17 @@ 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 000000000..a4fc63bb3 --- /dev/null +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -0,0 +1,118 @@ +// 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, 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 +}; + +struct ErrorCode +{ + int32_t message_code{ -1 }; + int32_t message_argument{ -1 }; + ReportLevel report_level{ ReportLevel::DEBUG }; + uint32_t data_type{ 0 }; + uint32_t data{ 0 }; + std::string text; + uint64_t timestamp{ 0 }; + std::string to_string; +}; + +/*! + * \brief The ErrorCodeMessage class handles the error code messages sent via the primary UR interface. + */ +class ErrorCodeMessage : public RobotMessage +{ +public: + ErrorCodeMessage() = delete; + /*! + * \brief Creates a new ErrorCodeMessage object to be filled from a package. + * + * \param timestamp Timestamp of the package + * \param source The package's source + */ + ErrorCodeMessage(uint64_t timestamp, int8_t source) + : RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE) + { + } + virtual ~ErrorCodeMessage() = default; + + /*! + * \brief Sets the attributes of the package by parsing a serialized representation of the + * package. + * + * \param bp A parser containing a serialized text of the package + * + * \returns True, if the package was parsed successfully, false otherwise + */ + virtual bool parseWith(comm::BinParser& bp); + + /*! + * \brief 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_; + uint32_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/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 6949c2d36..6a34d38b9 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -38,6 +38,7 @@ #include "ur_client_library/ur/tool_communication.h" #include "ur_client_library/ur/version_information.h" #include "ur_client_library/ur/robot_receive_timeout.h" +#include "ur_client_library/primary/primary_client.h" #include "ur_client_library/primary/robot_message/version_message.h" #include "ur_client_library/rtde/rtde_writer.h" @@ -534,6 +535,15 @@ class UrDriver */ bool checkCalibration(const std::string& checksum); + /*! + * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be + * deleted. + * + * \returns list of error codes + * + */ + std::deque getErrorCodes(); + /*! * \brief Getter for the RTDE writer used to write to the robot's RTDE interface. * @@ -647,6 +657,11 @@ class UrDriver void resetRTDEClient(const std::string& output_recipe_filename, const std::string& input_recipe_filename, double target_frequency = 0.0, bool ignore_unavailable_outputs = false); + /*! + * \brief Starts the primary client + */ + void startPrimaryClientCommunication(); + void registerTrajectoryInterfaceDisconnectedCallback(std::function fun) { trajectory_interface_->registerDisconnectionCallback(fun); @@ -668,6 +683,7 @@ class UrDriver comm::INotifier notifier_; std::unique_ptr rtde_client_; + std::unique_ptr primary_client_; std::unique_ptr reverse_interface_; std::unique_ptr trajectory_interface_; std::unique_ptr script_command_interface_; diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp new file mode 100644 index 000000000..38ad47277 --- /dev/null +++ b/src/primary/primary_client.cpp @@ -0,0 +1,95 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright © 2024-2025 Ocado Group +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +PrimaryClient::PrimaryClient(const std::string& robot_ip, comm::INotifier& notifier) + : stream_(robot_ip, UR_PRIMARY_PORT) +{ + prod_.reset(new comm::URProducer(stream_, parser_)); + + consumer_.reset(new PrimaryConsumer()); + consumer_->setErrorCodeMessageCallback(std::bind(&PrimaryClient::errorMessageCallback, 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(*prod_, multi_consumer_.get(), "PrimaryClient Pipeline", notifier_)); +} + +PrimaryClient::~PrimaryClient() +{ + URCL_LOG_INFO("Stopping primary client pipeline"); + pipeline_->stop(); +} + +void PrimaryClient::start() +{ + URCL_LOG_INFO("Starting primary client pipeline"); + pipeline_->init(); + 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::errorMessageCallback(ErrorCode& code) +{ + std::lock_guard lock_guard(error_code_queue_mutex_); + error_code_queue_.push_back(code); +} + +std::deque PrimaryClient::getErrorCodes() +{ + std::lock_guard lock_guard(error_code_queue_mutex_); + std::deque error_codes; + error_codes = error_code_queue_; + error_code_queue_.clear(); + return error_codes; +} +} // 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 000000000..a52a482fd --- /dev/null +++ b/src/primary/robot_message/error_code_message.cpp @@ -0,0 +1,61 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2020 FZI Forschungszentrum Informatik +// Created on behalf of Universal Robots A/S +// +// 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 +{ +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 << "C" << message_code_ << "A" << message_argument_; + return ss.str(); +} + +} // namespace primary_interface +} // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 88a485fdb..6e62c3150 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -74,6 +74,8 @@ urcl::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_ new comm::URStream(robot_ip_, urcl::primary_interface::UR_SECONDARY_PORT)); secondary_stream_->connect(); + primary_client_.reset(new urcl::primary_interface::PrimaryClient(robot_ip_, notifier_)); + non_blocking_read_ = non_blocking_read; get_packet_timeout_ = non_blocking_read_ ? 0 : 100; @@ -727,4 +729,13 @@ void UrDriver::setupReverseInterface(const uint32_t reverse_port) reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state_, step_time)); } +void UrDriver::startPrimaryClientCommunication() +{ + primary_client_->start(); +} + +std::deque UrDriver::getErrorCodes() +{ + return primary_client_->getErrorCodes(); +} } // namespace urcl diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 3a27051dc..890092809 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -78,6 +78,22 @@ if (INTEGRATION_TESTS) EXTRA_ARGS --headless true TEST_SUFFIX _headless ) + + # PrimaryClient tests + add_executable(primary_client_test_urcap test_primary_client.cpp) + target_link_libraries(primary_client_test_urcap PRIVATE ur_client_library::urcl GTest::gtest_main) + gtest_add_tests(TARGET primary_client_test_urcap + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + EXTRA_ARGS --headless false + TEST_SUFFIX _urcap + ) + add_executable(primary_client_test_headless test_primary_client.cpp) + target_link_libraries(primary_client_test_headless PRIVATE ur_client_library::urcl GTest::gtest_main) + gtest_add_tests(TARGET primary_client_test_headless + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} + EXTRA_ARGS --headless true + TEST_SUFFIX _headless + ) else() message(STATUS "Skipping integration tests.") endif() diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp new file mode 100644 index 000000000..45d6fcce0 --- /dev/null +++ b/tests/test_primary_client.cpp @@ -0,0 +1,82 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include + +#include +#include +#include + +using namespace urcl; + +std::string g_ROBOT_IP = "192.168.56.101"; + +class PrimaryClientTest : public ::testing::Test +{ +protected: + void SetUp() override + { + client_ = std::make_unique(g_ROBOT_IP, notifier_); + } + + std::unique_ptr client_; + comm::INotifier notifier_; +}; + +TEST_F(PrimaryClientTest, start_communication_succeeds) +{ + EXPECT_NO_THROW(client_->start()); +} + +TEST_F(PrimaryClientTest, add_and_remove_consumer) +{ + auto calibration_consumer = std::make_shared("test"); + + client_->addPrimaryConsumer(calibration_consumer); + + EXPECT_NO_THROW(client_->start()); + + auto start_time = std::chrono::system_clock::now(); + const auto timeout = std::chrono::seconds(5); + while (!calibration_consumer->isChecked() && std::chrono::system_clock::now() - start_time < timeout) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_TRUE(calibration_consumer->isChecked()); + + client_->removePrimaryConsumer(calibration_consumer); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index b6fc1a354..e1322c6d1 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -245,6 +245,34 @@ TEST_F(UrDriverTest, reset_rtde_client) ASSERT_EQ(g_my_robot->ur_driver_->getControlFrequency(), target_frequency); } +TEST_F(UrDriverTest, read_error_code) +{ + g_my_robot->ur_driver_->startPrimaryClientCommunication(); + // Wait until we actually received a package + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + std::stringstream cmd; + cmd << "sec setup():" << std::endl << " protective_stop()" << std::endl << "end"; + EXPECT_TRUE(g_my_robot->ur_driver_->sendScript(cmd.str())); + + auto error_codes = g_my_robot->ur_driver_->getErrorCodes(); + while (error_codes.size() == 0) + { + error_codes = g_my_robot->ur_driver_->getErrorCodes(); + } + + ASSERT_EQ(error_codes.size(), 1); + // Check whether it is a "A protective stop was triggered" + // https://www.universal-robots.com/manuals/EN/HTML/SW5_21/Content/prod-err-codes/topics/CODE_209.html + ASSERT_EQ(error_codes.at(0).message_code, 209); + ASSERT_EQ(error_codes.at(0).message_argument, 0); + + // Wait for after PSTOP before clearing it + std::this_thread::sleep_for(std::chrono::seconds(6)); + + EXPECT_TRUE(g_my_robot->dashboard_client_->commandCloseSafetyPopup()); + EXPECT_TRUE(g_my_robot->dashboard_client_->commandUnlockProtectiveStop()); +} + // TODO we should add more tests for the UrDriver class. int main(int argc, char* argv[]) @@ -267,4 +295,4 @@ int main(int argc, char* argv[]) } return RUN_ALL_TESTS(); -} +} \ No newline at end of file