diff --git a/ethercat_sdk_master/CMakeLists.txt b/ethercat_sdk_master/CMakeLists.txt index ff3b0ad..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} @@ -47,12 +48,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 ## 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..a1d1ca0 --- /dev/null +++ b/ethercat_sdk_master/include/ethercat_sdk_master/EthercatMasterSingleton.hpp @@ -0,0 +1,75 @@ +#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()) + { + 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); + } + + 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(){ + + } + ~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_){ + master_->setRealtimePriority(); + while(!abort_){ + master_->update(UpdateMode::StandaloneEnforceRate); + } + } + + std::map> ecat_masters_; + std::map> spin_threads_; + std::recursive_mutex lock_; + std::atomic_bool abort_ = false; + }; + +} \ 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