From 48de73faef6f8879ad5fae4897de24aeebb515a3 Mon Sep 17 00:00:00 2001 From: Jessica Chen Date: Wed, 6 Nov 2024 15:38:21 -0500 Subject: [PATCH 01/22] Exposed error code messages --- CMakeLists.txt | 3 + .../primary/primary_parser.h | 3 + .../ur_client_library/primary/robot_message.h | 11 ++ .../robot_message/error_code_message.h | 122 ++++++++++++++++++ .../ur_client_library/ur/error_code_client.h | 30 +++++ .../ur_client_library/ur/error_code_reader.h | 68 ++++++++++ include/ur_client_library/ur/ur_driver.h | 11 ++ .../robot_message/error_code_message.cpp | 70 ++++++++++ src/ur/error_code_client.cpp | 32 +++++ src/ur/error_code_reader.cpp | 59 +++++++++ src/ur/ur_driver.cpp | 13 ++ 11 files changed, 422 insertions(+) create mode 100644 include/ur_client_library/primary/robot_message/error_code_message.h create mode 100644 include/ur_client_library/ur/error_code_client.h create mode 100644 include/ur_client_library/ur/error_code_reader.h create mode 100644 src/primary/robot_message/error_code_message.cpp create mode 100644 src/ur/error_code_client.cpp create mode 100644 src/ur/error_code_reader.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 306df182d..5e1059eb8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ add_library(urcl SHARED src/primary/robot_message.cpp src/primary/robot_state.cpp src/primary/robot_message/version_message.cpp + src/primary/robot_message/error_code_message.cpp src/primary/robot_state/kinematics_info.cpp src/rtde/control_package_pause.cpp src/rtde/control_package_setup_inputs.cpp @@ -32,6 +33,8 @@ add_library(urcl SHARED src/rtde/rtde_client.cpp src/ur/ur_driver.cpp src/ur/calibration_checker.cpp + src/ur/error_code_reader.cpp + src/ur/error_code_client.cpp src/ur/dashboard_client.cpp src/ur/instruction_executor.cpp src/ur/tool_communication.cpp 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..c7b8701f7 100644 --- a/include/ur_client_library/primary/robot_message.h +++ b/include/ur_client_library/primary/robot_message.h @@ -65,6 +65,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..11ccdbc7a --- /dev/null +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -0,0 +1,122 @@ +// Below code is copied from a branch on the Universal_Robot_Client_Library +// with some modification. Link to branch: +// https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/improve_primary_interface +// +// 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 +}; + +struct ErrorCode +{ + int32_t message_code{-1}; + int32_t message_argument{-1}; + ReportLevel report_level{ReportLevel::DEBUG}; + uint8_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_; + 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/ur/error_code_client.h b/include/ur_client_library/ur/error_code_client.h new file mode 100644 index 000000000..9d3c35ab2 --- /dev/null +++ b/include/ur_client_library/ur/error_code_client.h @@ -0,0 +1,30 @@ +#include + +#include "ur_client_library/comm/stream.h" +#include +#include "ur_client_library/comm/producer.h" +#include "ur_client_library/primary/primary_package.h" +#include +#include +namespace urcl +{ +class ErrorCodeClient +{ +public: + ErrorCodeClient() = delete; + ErrorCodeClient(comm::URStream& stream, comm::INotifier& notifier, + primary_interface::PrimaryParser& parser); + ~ErrorCodeClient(); + +void start(); + +std::deque getErrorCodes(); + +private: + comm::URStream& stream_; + urcl::ErrorCodeReader consumer_; + primary_interface::PrimaryParser parser_; + comm::URProducer prod_; + comm::Pipeline pipeline_; +}; +} diff --git a/include/ur_client_library/ur/error_code_reader.h b/include/ur_client_library/ur/error_code_reader.h new file mode 100644 index 000000000..83049e51c --- /dev/null +++ b/include/ur_client_library/ur/error_code_reader.h @@ -0,0 +1,68 @@ +#ifndef UR_CLIENT_LIBRARY_UR_ERROR_CODE_READER_H_INCLUDED +#define UR_CLIENT_LIBRARY_UR_ERROR_CODE_READER_H_INCLUDED + +#include +#include +#include + +#include + +namespace urcl +{ +/*! + * \brief The ErrorCodeReader class consumes primary packages ignoring all but RobotCommMessage + * packages to retrieve the latest code reported by the robot. + */ +class ErrorCodeReader : public comm::IConsumer +{ +public: + virtual ~ErrorCodeReader() = default; + + /*! + * \brief Empty setup function, as no setup is needed. + */ + virtual void setupConsumer() + { + } + /*! + * \brief Tears down the consumer. + */ + virtual void teardownConsumer() + { + } + /*! + * \brief Stops the consumer. + */ + virtual void stopConsumer() + { + } + /*! + * \brief Handles timeouts. + */ + virtual void onTimeout() + { + } + + /*! + * \brief Consumes a package and pushes it into a queue if it is an ErrorCodeMessage package. + * + * \param product The package to consume + * + * \returns True, if the package was consumed correctly + */ + virtual bool consume(std::shared_ptr product); + + /*! + * \brief Retrieves a list of error codes from the queue if there are any. + * + * \return A list of error codes + */ + std::deque getErrorCodesFromQueue(); +private: + std::deque queue_; + std::mutex queue_mutex_; + +}; +} // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_UR_ERROR_CODE_READER_H_INCLUDED diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 6949c2d36..d3b62bff5 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -36,6 +36,7 @@ #include "ur_client_library/control/script_command_interface.h" #include "ur_client_library/control/script_sender.h" #include "ur_client_library/ur/tool_communication.h" +#include "ur_client_library/ur/error_code_client.h" #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" @@ -534,6 +535,14 @@ class UrDriver */ bool checkCalibration(const std::string& checksum); + /*! + * \brief Retrieves error codes ErrorCodeClient. + * + * \returns list of error codes + * + */ + std::deque getErrorCodes(); + /*! * \brief Getter for the RTDE writer used to write to the robot's RTDE interface. * @@ -668,6 +677,8 @@ class UrDriver comm::INotifier notifier_; std::unique_ptr rtde_client_; + comm::INotifier error_code_notifier_; + std::unique_ptr error_code_client_; std::unique_ptr reverse_interface_; std::unique_ptr trajectory_interface_; std::unique_ptr script_command_interface_; 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..f571e7aa3 --- /dev/null +++ b/src/primary/robot_message/error_code_message.cpp @@ -0,0 +1,70 @@ +// Below code is copied from a branch on the Universal_Robot_Client_Library +// with some modification. Link to branch: +// https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/improve_primary_interface +// +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver) +// Copyright 2017, 2018 Simon Rasmussen (refactor) +// +// Copyright 2015, 2016 Thomas Timm Andersen (original version) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Exner exner@fzi.de + * \date 2020-04-30 + * + */ +//---------------------------------------------------------------------- + +#include "ur_client_library/log.h" +#include "ur_client_library/primary/robot_message/error_code_message.h" +#include "ur_client_library/primary/abstract_primary_consumer.h" + +namespace urcl +{ +namespace primary_interface +{ +bool ErrorCodeMessage::parseWith(comm::BinParser& bp) +{ + bp.parse(message_code_); + bp.parse(message_argument_); + int32_t report_level; + bp.parse(report_level); + report_level_ = static_cast(report_level); + bp.parse(data_type_); + bp.parse(data_); + bp.parseRemainder(text_); + + return true; // not really possible to check dynamic size packets +} + +bool ErrorCodeMessage::consumeWith(AbstractPrimaryConsumer& consumer) +{ + return consumer.consume(*this); +} + +std::string ErrorCodeMessage::toString() const +{ + std::stringstream ss; + ss << "C" << message_code_ << "A" << message_argument_; + return ss.str(); +} + +} // namespace primary_interface +} // namespace urcl diff --git a/src/ur/error_code_client.cpp b/src/ur/error_code_client.cpp new file mode 100644 index 000000000..8b7120335 --- /dev/null +++ b/src/ur/error_code_client.cpp @@ -0,0 +1,32 @@ +#include "ur_client_library/ur/error_code_client.h" +#include "ur_client_library/primary/robot_message.h" +#include "ur_client_library/primary/robot_state.h" + +namespace urcl +{ +ErrorCodeClient::ErrorCodeClient(comm::URStream& stream, comm::INotifier& notifier, + primary_interface::PrimaryParser& parser) +: stream_(stream) +, parser_(parser) +, prod_(stream_, parser_) +, pipeline_(prod_, &consumer_, "ErrorCodeClient Pipeline", notifier) +{ +} + +ErrorCodeClient::~ErrorCodeClient() +{ + URCL_LOG_INFO("Stopping error code client pipeline"); + pipeline_.stop(); +} + +void ErrorCodeClient::start() +{ + URCL_LOG_INFO("Starting error code client pipeline"); + pipeline_.run(); +} + +std::deque ErrorCodeClient::getErrorCodes() +{ + return consumer_.getErrorCodesFromQueue(); +} +} diff --git a/src/ur/error_code_reader.cpp b/src/ur/error_code_reader.cpp new file mode 100644 index 000000000..527626971 --- /dev/null +++ b/src/ur/error_code_reader.cpp @@ -0,0 +1,59 @@ +#include + +namespace urcl +{ +bool ErrorCodeReader::consume(std::shared_ptr product) +{ + auto error_code_message = std::dynamic_pointer_cast(product); + if (error_code_message != nullptr) + { + urcl::primary_interface::ErrorCode code; + code.message_code = error_code_message->message_code_; + code.message_argument = error_code_message->message_argument_; + code.report_level = error_code_message->report_level_; + code.data_type = error_code_message->data_type_; + code.data = error_code_message->data_; + code.text = error_code_message->text_; + code.timestamp = error_code_message->timestamp_; + code.to_string = error_code_message->toString(); + + const auto logContents = "Logging an ErrorCodeMessage from the UR Controller Box: " + error_code_message->toString(); + + switch(code.report_level) { + case urcl::primary_interface::ReportLevel::DEBUG: + case urcl::primary_interface::ReportLevel::DEVL_DEBUG: + URCL_LOG_DEBUG(logContents.c_str()); + break; + case urcl::primary_interface::ReportLevel::INFO: + case urcl::primary_interface::ReportLevel::DEVL_INFO: + URCL_LOG_INFO(logContents.c_str()); + break; + case urcl::primary_interface::ReportLevel::WARNING: + case urcl::primary_interface::ReportLevel::DEVL_WARNING: + URCL_LOG_WARN(logContents.c_str()); + break; + default: + //urcl::primary_interface::ReportLevel::VIOLATION: + //urcl::primary_interface::ReportLevel::DEVL_VIOLATION: + //urcl::primary_interface::ReportLevel::FAULT: + //urcl::primary_interface::ReportLevel::DEVL_FAULT: + URCL_LOG_ERROR(logContents.c_str()); + break; + } + + std::lock_guard lock_guard(queue_mutex_); + queue_.push_back(code); + } + return true; +} + +std::deque ErrorCodeReader::getErrorCodesFromQueue() +{ + std::lock_guard lock_guard(queue_mutex_); + std::deque error_codes; + error_codes = queue_; + queue_.clear(); + return error_codes; +} + +} // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 88a485fdb..2efa9e238 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -38,6 +38,7 @@ #include #include +#include namespace urcl { @@ -74,6 +75,9 @@ 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_interface::PrimaryParser parser; + error_code_client_.reset(new ErrorCodeClient(*primary_stream_, error_code_notifier_, parser)); + non_blocking_read_ = non_blocking_read; get_packet_timeout_ = non_blocking_read_ ? 0 : 100; @@ -727,4 +731,13 @@ void UrDriver::setupReverseInterface(const uint32_t reverse_port) reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state_, step_time)); } +void UrDriver::startErrorCodeClientCommunication() +{ + error_code_client_->start(); +} + +std::deque UrDriver::getErrorCodes() +{ + return error_code_client_->getErrorCodes(); +} } // namespace urcl From a227b37b6ea7e8f2b6606643134437147fbbfb4f Mon Sep 17 00:00:00 2001 From: Jessica Chen Date: Wed, 6 Nov 2024 16:10:09 -0500 Subject: [PATCH 02/22] Log all ReportLevel::DEVL_* as debug --- src/ur/error_code_reader.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/ur/error_code_reader.cpp b/src/ur/error_code_reader.cpp index 527626971..12208c44c 100644 --- a/src/ur/error_code_reader.cpp +++ b/src/ur/error_code_reader.cpp @@ -22,21 +22,21 @@ bool ErrorCodeReader::consume(std::shared_ptr 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(logContents.c_str()); break; case urcl::primary_interface::ReportLevel::INFO: - case urcl::primary_interface::ReportLevel::DEVL_INFO: URCL_LOG_INFO(logContents.c_str()); break; case urcl::primary_interface::ReportLevel::WARNING: - case urcl::primary_interface::ReportLevel::DEVL_WARNING: URCL_LOG_WARN(logContents.c_str()); break; default: //urcl::primary_interface::ReportLevel::VIOLATION: - //urcl::primary_interface::ReportLevel::DEVL_VIOLATION: //urcl::primary_interface::ReportLevel::FAULT: - //urcl::primary_interface::ReportLevel::DEVL_FAULT: URCL_LOG_ERROR(logContents.c_str()); break; } From b8c9b12cc49d492303551cc6fc496cad64bc1294 Mon Sep 17 00:00:00 2001 From: Jessica Chen Date: Wed, 6 Nov 2024 16:34:06 -0500 Subject: [PATCH 03/22] Changed data_type to uint32_t --- .../primary/robot_message/error_code_message.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ur_client_library/primary/robot_message/error_code_message.h b/include/ur_client_library/primary/robot_message/error_code_message.h index 11ccdbc7a..5e3825ef1 100644 --- a/include/ur_client_library/primary/robot_message/error_code_message.h +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -57,7 +57,7 @@ struct ErrorCode int32_t message_code{-1}; int32_t message_argument{-1}; ReportLevel report_level{ReportLevel::DEBUG}; - uint8_t data_type{0}; + uint32_t data_type{0}; uint32_t data{0}; std::string text; uint64_t timestamp{0}; From 5252119bed5f82f95019b0658acae2bc40103476 Mon Sep 17 00:00:00 2001 From: Jessica Chen Date: Thu, 14 Nov 2024 16:23:40 -0500 Subject: [PATCH 04/22] Added integration test --- tests/test_ur_driver.cpp | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index b6fc1a354..bbbfd581b 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -245,6 +245,29 @@ TEST_F(UrDriverTest, reset_rtde_client) ASSERT_EQ(g_my_robot->ur_driver_->getControlFrequency(), target_frequency); } +TEST_F(UrDriverTest, read_error_code) +{ + g_ur_driver_->startErrorCodeClientCommunication(); + std::stringstream cmd; + cmd << "sec setup():" << std::endl << " protective_stop()" << std::endl << "end"; + EXPECT_TRUE(g_ur_driver_->sendScript(cmd.str())); + + auto error_codes = g_ur_driver_->getErrorCodes(); + while (error_codes.size() == 0) { + error_codes = g_ur_driver_->getErrorCodes(); + } + + ASSERT_EQ(error_codes.size(), 1); + ASSERT_EQ(error_codes.at(0).message_code, 209); + ASSERT_EQ(error_codes.at(0).message_argument, 0); + + // Wait for PSTOP to show up on TP so we can clear it + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + EXPECT_TRUE(g_dashboard_client_->commandCloseSafetyPopup()); + EXPECT_TRUE(g_dashboard_client_->commandUnlockProtectiveStop()); +} + // TODO we should add more tests for the UrDriver class. int main(int argc, char* argv[]) From 3b0f9642c56b5873fedef403ef9948866bf9ae65 Mon Sep 17 00:00:00 2001 From: Jessica Chen Date: Fri, 15 Nov 2024 10:30:56 -0500 Subject: [PATCH 05/22] Increased wait before clearing PStop --- tests/test_ur_driver.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index bbbfd581b..29c683b56 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -251,9 +251,10 @@ TEST_F(UrDriverTest, read_error_code) std::stringstream cmd; cmd << "sec setup():" << std::endl << " protective_stop()" << std::endl << "end"; EXPECT_TRUE(g_ur_driver_->sendScript(cmd.str())); - + auto error_codes = g_ur_driver_->getErrorCodes(); - while (error_codes.size() == 0) { + while (error_codes.size() == 0) + { error_codes = g_ur_driver_->getErrorCodes(); } @@ -261,8 +262,8 @@ TEST_F(UrDriverTest, read_error_code) ASSERT_EQ(error_codes.at(0).message_code, 209); ASSERT_EQ(error_codes.at(0).message_argument, 0); - // Wait for PSTOP to show up on TP so we can clear it - std::this_thread::sleep_for(std::chrono::milliseconds(200)); + // Wait for 5s after PSTOP before clearing it + std::this_thread::sleep_for(std::chrono::seconds(5)); EXPECT_TRUE(g_dashboard_client_->commandCloseSafetyPopup()); EXPECT_TRUE(g_dashboard_client_->commandUnlockProtectiveStop()); From 262cb0cc9e16bacfd5c17301f36865b116cd05f8 Mon Sep 17 00:00:00 2001 From: Jessica Chen Date: Fri, 15 Nov 2024 10:35:27 -0500 Subject: [PATCH 06/22] Lint --- .../ur_client_library/primary/robot_message.h | 2 +- .../robot_message/error_code_message.h | 20 ++++++++-------- .../ur_client_library/ur/error_code_client.h | 12 +++++----- .../ur_client_library/ur/error_code_reader.h | 12 +++++----- include/ur_client_library/ur/ur_driver.h | 10 ++++---- .../robot_message/error_code_message.cpp | 2 +- src/ur/error_code_client.cpp | 24 +++++++++---------- src/ur/error_code_reader.cpp | 12 ++++++---- 8 files changed, 48 insertions(+), 46 deletions(-) diff --git a/include/ur_client_library/primary/robot_message.h b/include/ur_client_library/primary/robot_message.h index c7b8701f7..120a37e26 100644 --- a/include/ur_client_library/primary/robot_message.h +++ b/include/ur_client_library/primary/robot_message.h @@ -66,7 +66,7 @@ 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 diff --git a/include/ur_client_library/primary/robot_message/error_code_message.h b/include/ur_client_library/primary/robot_message/error_code_message.h index 5e3825ef1..27edb0ab4 100644 --- a/include/ur_client_library/primary/robot_message/error_code_message.h +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -1,7 +1,7 @@ // Below code is copied from a branch on the Universal_Robot_Client_Library -// with some modification. Link to branch: +// with some modification. Link to branch: // https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/improve_primary_interface -// +// // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- // -- BEGIN LICENSE BLOCK ---------------------------------------------- @@ -52,15 +52,15 @@ enum class ReportLevel : int32_t DEVL_FAULT = 132 }; -struct ErrorCode +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}; + 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}; + uint64_t timestamp{ 0 }; std::string to_string; }; @@ -112,7 +112,7 @@ class ErrorCodeMessage : public RobotMessage int32_t message_code_; int32_t message_argument_; ReportLevel report_level_; - uint8_t data_type_; + uint32_t data_type_; uint32_t data_; std::string text_; }; diff --git a/include/ur_client_library/ur/error_code_client.h b/include/ur_client_library/ur/error_code_client.h index 9d3c35ab2..4e1e04f3c 100644 --- a/include/ur_client_library/ur/error_code_client.h +++ b/include/ur_client_library/ur/error_code_client.h @@ -10,15 +10,15 @@ namespace urcl { class ErrorCodeClient { -public: +public: ErrorCodeClient() = delete; - ErrorCodeClient(comm::URStream& stream, comm::INotifier& notifier, - primary_interface::PrimaryParser& parser); + ErrorCodeClient(comm::URStream& stream, comm::INotifier& notifier, + primary_interface::PrimaryParser& parser); ~ErrorCodeClient(); -void start(); + void start(); -std::deque getErrorCodes(); + std::deque getErrorCodes(); private: comm::URStream& stream_; @@ -27,4 +27,4 @@ std::deque getErrorCodes(); comm::URProducer prod_; comm::Pipeline pipeline_; }; -} +} // namespace urcl diff --git a/include/ur_client_library/ur/error_code_reader.h b/include/ur_client_library/ur/error_code_reader.h index 83049e51c..ca2ac8181 100644 --- a/include/ur_client_library/ur/error_code_reader.h +++ b/include/ur_client_library/ur/error_code_reader.h @@ -11,7 +11,7 @@ namespace urcl { /*! * \brief The ErrorCodeReader class consumes primary packages ignoring all but RobotCommMessage - * packages to retrieve the latest code reported by the robot. + * packages to retrieve the latest code reported by the robot. */ class ErrorCodeReader : public comm::IConsumer { @@ -53,15 +53,15 @@ class ErrorCodeReader : public comm::IConsumer product); /*! - * \brief Retrieves a list of error codes from the queue if there are any. - * - * \return A list of error codes - */ + * \brief Retrieves a list of error codes from the queue if there are any. + * + * \return A list of error codes + */ std::deque getErrorCodesFromQueue(); + private: std::deque queue_; std::mutex queue_mutex_; - }; } // namespace urcl diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index d3b62bff5..1f269ce49 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -536,11 +536,11 @@ class UrDriver bool checkCalibration(const std::string& checksum); /*! - * \brief Retrieves error codes ErrorCodeClient. - * - * \returns list of error codes - * - */ + * \brief Retrieves error codes ErrorCodeClient. + * + * \returns list of error codes + * + */ std::deque getErrorCodes(); /*! diff --git a/src/primary/robot_message/error_code_message.cpp b/src/primary/robot_message/error_code_message.cpp index f571e7aa3..6524f5558 100644 --- a/src/primary/robot_message/error_code_message.cpp +++ b/src/primary/robot_message/error_code_message.cpp @@ -1,7 +1,7 @@ // Below code is copied from a branch on the Universal_Robot_Client_Library // with some modification. Link to branch: // https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/improve_primary_interface -// +// // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- // -- BEGIN LICENSE BLOCK ---------------------------------------------- diff --git a/src/ur/error_code_client.cpp b/src/ur/error_code_client.cpp index 8b7120335..73f6a5a3b 100644 --- a/src/ur/error_code_client.cpp +++ b/src/ur/error_code_client.cpp @@ -4,29 +4,29 @@ namespace urcl { -ErrorCodeClient::ErrorCodeClient(comm::URStream& stream, comm::INotifier& notifier, - primary_interface::PrimaryParser& parser) -: stream_(stream) -, parser_(parser) -, prod_(stream_, parser_) -, pipeline_(prod_, &consumer_, "ErrorCodeClient Pipeline", notifier) +ErrorCodeClient::ErrorCodeClient(comm::URStream& stream, comm::INotifier& notifier, + primary_interface::PrimaryParser& parser) + : stream_(stream) + , parser_(parser) + , prod_(stream_, parser_) + , pipeline_(prod_, &consumer_, "ErrorCodeClient Pipeline", notifier) { } ErrorCodeClient::~ErrorCodeClient() { - URCL_LOG_INFO("Stopping error code client pipeline"); - pipeline_.stop(); + URCL_LOG_INFO("Stopping error code client pipeline"); + pipeline_.stop(); } void ErrorCodeClient::start() { - URCL_LOG_INFO("Starting error code client pipeline"); - pipeline_.run(); + URCL_LOG_INFO("Starting error code client pipeline"); + pipeline_.run(); } std::deque ErrorCodeClient::getErrorCodes() { - return consumer_.getErrorCodesFromQueue(); -} + return consumer_.getErrorCodesFromQueue(); } +} // namespace urcl diff --git a/src/ur/error_code_reader.cpp b/src/ur/error_code_reader.cpp index 12208c44c..220987ef0 100644 --- a/src/ur/error_code_reader.cpp +++ b/src/ur/error_code_reader.cpp @@ -17,9 +17,11 @@ bool ErrorCodeReader::consume(std::shared_ptr code.timestamp = error_code_message->timestamp_; code.to_string = error_code_message->toString(); - const auto logContents = "Logging an ErrorCodeMessage from the UR Controller Box: " + error_code_message->toString(); + const auto logContents = + "Logging an ErrorCodeMessage from the UR Controller Box: " + error_code_message->toString(); - switch(code.report_level) { + switch (code.report_level) + { case urcl::primary_interface::ReportLevel::DEBUG: case urcl::primary_interface::ReportLevel::DEVL_DEBUG: case urcl::primary_interface::ReportLevel::DEVL_INFO: @@ -35,8 +37,8 @@ bool ErrorCodeReader::consume(std::shared_ptr URCL_LOG_WARN(logContents.c_str()); break; default: - //urcl::primary_interface::ReportLevel::VIOLATION: - //urcl::primary_interface::ReportLevel::FAULT: + // urcl::primary_interface::ReportLevel::VIOLATION: + // urcl::primary_interface::ReportLevel::FAULT: URCL_LOG_ERROR(logContents.c_str()); break; } @@ -47,7 +49,7 @@ bool ErrorCodeReader::consume(std::shared_ptr return true; } -std::deque ErrorCodeReader::getErrorCodesFromQueue() +std::deque ErrorCodeReader::getErrorCodesFromQueue() { std::lock_guard lock_guard(queue_mutex_); std::deque error_codes; From be9fc61b8ff979543b9a003748bd112ca8b7243f Mon Sep 17 00:00:00 2001 From: Jessica Chen Date: Wed, 15 Jan 2025 15:58:54 -0500 Subject: [PATCH 07/22] Addressed PR comments --- CMakeLists.txt | 2 +- include/ur_client_library/comm/pipeline.h | 42 ++++- .../primary/abstract_primary_consumer.h | 4 +- .../primary/primary_client.h | 59 ++++++ .../primary/primary_consumer.h | 172 ++++++++++++++++++ .../robot_message/error_code_message.h | 4 - .../ur_client_library/ur/error_code_client.h | 30 --- include/ur_client_library/ur/ur_driver.h | 9 +- src/primary/primary_client.cpp | 63 +++++++ .../robot_message/error_code_message.cpp | 4 - src/ur/error_code_client.cpp | 32 ---- src/ur/ur_driver.cpp | 9 +- tests/test_ur_driver.cpp | 12 +- 13 files changed, 353 insertions(+), 89 deletions(-) create mode 100644 include/ur_client_library/primary/primary_client.h create mode 100644 include/ur_client_library/primary/primary_consumer.h delete mode 100644 include/ur_client_library/ur/error_code_client.h create mode 100644 src/primary/primary_client.cpp delete mode 100644 src/ur/error_code_client.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5e1059eb8..1ac2eee3f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ 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 @@ -34,7 +35,6 @@ add_library(urcl SHARED src/ur/ur_driver.cpp src/ur/calibration_checker.cpp src/ur/error_code_reader.cpp - src/ur/error_code_client.cpp src/ur/dashboard_client.cpp src/ur/instruction_executor.cpp src/ur/tool_communication.cpp diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h index afd9100bf..2e7176554 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..215cbe854 --- /dev/null +++ b/include/ur_client_library/primary/primary_client.h @@ -0,0 +1,59 @@ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +namespace urcl +{ +namespace primary_interface +{ +class PrimaryClient +{ +public: + PrimaryClient() = delete; + PrimaryClient(comm::URStream& stream); + ~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(); + + 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 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..1cbd78934 --- /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 logContents = + "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(logContents.c_str()); + break; + case urcl::primary_interface::ReportLevel::INFO: + URCL_LOG_INFO(logContents.c_str()); + break; + case urcl::primary_interface::ReportLevel::WARNING: + URCL_LOG_WARN(logContents.c_str()); + break; + default: + // urcl::primary_interface::ReportLevel::VIOLATION: + // urcl::primary_interface::ReportLevel::FAULT: + URCL_LOG_ERROR(logContents.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 \ No newline at end of file diff --git a/include/ur_client_library/primary/robot_message/error_code_message.h b/include/ur_client_library/primary/robot_message/error_code_message.h index 27edb0ab4..e672d927e 100644 --- a/include/ur_client_library/primary/robot_message/error_code_message.h +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -1,7 +1,3 @@ -// Below code is copied from a branch on the Universal_Robot_Client_Library -// with some modification. Link to branch: -// https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/improve_primary_interface -// // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- // -- BEGIN LICENSE BLOCK ---------------------------------------------- diff --git a/include/ur_client_library/ur/error_code_client.h b/include/ur_client_library/ur/error_code_client.h deleted file mode 100644 index 4e1e04f3c..000000000 --- a/include/ur_client_library/ur/error_code_client.h +++ /dev/null @@ -1,30 +0,0 @@ -#include - -#include "ur_client_library/comm/stream.h" -#include -#include "ur_client_library/comm/producer.h" -#include "ur_client_library/primary/primary_package.h" -#include -#include -namespace urcl -{ -class ErrorCodeClient -{ -public: - ErrorCodeClient() = delete; - ErrorCodeClient(comm::URStream& stream, comm::INotifier& notifier, - primary_interface::PrimaryParser& parser); - ~ErrorCodeClient(); - - void start(); - - std::deque getErrorCodes(); - -private: - comm::URStream& stream_; - urcl::ErrorCodeReader consumer_; - primary_interface::PrimaryParser parser_; - comm::URProducer prod_; - comm::Pipeline pipeline_; -}; -} // namespace urcl diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 1f269ce49..c6f403951 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -36,7 +36,7 @@ #include "ur_client_library/control/script_command_interface.h" #include "ur_client_library/control/script_sender.h" #include "ur_client_library/ur/tool_communication.h" -#include "ur_client_library/ur/error_code_client.h" +#include "ur_client_library/primary/primary_client.h" #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" @@ -656,6 +656,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); @@ -678,7 +683,7 @@ class UrDriver comm::INotifier notifier_; std::unique_ptr rtde_client_; comm::INotifier error_code_notifier_; - std::unique_ptr error_code_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..dd7cc013b --- /dev/null +++ b/src/primary/primary_client.cpp @@ -0,0 +1,63 @@ +#include +#include +#include + +namespace urcl +{ +namespace primary_interface +{ +PrimaryClient::PrimaryClient(comm::URStream& stream) + : stream_(stream) +{ + 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_->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 index 6524f5558..1dc558be8 100644 --- a/src/primary/robot_message/error_code_message.cpp +++ b/src/primary/robot_message/error_code_message.cpp @@ -1,7 +1,3 @@ -// Below code is copied from a branch on the Universal_Robot_Client_Library -// with some modification. Link to branch: -// https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/improve_primary_interface -// // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- // -- BEGIN LICENSE BLOCK ---------------------------------------------- diff --git a/src/ur/error_code_client.cpp b/src/ur/error_code_client.cpp deleted file mode 100644 index 73f6a5a3b..000000000 --- a/src/ur/error_code_client.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include "ur_client_library/ur/error_code_client.h" -#include "ur_client_library/primary/robot_message.h" -#include "ur_client_library/primary/robot_state.h" - -namespace urcl -{ -ErrorCodeClient::ErrorCodeClient(comm::URStream& stream, comm::INotifier& notifier, - primary_interface::PrimaryParser& parser) - : stream_(stream) - , parser_(parser) - , prod_(stream_, parser_) - , pipeline_(prod_, &consumer_, "ErrorCodeClient Pipeline", notifier) -{ -} - -ErrorCodeClient::~ErrorCodeClient() -{ - URCL_LOG_INFO("Stopping error code client pipeline"); - pipeline_.stop(); -} - -void ErrorCodeClient::start() -{ - URCL_LOG_INFO("Starting error code client pipeline"); - pipeline_.run(); -} - -std::deque ErrorCodeClient::getErrorCodes() -{ - return consumer_.getErrorCodesFromQueue(); -} -} // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 2efa9e238..7ae1a5fed 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -75,8 +75,7 @@ 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_interface::PrimaryParser parser; - error_code_client_.reset(new ErrorCodeClient(*primary_stream_, error_code_notifier_, parser)); + primary_client_.reset(new urcl::primary_interface::PrimaryClient(*primary_stream_)); non_blocking_read_ = non_blocking_read; get_packet_timeout_ = non_blocking_read_ ? 0 : 100; @@ -731,13 +730,13 @@ void UrDriver::setupReverseInterface(const uint32_t reverse_port) reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state_, step_time)); } -void UrDriver::startErrorCodeClientCommunication() +void UrDriver::startPrimaryClientCommunication() { - error_code_client_->start(); + primary_client_->start(); } std::deque UrDriver::getErrorCodes() { - return error_code_client_->getErrorCodes(); + return primary_client_->getErrorCodes(); } } // namespace urcl diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 29c683b56..b1a3bc828 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -247,15 +247,15 @@ TEST_F(UrDriverTest, reset_rtde_client) TEST_F(UrDriverTest, read_error_code) { - g_ur_driver_->startErrorCodeClientCommunication(); + g_ur_driver->startPrimaryClientCommunication(); std::stringstream cmd; cmd << "sec setup():" << std::endl << " protective_stop()" << std::endl << "end"; - EXPECT_TRUE(g_ur_driver_->sendScript(cmd.str())); + EXPECT_TRUE(g_ur_driver->sendScript(cmd.str())); - auto error_codes = g_ur_driver_->getErrorCodes(); + auto error_codes = g_ur_driver->getErrorCodes(); while (error_codes.size() == 0) { - error_codes = g_ur_driver_->getErrorCodes(); + error_codes = g_ur_driver->getErrorCodes(); } ASSERT_EQ(error_codes.size(), 1); @@ -265,8 +265,8 @@ TEST_F(UrDriverTest, read_error_code) // Wait for 5s after PSTOP before clearing it std::this_thread::sleep_for(std::chrono::seconds(5)); - EXPECT_TRUE(g_dashboard_client_->commandCloseSafetyPopup()); - EXPECT_TRUE(g_dashboard_client_->commandUnlockProtectiveStop()); + EXPECT_TRUE(g_dashboard_client->commandCloseSafetyPopup()); + EXPECT_TRUE(g_dashboard_client->commandUnlockProtectiveStop()); } // TODO we should add more tests for the UrDriver class. From 4aaee742f303f4cae759754a7c229436ae4f58f3 Mon Sep 17 00:00:00 2001 From: Jessica Chen Date: Wed, 15 Jan 2025 16:44:28 -0500 Subject: [PATCH 08/22] Cleanup --- CMakeLists.txt | 1 - .../primary/primary_client.h | 8 ++- .../ur_client_library/ur/error_code_reader.h | 68 ------------------- src/ur/error_code_reader.cpp | 61 ----------------- src/ur/ur_driver.cpp | 1 - tests/test_ur_driver.cpp | 2 + 6 files changed, 9 insertions(+), 132 deletions(-) delete mode 100644 include/ur_client_library/ur/error_code_reader.h delete mode 100644 src/ur/error_code_reader.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1ac2eee3f..d1d5338cf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,7 +34,6 @@ add_library(urcl SHARED src/rtde/rtde_client.cpp src/ur/ur_driver.cpp src/ur/calibration_checker.cpp - src/ur/error_code_reader.cpp src/ur/dashboard_client.cpp src/ur/instruction_executor.cpp src/ur/tool_communication.cpp diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index 215cbe854..2851027b8 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -1,4 +1,8 @@ +#ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED +#define UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED + #include +#include #include #include @@ -7,7 +11,7 @@ #include #include #include -#include + namespace urcl { namespace primary_interface @@ -57,3 +61,5 @@ class PrimaryClient } // namespace primary_interface } // namespace urcl + +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CLIENT_H_INCLUDED diff --git a/include/ur_client_library/ur/error_code_reader.h b/include/ur_client_library/ur/error_code_reader.h deleted file mode 100644 index ca2ac8181..000000000 --- a/include/ur_client_library/ur/error_code_reader.h +++ /dev/null @@ -1,68 +0,0 @@ -#ifndef UR_CLIENT_LIBRARY_UR_ERROR_CODE_READER_H_INCLUDED -#define UR_CLIENT_LIBRARY_UR_ERROR_CODE_READER_H_INCLUDED - -#include -#include -#include - -#include - -namespace urcl -{ -/*! - * \brief The ErrorCodeReader class consumes primary packages ignoring all but RobotCommMessage - * packages to retrieve the latest code reported by the robot. - */ -class ErrorCodeReader : public comm::IConsumer -{ -public: - virtual ~ErrorCodeReader() = default; - - /*! - * \brief Empty setup function, as no setup is needed. - */ - virtual void setupConsumer() - { - } - /*! - * \brief Tears down the consumer. - */ - virtual void teardownConsumer() - { - } - /*! - * \brief Stops the consumer. - */ - virtual void stopConsumer() - { - } - /*! - * \brief Handles timeouts. - */ - virtual void onTimeout() - { - } - - /*! - * \brief Consumes a package and pushes it into a queue if it is an ErrorCodeMessage package. - * - * \param product The package to consume - * - * \returns True, if the package was consumed correctly - */ - virtual bool consume(std::shared_ptr product); - - /*! - * \brief Retrieves a list of error codes from the queue if there are any. - * - * \return A list of error codes - */ - std::deque getErrorCodesFromQueue(); - -private: - std::deque queue_; - std::mutex queue_mutex_; -}; -} // namespace urcl - -#endif // ifndef UR_CLIENT_LIBRARY_UR_ERROR_CODE_READER_H_INCLUDED diff --git a/src/ur/error_code_reader.cpp b/src/ur/error_code_reader.cpp deleted file mode 100644 index 220987ef0..000000000 --- a/src/ur/error_code_reader.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#include - -namespace urcl -{ -bool ErrorCodeReader::consume(std::shared_ptr product) -{ - auto error_code_message = std::dynamic_pointer_cast(product); - if (error_code_message != nullptr) - { - urcl::primary_interface::ErrorCode code; - code.message_code = error_code_message->message_code_; - code.message_argument = error_code_message->message_argument_; - code.report_level = error_code_message->report_level_; - code.data_type = error_code_message->data_type_; - code.data = error_code_message->data_; - code.text = error_code_message->text_; - code.timestamp = error_code_message->timestamp_; - code.to_string = error_code_message->toString(); - - const auto logContents = - "Logging an ErrorCodeMessage from the UR Controller Box: " + error_code_message->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(logContents.c_str()); - break; - case urcl::primary_interface::ReportLevel::INFO: - URCL_LOG_INFO(logContents.c_str()); - break; - case urcl::primary_interface::ReportLevel::WARNING: - URCL_LOG_WARN(logContents.c_str()); - break; - default: - // urcl::primary_interface::ReportLevel::VIOLATION: - // urcl::primary_interface::ReportLevel::FAULT: - URCL_LOG_ERROR(logContents.c_str()); - break; - } - - std::lock_guard lock_guard(queue_mutex_); - queue_.push_back(code); - } - return true; -} - -std::deque ErrorCodeReader::getErrorCodesFromQueue() -{ - std::lock_guard lock_guard(queue_mutex_); - std::deque error_codes; - error_codes = queue_; - queue_.clear(); - return error_codes; -} - -} // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 7ae1a5fed..68226f5f2 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -38,7 +38,6 @@ #include #include -#include namespace urcl { diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index b1a3bc828..9e2397dea 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -259,6 +259,8 @@ TEST_F(UrDriverTest, read_error_code) } ASSERT_EQ(error_codes.size(), 1); + // Error code description can be found here: + // https://myur.universal-robots.com/manuals/content/SW_5_16/Documentation%20Menu/Error%20Codes/Introduction/C209%20A%20protective%20stop%20was%20triggered%20%28for%20test%20purposes%20only%29 ASSERT_EQ(error_codes.at(0).message_code, 209); ASSERT_EQ(error_codes.at(0).message_argument, 0); From c4c74d24158d9745af60922b9be9526fe7694f3d Mon Sep 17 00:00:00 2001 From: Jessica Chen Date: Thu, 16 Jan 2025 14:53:16 -0500 Subject: [PATCH 09/22] Minor cleanup --- include/ur_client_library/primary/primary_consumer.h | 2 +- include/ur_client_library/ur/ur_driver.h | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h index 1cbd78934..9496405a7 100644 --- a/include/ur_client_library/primary/primary_consumer.h +++ b/include/ur_client_library/primary/primary_consumer.h @@ -169,4 +169,4 @@ class PrimaryConsumer : public AbstractPrimaryConsumer } // namespace primary_interface } // namespace urcl -#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED \ No newline at end of file +#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_CONSUMER_H_INCLUDED diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index c6f403951..a72edb847 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -36,9 +36,9 @@ #include "ur_client_library/control/script_command_interface.h" #include "ur_client_library/control/script_sender.h" #include "ur_client_library/ur/tool_communication.h" -#include "ur_client_library/primary/primary_client.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" @@ -536,7 +536,7 @@ class UrDriver bool checkCalibration(const std::string& checksum); /*! - * \brief Retrieves error codes ErrorCodeClient. + * \brief Retrieves error codes from PrimaryClient. * * \returns list of error codes * @@ -682,7 +682,6 @@ class UrDriver comm::INotifier notifier_; std::unique_ptr rtde_client_; - comm::INotifier error_code_notifier_; std::unique_ptr primary_client_; std::unique_ptr reverse_interface_; std::unique_ptr trajectory_interface_; From ab051611983cbdb6002928c809659539541a6a22 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 17 Jan 2025 17:20:25 +0100 Subject: [PATCH 10/22] Apply suggestions from code review --- include/ur_client_library/primary/primary_client.h | 11 +++++++---- include/ur_client_library/ur/ur_driver.h | 2 +- src/primary/primary_client.cpp | 9 +++++---- src/ur/ur_driver.cpp | 2 +- tests/test_ur_driver.cpp | 7 +++++-- 5 files changed, 19 insertions(+), 12 deletions(-) diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index 2851027b8..cd6fe7091 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -20,7 +20,7 @@ class PrimaryClient { public: PrimaryClient() = delete; - PrimaryClient(comm::URStream& stream); + PrimaryClient(const std::string& robot_ip, comm::INotifier& notifier); ~PrimaryClient(); /*! @@ -28,16 +28,19 @@ class PrimaryClient * * \param primary_consumer Primary consumer that should be added to the list */ - void addPrimaryConsumer (std::shared_ptr primary_consumer); + 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 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: @@ -51,7 +54,7 @@ class PrimaryClient comm::INotifier notifier_; - comm::URStream& stream_; + comm::URStream stream_; std::unique_ptr> prod_; std::unique_ptr> pipeline_; diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index a72edb847..b7d8e3f2a 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -536,7 +536,7 @@ class UrDriver bool checkCalibration(const std::string& checksum); /*! - * \brief Retrieves error codes from PrimaryClient. + * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be deleted. * * \returns list of error codes * diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index dd7cc013b..77df29a7a 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -6,8 +6,8 @@ namespace urcl { namespace primary_interface { -PrimaryClient::PrimaryClient(comm::URStream& stream) - : stream_(stream) +PrimaryClient::PrimaryClient(const std::string& robot_ip, comm::INotifier& notifier) + : stream_(robot_ip, UR_PRIMARY_PORT) { prod_.reset(new comm::URProducer(stream_, parser_)); @@ -32,15 +32,16 @@ PrimaryClient::~PrimaryClient() void PrimaryClient::start() { URCL_LOG_INFO("Starting primary client pipeline"); + pipeline_->init(); pipeline_->run(); } -void PrimaryClient::addPrimaryConsumer(std::shared_ptr primary_consumer) +void PrimaryClient::addPrimaryConsumer(std::shared_ptr> primary_consumer) { multi_consumer_->addConsumer(primary_consumer); } -void PrimaryClient::removePrimaryConsumer(std::shared_ptr primary_consumer) +void PrimaryClient::removePrimaryConsumer(std::shared_ptr> primary_consumer) { multi_consumer_->removeConsumer(primary_consumer); } diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 68226f5f2..5e36cf1a6 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -74,7 +74,7 @@ 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(*primary_stream_)); + 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; diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 9e2397dea..92ae97ded 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -247,7 +247,10 @@ TEST_F(UrDriverTest, reset_rtde_client) TEST_F(UrDriverTest, read_error_code) { + g_consume_rtde_packages = true; g_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_ur_driver->sendScript(cmd.str())); @@ -264,8 +267,8 @@ TEST_F(UrDriverTest, read_error_code) ASSERT_EQ(error_codes.at(0).message_code, 209); ASSERT_EQ(error_codes.at(0).message_argument, 0); - // Wait for 5s after PSTOP before clearing it - std::this_thread::sleep_for(std::chrono::seconds(5)); + // Wait for after PSTOP before clearing it + std::this_thread::sleep_for(std::chrono::milliseconds(100)); EXPECT_TRUE(g_dashboard_client->commandCloseSafetyPopup()); EXPECT_TRUE(g_dashboard_client->commandUnlockProtectiveStop()); From 0460982c916b2d119d058914548cd0ada9701b13 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 17 Jan 2025 17:35:04 +0100 Subject: [PATCH 11/22] Code formatting --- include/ur_client_library/primary/primary_client.h | 6 +++--- include/ur_client_library/primary/primary_consumer.h | 10 +++++----- include/ur_client_library/ur/ur_driver.h | 3 ++- src/primary/primary_client.cpp | 7 ++++--- src/ur/ur_driver.cpp | 2 +- 5 files changed, 15 insertions(+), 13 deletions(-) diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index cd6fe7091..46eadd59a 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -28,7 +28,7 @@ class PrimaryClient * * \param primary_consumer Primary consumer that should be added to the list */ - void addPrimaryConsumer (std::shared_ptr> primary_consumer); + void addPrimaryConsumer(std::shared_ptr> primary_consumer); /*! * \brief Remove a primary consumer from the list of consumers @@ -39,12 +39,12 @@ class PrimaryClient void start(); /*! - * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be deleted. + * \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); diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h index 9496405a7..10e6b285d 100644 --- a/include/ur_client_library/primary/primary_consumer.h +++ b/include/ur_client_library/primary/primary_consumer.h @@ -47,7 +47,9 @@ namespace primary_interface class PrimaryConsumer : public AbstractPrimaryConsumer { public: - PrimaryConsumer() {} + PrimaryConsumer() + { + } virtual ~PrimaryConsumer() = default; /*! @@ -117,8 +119,7 @@ class PrimaryConsumer : public AbstractPrimaryConsumer code.timestamp = pkg.timestamp_; code.to_string = pkg.toString(); - const auto logContents = - "Logging an ErrorCodeMessage from the UR Controller Box: " + pkg.toString(); + const auto logContents = "Logging an ErrorCodeMessage from the UR Controller Box: " + pkg.toString(); switch (code.report_level) { @@ -150,7 +151,7 @@ class PrimaryConsumer : public AbstractPrimaryConsumer 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 @@ -163,7 +164,6 @@ class PrimaryConsumer : public AbstractPrimaryConsumer private: std::function error_code_message_callback_; - }; } // namespace primary_interface diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index b7d8e3f2a..477c82c73 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -536,7 +536,8 @@ class UrDriver bool checkCalibration(const std::string& checksum); /*! - * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be deleted. + * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be + * deleted. * * \returns list of error codes * diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index 77df29a7a..3eebbeaa9 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -13,14 +13,15 @@ PrimaryClient::PrimaryClient(const std::string& robot_ip, comm::INotifier& notif 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_)); + + pipeline_.reset( + new comm::Pipeline(*prod_, multi_consumer_.get(), "PrimaryClient Pipeline", notifier_)); } PrimaryClient::~PrimaryClient() diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 5e36cf1a6..6e62c3150 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -729,7 +729,7 @@ void UrDriver::setupReverseInterface(const uint32_t reverse_port) reverse_interface_.reset(new control::ReverseInterface(reverse_port, handle_program_state_, step_time)); } -void UrDriver::startPrimaryClientCommunication() +void UrDriver::startPrimaryClientCommunication() { primary_client_->start(); } From 6224be620e1889cdde7e15af700259241b815821 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 20 Jan 2025 15:15:40 +0100 Subject: [PATCH 12/22] Revert shorting the wait after clearing PSTOP Older versions of PolyScope seem to require that. --- tests/test_ur_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 92ae97ded..dbb67e6ae 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -268,7 +268,7 @@ TEST_F(UrDriverTest, read_error_code) ASSERT_EQ(error_codes.at(0).message_argument, 0); // Wait for after PSTOP before clearing it - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::this_thread::sleep_for(std::chrono::seconds(5)); EXPECT_TRUE(g_dashboard_client->commandCloseSafetyPopup()); EXPECT_TRUE(g_dashboard_client->commandUnlockProtectiveStop()); From 9c1d40dbe45fadc455c6473f23dfc1aed2f72c03 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Jan 2025 13:50:41 +0100 Subject: [PATCH 13/22] REVERT_ME: Add color to default log handler --- src/default_log_handler.cpp | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/src/default_log_handler.cpp b/src/default_log_handler.cpp index 6e0d6c26a..50f8c750d 100644 --- a/src/default_log_handler.cpp +++ b/src/default_log_handler.cpp @@ -30,6 +30,7 @@ #include "ur_client_library/default_log_handler.h" #include +#include namespace urcl { @@ -37,22 +38,31 @@ DefaultLogHandler::DefaultLogHandler() = default; void DefaultLogHandler::log(const char* file, int line, LogLevel loglevel, const char* log) { + time_t timestamp = time(NULL); + double time = + std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()) + .count() / + 1000.0; + + const char color_red[] = "\033[31m"; + const char color_orange[] = "\033[93m"; + const char color_none[] = "\033[39m"; switch (loglevel) { case LogLevel::INFO: - printf("%s%s %i: %s \n", "INFO ", file, line, log); + printf("%f: %s%s %i: %s \n", time, "INFO ", file, line, log); break; case LogLevel::DEBUG: printf("%s%s %i: %s \n", "DEBUG ", file, line, log); break; case LogLevel::WARN: - printf("%s%s %i: %s \n", "WARN ", file, line, log); + printf("%s%f: %s%s %i: %s%s\n", color_orange, time, "WARN ", file, line, log, color_none); break; case LogLevel::ERROR: - printf("%s%s %i: %s \n", "ERROR ", file, line, log); + printf("%s%f: %s%s %i: %s%s\n", color_red, time, "ERROR ", file, line, log, color_none); break; case LogLevel::FATAL: - printf("%s%s %i: %s \n", "FATAL ", file, line, log); + printf("%s%s%s %i: %s%s\n", "FATAL ", color_red, file, line, log, color_none); break; default: break; From f4cb10ef9b337c8abd46e17de94e0b020cc123cc Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Jan 2025 14:00:32 +0100 Subject: [PATCH 14/22] Sleep 6 seconds before unlocking PSTOP If URsim's time in the container is running a bit slower than the caller's time, we raise an error. --- tests/test_ur_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index dbb67e6ae..f85ab6853 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -268,7 +268,7 @@ TEST_F(UrDriverTest, read_error_code) ASSERT_EQ(error_codes.at(0).message_argument, 0); // Wait for after PSTOP before clearing it - std::this_thread::sleep_for(std::chrono::seconds(5)); + std::this_thread::sleep_for(std::chrono::seconds(6)); EXPECT_TRUE(g_dashboard_client->commandCloseSafetyPopup()); EXPECT_TRUE(g_dashboard_client->commandUnlockProtectiveStop()); From 92608aa49e6bc402911efe73c3aede262b69ad1d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Jan 2025 14:01:42 +0100 Subject: [PATCH 15/22] Revert "REVERT_ME: Add color to default log handler" This reverts commit 04b7ebfd4163827e61f10c4cd4da85f814b46379. --- src/default_log_handler.cpp | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/src/default_log_handler.cpp b/src/default_log_handler.cpp index 50f8c750d..6e0d6c26a 100644 --- a/src/default_log_handler.cpp +++ b/src/default_log_handler.cpp @@ -30,7 +30,6 @@ #include "ur_client_library/default_log_handler.h" #include -#include namespace urcl { @@ -38,31 +37,22 @@ DefaultLogHandler::DefaultLogHandler() = default; void DefaultLogHandler::log(const char* file, int line, LogLevel loglevel, const char* log) { - time_t timestamp = time(NULL); - double time = - std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()) - .count() / - 1000.0; - - const char color_red[] = "\033[31m"; - const char color_orange[] = "\033[93m"; - const char color_none[] = "\033[39m"; switch (loglevel) { case LogLevel::INFO: - printf("%f: %s%s %i: %s \n", time, "INFO ", file, line, log); + printf("%s%s %i: %s \n", "INFO ", file, line, log); break; case LogLevel::DEBUG: printf("%s%s %i: %s \n", "DEBUG ", file, line, log); break; case LogLevel::WARN: - printf("%s%f: %s%s %i: %s%s\n", color_orange, time, "WARN ", file, line, log, color_none); + printf("%s%s %i: %s \n", "WARN ", file, line, log); break; case LogLevel::ERROR: - printf("%s%f: %s%s %i: %s%s\n", color_red, time, "ERROR ", file, line, log, color_none); + printf("%s%s %i: %s \n", "ERROR ", file, line, log); break; case LogLevel::FATAL: - printf("%s%s%s %i: %s%s\n", "FATAL ", color_red, file, line, log, color_none); + printf("%s%s %i: %s \n", "FATAL ", file, line, log); break; default: break; From ff60c081969226efc9a1ed1be7af8db3769b3702 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 24 Jan 2025 14:29:59 +0100 Subject: [PATCH 16/22] Clang-tidy fixes --- include/ur_client_library/comm/pipeline.h | 8 ++++---- include/ur_client_library/primary/primary_consumer.h | 10 +++++----- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h index 2e7176554..f18f17410 100644 --- a/include/ur_client_library/comm/pipeline.h +++ b/include/ur_client_library/comm/pipeline.h @@ -112,7 +112,7 @@ class MultiConsumer : public IConsumer */ void addConsumer(std::shared_ptr> consumer) { - std::lock_guard lk(consumer_list); + std::lock_guard lk(consumer_list_); consumers_.push_back(consumer); } @@ -123,7 +123,7 @@ class MultiConsumer : public IConsumer */ void removeConsumer(std::shared_ptr> consumer) { - std::lock_guard lk(consumer_list); + std::lock_guard lk(consumer_list_); auto it = std::find(consumers_.begin(), consumers_.end(), consumer); if (it == consumers_.end()) { @@ -183,7 +183,7 @@ class MultiConsumer : public IConsumer */ bool consume(std::shared_ptr product) { - std::lock_guard lk(consumer_list); + std::lock_guard lk(consumer_list_); bool res = true; for (auto& con : consumers_) { @@ -194,7 +194,7 @@ class MultiConsumer : public IConsumer } private: - std::mutex consumer_list; + std::mutex consumer_list_; }; /*! diff --git a/include/ur_client_library/primary/primary_consumer.h b/include/ur_client_library/primary/primary_consumer.h index 10e6b285d..cdb84057a 100644 --- a/include/ur_client_library/primary/primary_consumer.h +++ b/include/ur_client_library/primary/primary_consumer.h @@ -119,7 +119,7 @@ class PrimaryConsumer : public AbstractPrimaryConsumer code.timestamp = pkg.timestamp_; code.to_string = pkg.toString(); - const auto logContents = "Logging an ErrorCodeMessage from the UR Controller Box: " + pkg.toString(); + const auto log_contents = "Logging an ErrorCodeMessage from the UR Controller Box: " + pkg.toString(); switch (code.report_level) { @@ -129,18 +129,18 @@ class PrimaryConsumer : public AbstractPrimaryConsumer case urcl::primary_interface::ReportLevel::DEVL_WARNING: case urcl::primary_interface::ReportLevel::DEVL_VIOLATION: case urcl::primary_interface::ReportLevel::DEVL_FAULT: - URCL_LOG_DEBUG(logContents.c_str()); + URCL_LOG_DEBUG(log_contents.c_str()); break; case urcl::primary_interface::ReportLevel::INFO: - URCL_LOG_INFO(logContents.c_str()); + URCL_LOG_INFO(log_contents.c_str()); break; case urcl::primary_interface::ReportLevel::WARNING: - URCL_LOG_WARN(logContents.c_str()); + URCL_LOG_WARN(log_contents.c_str()); break; default: // urcl::primary_interface::ReportLevel::VIOLATION: // urcl::primary_interface::ReportLevel::FAULT: - URCL_LOG_ERROR(logContents.c_str()); + URCL_LOG_ERROR(log_contents.c_str()); break; } From 1754484e656fafe08f72e0667958f423fb5e9004 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 30 Jan 2025 08:31:02 +0100 Subject: [PATCH 17/22] Update license header for error_code_message Since I've created the file while working at FZI, I left the copyright owner and the license as is. --- .../primary/robot_message/error_code_message.h | 2 +- src/primary/robot_message/error_code_message.cpp | 9 ++------- 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/include/ur_client_library/primary/robot_message/error_code_message.h b/include/ur_client_library/primary/robot_message/error_code_message.h index e672d927e..a4fc63bb3 100644 --- a/include/ur_client_library/primary/robot_message/error_code_message.h +++ b/include/ur_client_library/primary/robot_message/error_code_message.h @@ -1,7 +1,7 @@ // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- // -- BEGIN LICENSE BLOCK ---------------------------------------------- -// Copyright 2019 FZI Forschungszentrum Informatik +// 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. diff --git a/src/primary/robot_message/error_code_message.cpp b/src/primary/robot_message/error_code_message.cpp index 1dc558be8..a52a482fd 100644 --- a/src/primary/robot_message/error_code_message.cpp +++ b/src/primary/robot_message/error_code_message.cpp @@ -1,10 +1,6 @@ -// 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) +// 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. @@ -28,7 +24,6 @@ */ //---------------------------------------------------------------------- -#include "ur_client_library/log.h" #include "ur_client_library/primary/robot_message/error_code_message.h" #include "ur_client_library/primary/abstract_primary_consumer.h" From f98d86cb4d23df3fddbf1855f3464ef921c2d91c Mon Sep 17 00:00:00 2001 From: Jessica Chen Date: Wed, 5 Feb 2025 10:41:57 -0500 Subject: [PATCH 18/22] Added license headers --- .../primary/primary_client.h | 30 +++++++++++++++++++ src/primary/primary_client.cpp | 30 +++++++++++++++++++ 2 files changed, 60 insertions(+) diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index 46eadd59a..d7fd7d58c 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -1,3 +1,33 @@ +// -- 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 diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index 3eebbeaa9..38ad47277 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -1,3 +1,33 @@ +// -- 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 From b8d59f0300b35d29b8ba3240963777ced0365bbc Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 6 Feb 2025 13:53:28 +0100 Subject: [PATCH 19/22] Update test API --- tests/test_ur_driver.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index f85ab6853..ceefc1cd6 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -247,18 +247,17 @@ TEST_F(UrDriverTest, reset_rtde_client) TEST_F(UrDriverTest, read_error_code) { - g_consume_rtde_packages = true; - g_ur_driver->startPrimaryClientCommunication(); + 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_ur_driver->sendScript(cmd.str())); + EXPECT_TRUE(g_my_robot->ur_driver_->sendScript(cmd.str())); - auto error_codes = g_ur_driver->getErrorCodes(); + auto error_codes = g_my_robot->ur_driver_->getErrorCodes(); while (error_codes.size() == 0) { - error_codes = g_ur_driver->getErrorCodes(); + error_codes = g_my_robot->ur_driver_->getErrorCodes(); } ASSERT_EQ(error_codes.size(), 1); @@ -270,8 +269,8 @@ TEST_F(UrDriverTest, read_error_code) // Wait for after PSTOP before clearing it std::this_thread::sleep_for(std::chrono::seconds(6)); - EXPECT_TRUE(g_dashboard_client->commandCloseSafetyPopup()); - EXPECT_TRUE(g_dashboard_client->commandUnlockProtectiveStop()); + 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. @@ -296,4 +295,4 @@ int main(int argc, char* argv[]) } return RUN_ALL_TESTS(); -} +} \ No newline at end of file From e24270b2ee8c4030bb3b5bb80a5eddc4141cb5d0 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 6 Feb 2025 13:58:16 +0100 Subject: [PATCH 20/22] formatting --- include/ur_client_library/ur/ur_driver.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 477c82c73..6a34d38b9 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -661,7 +661,7 @@ class UrDriver * \brief Starts the primary client */ void startPrimaryClientCommunication(); - + void registerTrajectoryInterfaceDisconnectedCallback(std::function fun) { trajectory_interface_->registerDisconnectionCallback(fun); From d400328ccb3cc5311d1dedd68650d847f5ea5e55 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 7 Feb 2025 10:41:57 +0100 Subject: [PATCH 21/22] Change link to error code documentation --- tests/test_ur_driver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index ceefc1cd6..e1322c6d1 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -261,8 +261,8 @@ TEST_F(UrDriverTest, read_error_code) } ASSERT_EQ(error_codes.size(), 1); - // Error code description can be found here: - // https://myur.universal-robots.com/manuals/content/SW_5_16/Documentation%20Menu/Error%20Codes/Introduction/C209%20A%20protective%20stop%20was%20triggered%20%28for%20test%20purposes%20only%29 + // 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); From 1abca399ac738f1b4d6dfce887c985f50d1bbef7 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 7 Feb 2025 12:50:53 +0100 Subject: [PATCH 22/22] Added integration test for primary client --- tests/CMakeLists.txt | 16 +++++++ tests/test_primary_client.cpp | 82 +++++++++++++++++++++++++++++++++++ 2 files changed, 98 insertions(+) create mode 100644 tests/test_primary_client.cpp 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(); +}