From 2aa058af2ed0ee9a8f9db63a116f410947330bb7 Mon Sep 17 00:00:00 2001 From: anubhav-dogra Date: Fri, 13 Jun 2025 12:26:56 +0000 Subject: [PATCH 1/2] multiple masters compatibility added --- .../ethercat_driver/ethercat_driver.hpp | 2 +- ethercat_driver/src/ethercat_driver.cpp | 31 +++++++++++++------ 2 files changed, 23 insertions(+), 10 deletions(-) diff --git a/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp b/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp index 8862c0a0..c5e633e7 100644 --- a/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp +++ b/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp @@ -83,7 +83,7 @@ class EthercatDriver : public hardware_interface::SystemInterface "ethercat_interface", "ethercat_interface::EcSlave"}; int control_frequency_; - ethercat_interface::EcMaster master_; + std::unique_ptr master_; std::mutex ec_mutex_; bool activated_; }; diff --git a/ethercat_driver/src/ethercat_driver.cpp b/ethercat_driver/src/ethercat_driver.cpp index deea06b5..b1a73675 100644 --- a/ethercat_driver/src/ethercat_driver.cpp +++ b/ethercat_driver/src/ethercat_driver.cpp @@ -33,6 +33,16 @@ CallbackReturn EthercatDriver::on_init( const std::lock_guard lock(ec_mutex_); activated_ = false; + // Parse master_id from hardware parameters + int master_id = 0; // Default to 0 if not specified + if (info_.hardware_parameters.find("master_id") != info_.hardware_parameters.end()) { + master_id = std::stoi(info_.hardware_parameters["master_id"]); + } + RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Master ID: %d", master_id); + + // Initialize master with the specified ID + master_ = std::make_unique(master_id); + hw_joint_states_.resize(info_.joints.size()); for (uint j = 0; j < info_.joints.size(); j++) { hw_joint_states_[j].resize( @@ -282,10 +292,10 @@ CallbackReturn EthercatDriver::on_activate( // start EC and wait until state operative - master_.setCtrlFrequency(control_frequency_); + master_->setCtrlFrequency(control_frequency_); for (auto i = 0ul; i < ec_modules_.size(); i++) { - master_.addSlave( + master_->addSlave( std::stod(ec_module_parameters_[i]["alias"]), std::stod(ec_module_parameters_[i]["position"]), ec_modules_[i].get()); @@ -295,7 +305,7 @@ CallbackReturn EthercatDriver::on_activate( for (auto i = 0ul; i < ec_modules_.size(); i++) { for (auto & sdo : ec_modules_[i]->sdo_config) { uint32_t abort_code; - int ret = master_.configSlaveSdo( + int ret = master_->configSlaveSdo( std::stod(ec_module_parameters_[i]["position"]), sdo, &abort_code @@ -311,7 +321,7 @@ CallbackReturn EthercatDriver::on_activate( } } - if (!master_.activate()) { + if (!master_->activate()) { RCLCPP_ERROR(rclcpp::get_logger("EthercatDriver"), "Activate EcMaster failed"); return CallbackReturn::ERROR; } @@ -328,7 +338,7 @@ CallbackReturn EthercatDriver::on_activate( clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); // update EtherCAT bus - master_.update(); + master_->update(); RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!"); // check if operational @@ -340,7 +350,7 @@ CallbackReturn EthercatDriver::on_activate( running = false; } // calculate next shot. carry over nanoseconds into microseconds. - t.tv_nsec += master_.getInterval(); + t.tv_nsec += master_->getInterval(); while (t.tv_nsec >= 1000000000) { t.tv_nsec -= 1000000000; t.tv_sec++; @@ -363,8 +373,11 @@ CallbackReturn EthercatDriver::on_deactivate( RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Stopping ...please wait..."); + // Clean up all modules before stopping the master + ec_modules_.clear(); + // stop EC and disconnect - master_.stop(); + master_->stop(); RCLCPP_INFO( rclcpp::get_logger("EthercatDriver"), "System successfully stopped!"); @@ -379,7 +392,7 @@ hardware_interface::return_type EthercatDriver::read( // try to lock so we can avoid blocking the read/write loop on the lock. const std::unique_lock lock(ec_mutex_, std::try_to_lock); if (lock.owns_lock() && activated_) { - master_.readData(); + master_->readData(); } return hardware_interface::return_type::OK; } @@ -391,7 +404,7 @@ hardware_interface::return_type EthercatDriver::write( // try to lock so we can avoid blocking the read/write loop on the lock. const std::unique_lock lock(ec_mutex_, std::try_to_lock); if (lock.owns_lock() && activated_) { - master_.writeData(); + master_->writeData(); } return hardware_interface::return_type::OK; } From 7aaf9c9d4263f423f4b00a96fbaa96579b7d95f8 Mon Sep 17 00:00:00 2001 From: Muhammad Awais Rafique Date: Thu, 3 Jul 2025 01:20:03 +0000 Subject: [PATCH 2/2] Fixed Warnings dufing colcon build 1. Fixed warning realted to zero-iniliazation of first member by repacing with brace-iniliazation i.e. valid for C++ 11 onwards. 2. There is an empty for loop for no apperent reason. Nevetheless added a prefix [[maybe_unused]] to avoid warning. 3. Added constexpr in the declaration of MAX_SAFE_STACK used to define constant length array but C++ thinks its variable length array without MAX_SAFE_STACK being constexpr. 4. The function type2bytes of return type size_t had no return outside if confition, added a return 0 at the end of the function. 5. Not all fields were being initialized for a vector syncs_ of type ec_sync_info_t. Added all fields. 6. Avoided narrowing conversions from 64-bit size_t to 32-bit unsigned int, by explicitly casting. --- .../src/generic_ec_slave.cpp | 17 ++++++++++------- .../include/ethercat_interface/ec_master.hpp | 2 +- .../ethercat_interface/ec_sdo_manager.hpp | 1 + ethercat_interface/src/ec_master.cpp | 12 ++++++------ 4 files changed, 18 insertions(+), 14 deletions(-) diff --git a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp index 7514cf01..6303126e 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp +++ b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp @@ -30,6 +30,7 @@ size_t type2bytes(std::string type) } else if (type == "int64" || type == "uint64") { return 8; } + return 0; } namespace ethercat_generic_plugins @@ -67,20 +68,22 @@ void GenericEcSlave::setup_syncs() if (sm_configs_.size() == 0) { syncs_.push_back({0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE}); syncs_.push_back({1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE}); - syncs_.push_back({2, EC_DIR_OUTPUT, rpdos_.size(), rpdos_.data(), EC_WD_ENABLE}); - syncs_.push_back({3, EC_DIR_INPUT, tpdos_.size(), tpdos_.data(), EC_WD_DISABLE}); + syncs_.push_back({2, EC_DIR_OUTPUT, static_cast(rpdos_.size()), rpdos_.data(), EC_WD_ENABLE}); + syncs_.push_back({3, EC_DIR_INPUT, static_cast(tpdos_.size()), tpdos_.data(), EC_WD_DISABLE}); } else { for (auto & sm : sm_configs_) { if (sm.pdo_name == "null") { syncs_.push_back({sm.index, sm.type, 0, NULL, sm.watchdog}); } else if (sm.pdo_name == "rpdo") { - syncs_.push_back({sm.index, sm.type, rpdos_.size(), rpdos_.data(), sm.watchdog}); + syncs_.push_back({sm.index, sm.type, static_cast(rpdos_.size()), rpdos_.data(), sm.watchdog}); } else if (sm.pdo_name == "tpdo") { - syncs_.push_back({sm.index, sm.type, tpdos_.size(), tpdos_.data(), sm.watchdog}); + syncs_.push_back({sm.index, sm.type, static_cast(tpdos_.size()), tpdos_.data(), sm.watchdog}); } } } - syncs_.push_back({0xff}); + //syncs_.push_back({0xff}); + syncs_.push_back(ec_sync_info_t{0xff, EC_DIR_INPUT, 0, nullptr, EC_WD_DISABLE}); + } bool GenericEcSlave::setupSlave( @@ -173,7 +176,7 @@ bool GenericEcSlave::setup_from_config(YAML::Node slave_config) rpdos_.push_back( { slave_config["rpdo"][i]["index"].as(), - rpdo_channels_size, + static_cast(rpdo_channels_size), all_channels_.data() + channels_nbr } ); @@ -195,7 +198,7 @@ bool GenericEcSlave::setup_from_config(YAML::Node slave_config) tpdos_.push_back( { slave_config["tpdo"][i]["index"].as(), - tpdo_channels_size, + static_cast(tpdo_channels_size), all_channels_.data() + channels_nbr } ); diff --git a/ethercat_interface/include/ethercat_interface/ec_master.hpp b/ethercat_interface/include/ethercat_interface/ec_master.hpp index 26ae64f0..337821a8 100644 --- a/ethercat_interface/include/ethercat_interface/ec_master.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_master.hpp @@ -157,7 +157,7 @@ class EcMaster { EcSlave * slave = NULL; ec_slave_config_t * config = NULL; - ec_slave_config_state_t config_state = {0}; + ec_slave_config_state_t config_state{}; }; std::vector slave_info_; diff --git a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp index 9f141595..125aee6a 100644 --- a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp @@ -110,6 +110,7 @@ class SdoConfigEntry } else if (type == "int64" || type == "uint64") { return 8; } + return 0; } }; diff --git a/ethercat_interface/src/ec_master.cpp b/ethercat_interface/src/ec_master.cpp index db31e680..f155e2b7 100644 --- a/ethercat_interface/src/ec_master.cpp +++ b/ethercat_interface/src/ec_master.cpp @@ -40,7 +40,7 @@ EcMaster::DomainInfo::DomainInfo(ec_master_t * master) return; } - const ec_pdo_entry_reg_t empty = {0}; + const ec_pdo_entry_reg_t empty{}; domain_regs.push_back(empty); } @@ -66,7 +66,7 @@ EcMaster::EcMaster(const int master) EcMaster::~EcMaster() { - for (SlaveInfo & slave : slave_info_) { + for ([[maybe_unused]] SlaveInfo & slave : slave_info_) { // } for (auto & domain : domain_info_) { @@ -211,7 +211,7 @@ void EcMaster::registerPDOInDomain( } // set the last element to null - ec_pdo_entry_reg_t empty = {0}; + ec_pdo_entry_reg_t empty{}; domain_info->domain_regs.back() = empty; } @@ -441,9 +441,9 @@ void EcMaster::setThreadRealTime() /* Pre-fault our stack 8*1024 is the maximum stack size which is guaranteed safe to access without faulting */ - int MAX_SAFE_STACK = 8 * 1024; - unsigned char dummy[MAX_SAFE_STACK]; - memset(dummy, 0, MAX_SAFE_STACK); + constexpr int MAX_SAFE_STACK = 8 * 1024; + unsigned char dummy[MAX_SAFE_STACK]{}; + //memset(dummy, 0, MAX_SAFE_STACK); } void EcMaster::checkDomainState(uint32_t domain)