From e0fe782fe64ba9c5890d1b3c351f2b0a5ad3081a Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Mon, 9 Mar 2026 14:06:12 +0000 Subject: [PATCH 1/3] Add ignore. --- soem_interface_examples/CATKIN_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 soem_interface_examples/CATKIN_IGNORE diff --git a/soem_interface_examples/CATKIN_IGNORE b/soem_interface_examples/CATKIN_IGNORE new file mode 100644 index 0000000..e69de29 From 7a63243709165cbd4499f87134531cabf7c556dd Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Wed, 11 Mar 2026 15:43:29 +0000 Subject: [PATCH 2/3] Add restart. --- .../soem_interface_rsl/EthercatBusBase.hpp | 9 ++++++ .../soem_interface_rsl/EthercatBusBase.cpp | 32 +++++++++++++++++++ 2 files changed, 41 insertions(+) diff --git a/soem_interface_rsl/include/soem_interface_rsl/EthercatBusBase.hpp b/soem_interface_rsl/include/soem_interface_rsl/EthercatBusBase.hpp index 166b996..0ce9e0a 100644 --- a/soem_interface_rsl/include/soem_interface_rsl/EthercatBusBase.hpp +++ b/soem_interface_rsl/include/soem_interface_rsl/EthercatBusBase.hpp @@ -139,6 +139,15 @@ class SOEM_RSL_EXPORT EthercatBusBase : private EthercatBusBaseTemplateAdapter { bool startup(std::atomic& abortFlag, bool sizeCheck, int maxDiscoverRetries = 10); + /*! + * Restart the bus communication: shutdown internally and startup again. + * Unlike shutdown(), this preserves the bus object for reuse. + * @param sizeCheck perform a check of the Rx and Tx Pdo sizes + * @param maxDiscoverRetries number of retries for slave discovery + * @return True if successful. + */ + bool restart(bool sizeCheck = true, int maxDiscoverRetries = 10); + /*! * Update step 1: Read all PDOs. */ diff --git a/soem_interface_rsl/src/soem_interface_rsl/EthercatBusBase.cpp b/soem_interface_rsl/src/soem_interface_rsl/EthercatBusBase.cpp index 7cc52fb..aaebb26 100644 --- a/soem_interface_rsl/src/soem_interface_rsl/EthercatBusBase.cpp +++ b/soem_interface_rsl/src/soem_interface_rsl/EthercatBusBase.cpp @@ -370,6 +370,33 @@ struct EthercatBusBaseTemplateAdapter::EthercatSlaveBaseImpl { return ETHERCAT_SM_STATE::NONE; } + bool restart(std::atomic& abortFlag, const bool sizeCheck, int maxDiscoverRetries) { + if (initlialized_) { + { + std::lock_guard guard(contextMutex_); + if (*ecatContext_.slavecount > 0) { + setStateLocked(EC_STATE_INIT); + waitForStateLocked(EC_STATE_INIT, 0, 2); + } + } + for (auto& slave : slaves_) { + slave->shutdown(); + } + } + { + std::lock_guard guard(contextMutex_); + if (ecatContext_.port != nullptr) { + MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Closing socket for restart ..."); + ecx_close(&ecatContext_); + soem_interface_rsl::threadSleep(0.2); + } + initlialized_ = false; + } + + MELO_INFO_STREAM("[soem_interface_rsl::" << name_ << "] Restarting bus ..."); + return startup(abortFlag, sizeCheck, maxDiscoverRetries); + } + bool busIsOk() const { return workingCounterTooLowCounter_ < maxWorkingCounterTooLow_; } bool doBusMonitoring(bool logErrorCounterForDiagnosis) { @@ -972,6 +999,11 @@ void EthercatBusBase::shutdown() { pImpl_.reset(nullptr); } +bool EthercatBusBase::restart(bool sizeCheck, int maxDiscoverRetries) { + std::atomic tmpAtomicForStart{false}; + return pImpl_->restart(tmpAtomicForStart, sizeCheck, maxDiscoverRetries); +} + void EthercatBusBase::setState(const uint16_t state, const uint16_t slave) { pImpl_->setState(state, slave); } From bd1181f414af11ec2ed5a9388cf0e247144e7f95 Mon Sep 17 00:00:00 2001 From: ZiDXie Date: Tue, 24 Mar 2026 09:06:50 +0000 Subject: [PATCH 3/3] Add reconnect. --- .../EthercatBusManagerBase.cpp | 72 ++++++++++++------- 1 file changed, 46 insertions(+), 26 deletions(-) diff --git a/soem_interface_rsl/src/soem_interface_rsl/EthercatBusManagerBase.cpp b/soem_interface_rsl/src/soem_interface_rsl/EthercatBusManagerBase.cpp index 58eeac7..fead83b 100644 --- a/soem_interface_rsl/src/soem_interface_rsl/EthercatBusManagerBase.cpp +++ b/soem_interface_rsl/src/soem_interface_rsl/EthercatBusManagerBase.cpp @@ -6,7 +6,8 @@ ** ** This file is part of the soem_interface_rsl. ** -** The soem_interface_rsl is free software: you can redistribute it and/or modify +** The soem_interface_rsl is free software: you can redistribute it and/or +* modify ** it under the terms of the GNU General Public License as published by ** the Free Software Foundation, either version 3 of the License, or ** (at your option) any later version. @@ -17,7 +18,8 @@ ** GNU General Public License for more details. ** ** You should have received a copy of the GNU General Public License -** along with the soem_interface_rsl. If not, see . +** along with the soem_interface_rsl. If not, see +* . */ // anydrive @@ -25,30 +27,36 @@ namespace soem_interface_rsl { -bool EthercatBusManagerBase::addEthercatBus(soem_interface_rsl::EthercatBusBase* bus) { +bool EthercatBusManagerBase::addEthercatBus( + soem_interface_rsl::EthercatBusBase *bus) { if (bus == nullptr) { - MELO_ERROR_STREAM("[RokubiminiEthercatBusManager::addEthercatBus] bus is nullptr") + MELO_ERROR_STREAM( + "[RokubiminiEthercatBusManager::addEthercatBus] bus is nullptr") return false; } std::lock_guard lock(busMutex_); - const auto& it = buses_.find(bus->getName()); + const auto &it = buses_.find(bus->getName()); if (it == buses_.end()) { - buses_.insert(std::make_pair(bus->getName(), std::unique_ptr(bus))); + buses_.insert(std::make_pair( + bus->getName(), + std::unique_ptr(bus))); return true; } else { return false; } } -bool EthercatBusManagerBase::addEthercatBus(std::unique_ptr bus) { +bool EthercatBusManagerBase::addEthercatBus( + std::unique_ptr bus) { if (bus == nullptr) { - MELO_ERROR_STREAM("[RokubiminiEthercatBusManager::addEthercatBus] bus is nullptr") + MELO_ERROR_STREAM( + "[RokubiminiEthercatBusManager::addEthercatBus] bus is nullptr") return false; } std::lock_guard lock(busMutex_); - const auto& it = buses_.find(bus->getName()); + const auto &it = buses_.find(bus->getName()); if (it == buses_.end()) { buses_.insert(std::make_pair(bus->getName(), std::move(bus))); return true; @@ -65,33 +73,38 @@ bool EthercatBusManagerBase::startupAllBuses() { void EthercatBusManagerBase::setBussesOperational() { std::lock_guard lock(busMutex_); - // Only set the state but do not wait for it, since some devices (e.g. junctions) might not be able to reach it. - for (auto& bus : buses_) { + // Only set the state but do not wait for it, since some devices (e.g. + // junctions) might not be able to reach it. + for (auto &bus : buses_) { bus.second->setState(ETHERCAT_SM_STATE::OPERATIONAL); } } void EthercatBusManagerBase::setBussesPreOperational() { std::lock_guard lock(busMutex_); - // Only set the state but do not wait for it, since some devices (e.g. junctions) might not be able to reach it. - for (auto& bus : buses_) { + // Only set the state but do not wait for it, since some devices (e.g. + // junctions) might not be able to reach it. + for (auto &bus : buses_) { bus.second->setState(ETHERCAT_SM_STATE::PRE_OP); } } void EthercatBusManagerBase::setBussesSafeOperational() { std::lock_guard lock(busMutex_); - // Only set the state but do not wait for it, since some devices (e.g. junctions) might not be able to reach it. - for (auto& bus : buses_) { + // Only set the state but do not wait for it, since some devices (e.g. + // junctions) might not be able to reach it. + for (auto &bus : buses_) { bus.second->setState(ETHERCAT_SM_STATE::SAFE_OP); } } -void EthercatBusManagerBase::waitForState(const uint16_t state, const uint16_t slave, const std::string busName, +void EthercatBusManagerBase::waitForState(const uint16_t state, + const uint16_t slave, + const std::string busName, const unsigned int maxRetries) { std::lock_guard lock(busMutex_); if (busName.empty()) { - for (auto& bus : buses_) { + for (auto &bus : buses_) { bus.second->waitForState(state, slave, maxRetries); } } else { @@ -101,10 +114,16 @@ void EthercatBusManagerBase::waitForState(const uint16_t state, const uint16_t s bool EthercatBusManagerBase::startupCommunication() { std::lock_guard lock(busMutex_); - for (auto& bus : buses_) { + for (auto &bus : buses_) { if (!bus.second->startup(true)) { MELO_ERROR_STREAM("Failed to startup bus '" << bus.first << "'."); - return false; + MELO_WARN_STREAM("Attempting to reconnect bus '" + << bus.first << "' after startup failure."); + if (!bus.second->restart(true, 10)) { + MELO_ERROR_STREAM("Reconnect failed for bus '" << bus.first << "'."); + return false; + } + MELO_INFO_STREAM("Reconnect successful for bus '" << bus.first << "'."); } } return true; @@ -112,26 +131,27 @@ bool EthercatBusManagerBase::startupCommunication() { void EthercatBusManagerBase::readAllBuses() { std::lock_guard lock(busMutex_); - for (auto& bus : buses_) { + for (auto &bus : buses_) { bus.second->updateRead(); } } void EthercatBusManagerBase::writeToAllBuses() { std::lock_guard lock(busMutex_); - for (auto& bus : buses_) { + for (auto &bus : buses_) { bus.second->updateWrite(); } } void EthercatBusManagerBase::shutdownAllBuses() { std::lock_guard lock(busMutex_); - for (auto& bus : buses_) { + for (auto &bus : buses_) { bus.second->shutdown(); } } -std::unique_ptr EthercatBusManagerBase::extractBusByName(const std::string& name) { +std::unique_ptr +EthercatBusManagerBase::extractBusByName(const std::string &name) { std::unique_ptr busOut = std::move(buses_.at(name)); buses_.erase(name); return busOut; @@ -140,7 +160,7 @@ std::unique_ptr EthercatBusManagerBase::extractBusByName(const EthercatBusManagerBase::BusMap EthercatBusManagerBase::extractBuses() { BusMap busMapOut; - for (auto& bus : buses_) { + for (auto &bus : buses_) { busMapOut.insert(std::make_pair(bus.first, std::move(bus.second))); } @@ -149,7 +169,7 @@ EthercatBusManagerBase::BusMap EthercatBusManagerBase::extractBuses() { } bool EthercatBusManagerBase::allBusesAreOk() { - for (const auto& bus : buses_) { + for (const auto &bus : buses_) { if (!bus.second->busIsOk()) { return false; } @@ -157,4 +177,4 @@ bool EthercatBusManagerBase::allBusesAreOk() { return true; } -} // namespace soem_interface_rsl +} // namespace soem_interface_rsl