From ee8944c331355c0cc0dd4ed52baf8324e587adb9 Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Fri, 15 Dec 2023 15:19:56 +0100 Subject: [PATCH 1/5] added -fPIC --- ethercat_sdk_master/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ethercat_sdk_master/CMakeLists.txt b/ethercat_sdk_master/CMakeLists.txt index ff3b0ad..e6dc3e0 100644 --- a/ethercat_sdk_master/CMakeLists.txt +++ b/ethercat_sdk_master/CMakeLists.txt @@ -47,12 +47,13 @@ target_include_directories(${PROJECT_NAME} $ $) +target_compile_options(${PROJECT_NAME} PUBLIC -fPIC) + ament_target_dependencies( ${PROJECT_NAME} ${PACKAGE_DEPENDENCIES}) ament_export_dependencies(${PACKAGE_DEPENDENCIES}) ament_export_targets(${PROJECT_NAME}Target) #Does Threads needs to be exportet? -ament_export_dependencies(${PACKAGE_DEPENDENCIES}) ############# ## Install ## From 4f6481af2d3d74342d1f3ffb2555f32dbb29377e Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Fri, 15 Dec 2023 15:28:08 +0100 Subject: [PATCH 2/5] Added EthercatMasterSingleton - allows to reuse the same ethercat master in different ros2control hardware interfaces --- ethercat_sdk_master/CMakeLists.txt | 1 + .../EthercatMasterConfiguration.hpp | 15 ++++++ .../EthercatMasterSingleton.hpp | 51 +++++++++++++++++++ .../EthercatMasterSingleton.cpp | 8 +++ 4 files changed, 75 insertions(+) create mode 100644 ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp create mode 100644 ethercat_sdk_master/src/ethercat_sdk_master/EthercatMasterSingleton.cpp diff --git a/ethercat_sdk_master/CMakeLists.txt b/ethercat_sdk_master/CMakeLists.txt index e6dc3e0..0774a58 100644 --- a/ethercat_sdk_master/CMakeLists.txt +++ b/ethercat_sdk_master/CMakeLists.txt @@ -40,6 +40,7 @@ find_package(Threads REQUIRED) add_library(${PROJECT_NAME} src/${PROJECT_NAME}/EthercatMaster.cpp src/${PROJECT_NAME}/EthercatDevice.cpp + src/${PROJECT_NAME}/EthercatMasterSingleton.cpp ) target_include_directories(${PROJECT_NAME} diff --git a/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterConfiguration.hpp b/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterConfiguration.hpp index a7675d8..7aaeedf 100644 --- a/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterConfiguration.hpp +++ b/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterConfiguration.hpp @@ -79,6 +79,21 @@ struct EthercatMasterConfiguration{ */ bool logErrorCounters{false}; + /*! + * Comparison operator + */ + bool operator==(const EthercatMasterConfiguration& o) const{ + return o.name == name && o.networkInterface == networkInterface && + o.timeStep == timeStep && o.pdoSizeCheck == pdoSizeCheck && + o.slaveDiscoverRetries == slaveDiscoverRetries && o.updateRateTooLowWarnThreshold == updateRateTooLowWarnThreshold && + o.rateCompensationCoefficient == rateCompensationCoefficient && + o.doBusDiagnosis == doBusDiagnosis && + o.logErrorCounters == logErrorCounters; + } + + bool operator!=(const EthercatMasterConfiguration& o) const{ + return !(o == *this); + } }; } // namespace ecat_master diff --git a/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp b/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp new file mode 100644 index 0000000..1ce1e71 --- /dev/null +++ b/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp @@ -0,0 +1,51 @@ +#pragma once + +#include +#include + +namespace ecat_master { + /** + * @brief Provides the only method how we can use the same ethercat bus in multiple ros2control hardware interfaces + * The idea is that we centrally manage the instances of the EthercatMasters and each hardware interface may attach its devices to it + */ + class EthercatMasterSingleton { + public: + static EthercatMasterSingleton& instance(); //Method has to be in cpp file in order to work properly + + std::shared_ptr get(const EthercatMasterConfiguration& config) { + std::lock_guard guard(lock_); + + if(ecat_masters_.find(config.networkInterface) == ecat_masters_.end()) + { + auto master = std::make_shared(); + master->loadEthercatMasterConfiguration(config); + } + + if(config != ecat_masters_[config.networkInterface]->getConfiguration()){ + //Print warning or abort if the configuration does not match! + } + + return ecat_masters_[config.networkInterface]; + } + + bool hasMaster(const EthercatMasterConfiguration& config){ + return ecat_masters_.find(config.networkInterface) != ecat_masters_.end(); + } + bool hasMaster(const std::string& networkInterface) { + return ecat_masters_.find(networkInterface) != ecat_masters_.end(); + } + + std::shared_ptr operator[] (const EthercatMasterConfiguration& config){ + return get(config); + } + private: + + EthercatMasterSingleton(){ + + } + + std::map> ecat_masters_; + static std::recursive_mutex lock_; + }; + +} \ No newline at end of file diff --git a/ethercat_sdk_master/src/ethercat_sdk_master/EthercatMasterSingleton.cpp b/ethercat_sdk_master/src/ethercat_sdk_master/EthercatMasterSingleton.cpp new file mode 100644 index 0000000..7f2a707 --- /dev/null +++ b/ethercat_sdk_master/src/ethercat_sdk_master/EthercatMasterSingleton.cpp @@ -0,0 +1,8 @@ +#include "ethercat_sdk_master/EthercatMasterSingleton.hpp" + +namespace ecat_master { + EthercatMasterSingleton& EthercatMasterSingleton::instance() { + static EthercatMasterSingleton instance_; + return instance_; + } +} \ No newline at end of file From a4bbeabe48b54f1877598072340c598ac0e9fe83 Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Fri, 15 Dec 2023 15:29:08 +0100 Subject: [PATCH 3/5] added warning if the ethercat master configurations do not match --- .../include/ethercat_sdk_master/EthercatMasterSingleton.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp b/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp index 1ce1e71..57bbcbd 100644 --- a/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp +++ b/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp @@ -23,6 +23,7 @@ namespace ecat_master { if(config != ecat_masters_[config.networkInterface]->getConfiguration()){ //Print warning or abort if the configuration does not match! + MELO_WARN_STREAM("Ethercat master configurations do not match for bus: " << config.networkInterface); } return ecat_masters_[config.networkInterface]; From 06ea86b4bf648d006de1d4d77616e23739fe4617 Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Sat, 16 Dec 2023 12:54:04 +0100 Subject: [PATCH 4/5] singleton will now spin each master asynchronously --- .../EthercatMasterSingleton.hpp | 26 +++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp b/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp index 57bbcbd..6832aa6 100644 --- a/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp +++ b/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp @@ -17,10 +17,14 @@ namespace ecat_master { if(ecat_masters_.find(config.networkInterface) == ecat_masters_.end()) { + MELO_INFO_STREAM("Setting up new EthercatMaster on interface: " << config.networkInterface << " and updating it"); auto master = std::make_shared(); master->loadEthercatMasterConfiguration(config); + + //Spin the master asynchronously + spin_threads_.emplace(config.networkInterface, std::make_unique(std::bind(&EthercatMasterSingleton::spin,this, std::placeholders::_1), master)); } - + if(config != ecat_masters_[config.networkInterface]->getConfiguration()){ //Print warning or abort if the configuration does not match! MELO_WARN_STREAM("Ethercat master configurations do not match for bus: " << config.networkInterface); @@ -44,9 +48,27 @@ namespace ecat_master { EthercatMasterSingleton(){ } + ~EthercatMasterSingleton() { + abort_ = true; + + for(const auto& [interface, thread]: spin_threads_){ + thread->join(); + } + for(const auto & [interfrace, master]: ecat_masters_){ + master->preShutdown(); + master->shutdown(); + } + } + void spin(std::shared_ptr master_){ + while(!abort_){ + master_->update(UpdateMode::StandaloneEnforceRate); + } + } std::map> ecat_masters_; - static std::recursive_mutex lock_; + std::map> spin_threads_; + std::recursive_mutex lock_; + std::atomic_bool abort_ = false; }; } \ No newline at end of file From 16ffef5007f5fe1e921ac43d9418f8491f0a913a Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Sat, 16 Dec 2023 12:58:49 +0100 Subject: [PATCH 5/5] forgot to set rt prio --- .../include/ethercat_sdk_master/EthercatMasterSingleton.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp b/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp index 6832aa6..a1d1ca0 100644 --- a/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp +++ b/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp @@ -60,6 +60,7 @@ namespace ecat_master { } } void spin(std::shared_ptr master_){ + master_->setRealtimePriority(); while(!abort_){ master_->update(UpdateMode::StandaloneEnforceRate); }