Bus.cpp

Go to the documentation of this file.

#include "Bus.h"
 
#include <algorithm>
#include <iomanip>
 
#include <ethercat.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/rapidxml/wrapper/DefaultRapidXmlReader.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
 
#include <RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h>
#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
 
#include <armarx/control/ethercat/DeviceInterface.h>
#include <armarx/control/ethercat/SlaveErrorRegistersDevice.h>
#include <armarx/control/ethercat/SlaveInterface.h>
#include <armarx/control/ethercat/Timing.h>
 
#include "ErrorReporting.h"
#include "bus_io/SlaveRegisterReadingScheduler.h"
 
namespace armarx::control::ethercat
{
    static constexpr std::uint8_t DEFAULT_ECAT_GROUP = 0;
 
    Bus&
    Bus::getBus()
    {
        static Bus _instance;
        return _instance;
    }
 
    std::string
    Bus::logErrorsToFile(std::filesystem::path path, unsigned int lastSeconds)
    {
        auto ret = Reporting::getErrorReporting().dumpToFile(path, lastSeconds);
        if (ret.has_value())
        {
            return ret.value().string();
        }
        else
        {
            return std::string();
        }
    }
 
    Bus::Bus() : busState(EtherCATState::init), currentGroup(DEFAULT_ECAT_GROUP), errorHandler(this)
    {
        setTag("ethercat::Bus");
    }
 
    bool
    Bus::setPDOMappings()
    {
        ARMARX_TRACE;
        bool retVal = true;
        int byteSum = 0;
 
        for (std::unique_ptr<SlaveInterface>& slave : slaves)
        {
            ARMARX_INFO << "Checking mapping for slave " << slave->getSlaveNumber()
                        << " name: " << slave->getSlaveIdentifier().getName();
            if (!slave->hasPDOMapping())
            {
                ARMARX_INFO << "No mapping needed for " << slave->getSlaveIdentifier().getName();
                continue;
            }
            byteSum +=
                ec_slave[slave->getSlaveNumber()].Obytes + ec_slave[slave->getSlaveNumber()].Ibytes;
            if (ec_slave[slave->getSlaveNumber()].outputs == nullptr ||
                ec_slave[slave->getSlaveNumber()].inputs == nullptr)
            {
                BUS_ERROR(iterationCount,
                          "There is a nullpointer in the mapping of slave %d",
                          slave->getSlaveNumber());
 
                ARMARX_IMPORTANT << "current slave" << slave->getSlaveNumber();
                ARMARX_IMPORTANT << "slaveCount: " << ec_slavecount;
                ARMARX_IMPORTANT << "iomap ptr: 0x" << std::hex << &ioMap << std::dec;
                ARMARX_IMPORTANT << "sn:" << slave->getSlaveNumber() << std::flush;
                ARMARX_IMPORTANT << "out: 0x" << std::hex
                                 << reinterpret_cast<long>(
                                        ec_slave[slave->getSlaveNumber()].outputs)
                                 << std::dec << std::flush;
                ARMARX_IMPORTANT << "in: 0x" << std::hex
                                 << reinterpret_cast<long>(ec_slave[slave->getSlaveNumber()].inputs)
                                 << std::dec << std::flush;
                ARMARX_IMPORTANT << "in diff:  "
                                 << static_cast<long>(ec_slave[slave->getSlaveNumber()].inputs -
                                                      ec_slave[0].outputs)
                                 << std::flush;
                ARMARX_IMPORTANT << "-------------------------------------------------------";
 
 
                auto ec_errorToString = [](ec_errort const& error) -> std::string
                {
                    std::stringstream result;
                    result << VAROUT(error.Etype) << "\n"
                           << VAROUT(error.Signal) << "\n"
                           << VAROUT(error.Slave) << "\n"
                           << VAROUT(error.Index) << "\n"
                           << VAROUT(error.SubIdx) << "\n"
                           << "\n";
                    return result.str();
                };
 
                ec_errort error_type;
                while (ec_poperror(&error_type))
                {
                    BUS_WARNING(
                        iterationCount, "SOEM error: %s", ec_errorToString(error_type).c_str());
                }
                retVal = false;
            }
            else
            {
                //setting pdo mappings slave inputs are master outputs and vice versa
                slave->setInputPDO(ec_slave[slave->getSlaveNumber()].outputs);
                slave->setOutputPDO(ec_slave[slave->getSlaveNumber()].inputs);
            }
        }
        ARMARX_VERBOSE << "PDO size: " << byteSum;
        return retVal;
    }
 
    bool
    Bus::createDevices()
    {
        auto printAdditionalInformation =
            [&](const std::string info,
                const std::vector<std::pair<std::string, std::string>> reasonAndSolutions)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
            std::stringstream ss;
            ss << "\n" << info << "\n\n";
            ss << "Possible reasons are:\n";
            for (const auto& [reason, solution] : reasonAndSolutions)
            {
                ss << "- " << reason << "\n";
                ss << "(" << solution << ")\n";
            }
            ARMARX_IMPORTANT << ss.str();
        };
 
 
        ARMARX_TRACE;
 
        // Error reporter for slave construction
        auto reporter = Reporting::getErrorReporting().getErrorReporter();
 
        ARMARX_VERBOSE << "Constructing slaves.";
        for (std::uint16_t currentSlaveIndex = 1; currentSlaveIndex <= ec_slavecount;
             ++currentSlaveIndex)
        {
            ARMARX_TRACE;
 
            auto createSlaveIdentifierFromESI =
                [&](std::uint16_t slaveIndex) -> std::optional<SlaveIdentifier>
            {
                auto esiHeaderBinary =
                    readESIBinaryBlob(slaveIndex, ESI_Header_StartAdress, ESI_Header_EndAdress);
                if (esiHeaderBinary.has_value())
                {
                    ESIParser p;
                    ESIHeader header = p.parseHeader(esiHeaderBinary.value());
 
                    SlaveIdentifier sid;
                    sid.slaveIndex = static_cast<std::int16_t>(slaveIndex);
                    sid.vendorID = header.vendorID;
                    sid.productCode = header.productCode;
                    sid.serialNumber = header.serialNumber;
                    sid.revisionNumber = header.revisionNumber;
                    return std::move(sid);
                }
                else
                {
                    return std::nullopt;
                }
            };
 
            auto maybeSid = createSlaveIdentifierFromESI(currentSlaveIndex);
            if (!maybeSid.has_value())
            {
                BUS_ERROR_LOCAL(reporter,
                                iterationCount,
                                "Failed to create slaveIdentifier from ESI for slave at index %d",
                                currentSlaveIndex);
                continue;
            }
            SlaveIdentifier slaveIdentifier = maybeSid.value();
 
            struct
            {
                bool found = false;
                std::string name = "";
            } foundSlaveInfo;
 
            try
            {
                BUS_INFO(iterationCount, "Number of slaveFactories is %d", slaveFactories.size());
                for (SlaveFactory& factory : slaveFactories)
                {
                    std::unique_ptr<SlaveInterface> slave = factory(slaveIdentifier);
 
                    if (slave)
                    {
                        if (not foundSlaveInfo.found)
                        {
                            foundSlaveInfo.name = slave->getSlaveIdentifier().getName();
                            slave->setLocalMinimumLoggingLevel(this->getEffectiveLoggingLevel());
                            slaves.push_back(std::move(slave));
                            foundSlaveInfo.found = true;
                        }
                        else
                        {
                            SLAVE_ERROR_LOCAL(reporter,
                                              slaveIdentifier,
                                              "Slave already constructed as an `%s`, "
                                              "but can also be constructed as an `%s`.",
                                              foundSlaveInfo.name.c_str(),
                                              slave->getSlaveIdentifier().getNameAsCStr());
                        }
                    }
                }
            }
            catch (LocalException& e)
            {
                SLAVE_ERROR_LOCAL(reporter,
                                  slaveIdentifier,
                                  "Error during slave creation; Reason: %s",
                                  e.getReason().c_str());
                continue;
            }
 
            if (not foundSlaveInfo.found)
            {
                SLAVE_ERROR_LOCAL(
                    reporter, slaveIdentifier, "Could not find a corresponding factory");
            }
        }
 
        // Not all slaves on the bus could be created correctly
        // In the sprit of "fail early, fail fast", we do not procede with the device creation
        if (reporter.hasErrors())
        {
            printAdditionalInformation(
                "One or more slave objects could not be created from the physical slaves found on "
                "the bus.",
                {{"No factory is known for a found slave",
                  "The corresponding factory must be registered in the specialization of the "
                  "RTUnit by calling bus.registerSlaveFactory(...) in its constructor"},
                 {"The found slave has different ESI informations (vendorID, productCode and "
                  "serialNumber) than expected by all registered slave factories",
                  "reflash the EEPROM with correct data or adapt the function "
                  "isSlaveIdentifierAccepted() in the corresponding slave class"},
                 {"Corrupted ESI information in the EEPROM of the slave",
                  "Reflash the EEPROM with correct data"}});
            return false;
        }
 
        BUS_INFO(iterationCount, "Number of slaves pushed into slavesView is %d", slaves.size());
        // constructing a view of observerptrs on the slaves
        std::transform(slaves.begin(),
                       slaves.end(),
                       std::back_inserter(slavesView),
                       [](auto& slave) { return std::experimental::make_observer(slave.get()); });
 
        // New error reporter for device construction
        reporter = Reporting::getErrorReporting().getErrorReporter();
 
        ARMARX_VERBOSE << "Constructing devices.";
        for (const auto& [_, deviceConfig] : hwconfig.deviceConfigs)
        {
            try
            {
                const std::string deviceClassName = deviceConfig->getType();
                const std::string deviceInstanceName = deviceConfig->getName();
                ARMARX_DEBUG << "Handling: " << deviceClassName << " name: " << deviceInstanceName
                             << ".";
 
                if (deviceFactories.find(deviceClassName) != deviceFactories.end())
                {
                    auto factory = deviceFactories.at(deviceClassName);
                    ARMARX_CHECK_NOT_NULL(robot)
                        << "Robot is null! You must set the robot via Bus::setRobot()!";
 
                    std::shared_ptr<DeviceInterface> device = factory(*deviceConfig, robot);
 
                    // Add devices
                    if (device)
                    {
                        devices.push_back(std::move(device));
                    }
                    else
                    {
                        BUS_WARNING_LOCAL(reporter,
                                          iterationCount,
                                          "Could not create device instance '%s' of class '%s'",
                                          deviceInstanceName.c_str(),
                                          deviceClassName.c_str());
                    }
                }
                else
                {
                    BUS_ERROR_LOCAL(reporter,
                                    iterationCount,
                                    "No device factory found for xml-node `%s` with name `%s` in "
                                    "hardware config.",
                                    deviceClassName.c_str(),
                                    deviceInstanceName.c_str());
                }
            }
            catch (LocalException& e)
            {
                BUS_ERROR_LOCAL(reporter,
                                iterationCount,
                                "Exception during device creation. Device class: %s; Reason: %s",
                                deviceConfig->getName().c_str(),
                                e.getReason().c_str());
                continue;
            }
            catch (...)
            {
                handleExceptions();
            }
        }
 
        ARMARX_VERBOSE << "Checking if hardware config does not contain unnecessary items.";
        std::vector<std::string> errorsList;
        if (not hwconfig.checkAllItemsRead(errorsList))
        {
            // TODO: Maybe do this on a per-device basis and use DEVICE_FATAL_AND_THROW
 
            for (auto& message : errorsList)
            {
                ARMARX_ERROR << message;
            }
            // TODO: Is this the right way to do it?
            throw std::runtime_error(
                "The hardware_config did not match the code for the devices, see log");
        }
 
        // There where errors during the creation of the devices.
        // Better return here instead of trying to continue with possible malformed list of devices.
        if (reporter.hasErrors())
        {
            printAdditionalInformation(
                "One or more devices could not created from the provided HardwareConfig.",
                {{"No factory known for device description",
                  "The corresponding device factory must be registered in the specialization of "
                  "the RTUnit by calling bus.registerDeviceFactory(...) in its constructor. It is "
                  "important that the first parameter fits exactly the name used in the "
                  "HardwareConfig"},
                 {"Exception during the construction of the device",
                  "Make sure that there are no programming errors in the constructor of the "
                  "device"}});
            return false;
        }
 
        // New error reporter for assigning and validating
        reporter = Reporting::getErrorReporting().getErrorReporter();
 
        ARMARX_VERBOSE << "Assigning slaves to devices.";
        for (std::unique_ptr<SlaveInterface>& slave : slaves)
        {
            ARMARX_TRACE;
 
            bool slaveAssigned = false;
            for (std::shared_ptr<DeviceInterface>& device : devices)
            {
                DeviceInterface::TryAssignResult result = device->tryAssign(*slave);
                switch (result)
                {
                    case DeviceInterface::TryAssignResult::assigned:
                        slaveAssigned = true;
                        slave->setParentDeviceName(device->getDeviceName());
                    case DeviceInterface::TryAssignResult::unknown:
                        // Okay, just continue.
                        break;
                    case DeviceInterface::TryAssignResult::alreadyAssigned:
                        BUS_FATAL_AND_THROW(iterationCount,
                                            "Tried to assign ambiguous slave with name `%s` (idx: "
                                            "%u) again to device (%s) with name `%s`.",
                                            slave->getSlaveIdentifier().getNameAsCStr(),
                                            slave->getSlaveIdentifier().slaveIndex,
                                            device->getClassName().c_str(),
                                            device->rtGetDeviceName());
                }
            }
 
            // Slaves without PDOs dont need a device to be assigned to.
            if (not slaveAssigned && slave->hasPDOMapping())
            {
                BUS_ERROR_LOCAL(reporter,
                                iterationCount,
                                "Could not find a device to assign following slave to:\n%s",
                                slave->getSlaveIdentifier().toString().c_str());
            }
        }
 
        // There where errors during the assigment of the slaves to the devices.
        // Most likely this is due to the fact, that there are slaves which do not have been assigned.
        // It does not make sense to continue since not all bus members will be controlled correctly.
        if (reporter.hasErrors())
        {
            printAdditionalInformation(
                "Not all found slaves could not be assigned to devices",
                {{"Unexpected slaves are part of the bus",
                  "Disconnect slaves that have not been described in the HardwareConfig from the "
                  "bus or add additional entries to the HardwareConfig to map the whole bus"},
                 {"SlaveIdentifier in the HardwareConfig do not match the actual slaves",
                  "Make sure, that the slave identifiers for all salves in the HardwareConfig are "
                  "correct. This includes vendorID, productCode and serialNumer."}});
            return false;
        }
 
        ARMARX_VERBOSE << "Sanity checking that devices have all needed slaves assigned.";
        for (std::shared_ptr<DeviceInterface>& device : devices)
        {
            DeviceInterface::AllAssignedResult result = device->onAllAssigned();
            switch (result)
            {
                case DeviceInterface::AllAssignedResult::ok:
                    // Okay, just continue.
                    break;
                case DeviceInterface::AllAssignedResult::slavesMissing:
                    BUS_FATAL_AND_THROW(
                        iterationCount,
                        "Device `%s` ("
                        "%s) reports not all slaves valid. Maybe a slave is missing on the bus",
                        device->getClassName().c_str(),
                        device->rtGetDeviceName());
            }
        }
 
        ARMARX_VERBOSE << "Adding SlaveErrorRegistersDevice to slaves.";
        for (auto& slave : slaves)
        {
            if (slave)
                slave->setErrorRegistersDevice(std::make_shared<SlaveErrorRegistersDevice>(
                    "SlaveIdx_" + std::to_string(slave->getSlaveIdentifier().slaveIndex) + "." +
                        slave->getSlaveIdentifier().getName(),
                    slave->getSlaveIdentifier().slaveIndex));
        }
 
 
        return true;
    }
 
    void
    Bus::setSocketFileDescriptor(int socketFileDescriptor)
    {
        this->socketFileDescriptor = socketFileDescriptor;
    }
 
    void
    Bus::setIfName(const std::string& ifname)
    {
        this->ifname = ifname;
    }
 
    bool
    Bus::rtUpdateBus(bool doErrorHandling, size_t iteration)
    {
        ARMARX_TRACE;
        if (iterationCount != iteration - 1)
        {
            BUS_WARNING(
                iteration,
                "Updating the bus has been skipped. The number of the last rt-thread iteration the "
                "bus has been updated was %d and the current iteration is %d.",
                iterationCount,
                iteration);
        }
        iterationCount = iteration;
        if (busState != EtherCATState::op)
        {
            BUS_WARNING(iterationCount,
                        "UpdateBus can not be executed because the bus is not in op-mode");
            return false;
        }
 
        // handle emergency stop release
        BUS_TIMING_START(bus_rtUpdateBus_isEmergencyStopActive);
        bool const emergencyStopActive = rtIsEmergencyStopActive();
        BUS_TIMING_CEND(bus_rtUpdateBus_isEmergencyStopActive, iterationCount, 0.3);
 
        // the elmo sometimes go from emergency stop state back into fault state for a moment - the time variable is the fix for that
        BUS_TIMING_START(bus_rtUpdateBus_recoverFromEmergencyStop)
        if ((not emergencyStopActive and emergencyStopWasActivated))
        {
            bool recovered = true;
            for (std::unique_ptr<SlaveInterface>& slave : slaves)
            {
                recovered &= slave->recoverFromEmergencyStop();
            }
            if (recovered)
            {
                BUS_WARNING(iterationCount, "Recovered from STO (Safe Torque Off)");
                emergencyStopWasActivated = false;
            }
        }
        else if (emergencyStopActive and not emergencyStopWasActivated)
        {
            BUS_WARNING(iterationCount, "STO (Safe Torque Off) active");
            emergencyStopWasActivated = emergencyStopActive;
        }
        BUS_TIMING_CEND(bus_rtUpdateBus_recoverFromEmergencyStop, iterationCount, 0.3)
        const auto delay = (armarx::rtNow() - busUpdateLastUpdateTime);
        if (delay.toMilliSecondsDouble() > 40)
        {
            BUS_WARNING(iterationCount,
                        "Update bus was not called for a long time: %ld ms",
                        delay.toMilliSeconds());
        }
 
        //send and receive process data
        BUS_TIMING_START(bus_rtUpdateBus_updatePDO)
        rtUpdatePDO();
        busUpdateLastUpdateTime = armarx::rtNow();
        BUS_TIMING_CEND(bus_rtUpdateBus_updatePDO, iterationCount, 0.6)
 
        // Execute every slave.
        BUS_TIMING_START(bus_rtUpdateBus_executeSlaves)
        for (std::unique_ptr<SlaveInterface>& slave : slaves)
        {
            slave->execute();
        }
        BUS_TIMING_CEND(bus_rtUpdateBus_executeSlaves, iterationCount, 0.3)
 
        //error handling
        if (doErrorHandling)
        {
            if (!emergencyStopActive)
            {
                BUS_TIMING_START(bus_rtUpdateBus_slaveErrorHandling)
                errorHandler.rtHandleSlaveErrors();
                BUS_TIMING_CEND(bus_rtUpdateBus_slaveErrorHandling, iterationCount, 0.3)
            }
            BUS_TIMING_START(bus_rtUpdateBus_busErrorHandling)
            errorHandler.rtHandleBusErrors();
            BUS_TIMING_CEND(bus_rtUpdateBus_busErrorHandling, iterationCount, 0.3)
        }
 
        // Update error registers
        BUS_TIMING_START(bus_rtUpdateBus_updateErrorRegisters)
        if (!errorHandler.hasError() && errorRegisterReadingScheduler)
        {
            if (errorRegisterReadingScheduler->allRegistersUpdated())
            {
                auto data_ref = errorRegisterReadingScheduler->getRegisterData();
                for (const auto& data : data_ref)
                {
                    auto index = data.slaveIndex;
                    this->getSlaveAtIndex(index)->getErrorRegistersDevice()->updateValues(&data);
                }
            }
            errorRegisterReadingScheduler->startReadingNextRegisters();
        }
        BUS_TIMING_CEND(bus_rtUpdateBus_updateErrorRegisters, iterationCount, 0.3)
 
        // Update functional state
        BUS_TIMING_START(bus_rtUpdateBus_updateFunctionalState)
        if (errorHandler.isReinitializationActive())
        {
            functionalState = BusFunctionalState::Reinitializing;
        }
        else if (emergencyStopActive)
        {
            functionalState = BusFunctionalState::EmergencyStop;
        }
        else
        {
            functionalState = BusFunctionalState::Running;
        }
        BUS_TIMING_CEND(bus_rtUpdateBus_updateFunctionalState, iterationCount, 0.3)
 
        return true;
    }
 
    void
    Bus::shutdown()
    {
        if (busState == EtherCATState::init)
        {
            BUS_WARNING(iterationCount, "Bus is already shutdown");
        }
        else
        {
            ARMARX_IMPORTANT << "Reading EtherCAT error registers from all slaves.";
            readAllErrorCounters();
 
            ARMARX_INFO << "Bus is shutting down";
 
            //shutdown the slaves if the bus hasn't died
            if (busState == EtherCATState::op)
            {
                bool found;
                do
                {
                    found = false;
                    for (std::unique_ptr<SlaveInterface>& slave : slaves)
                    {
                        ARMARX_INFO << deactivateSpam(1, std::to_string(slave->getSlaveNumber()))
                                    << "shutting down slave "
                                    << slave->getSlaveIdentifier().getName() << " ("
                                    << slave->getSlaveNumber() << "/" << slaves.size() << ")";
                        found |= !slave->shutdown();
                    }
                    rtUpdatePDO();
                } while (found);
            }
 
            // shutting down  bus
            // requesting init for all slaves
            if (!switchBusToInit())
            {
                ARMARX_DEBUG << "Could not switch all slaves to Init.";
            }
        }
 
        // closing bus
        ec_close();
 
        socketInitialized = false;
 
        ARMARX_DEBUG << "Bus shutdown";
 
        functionalState = BusFunctionalState::Shutdown;
    }
 
    PDOValidity
    Bus::getPDOValidity() const
    {
        return pdoValidity;
    }
 
    bool
    Bus::switchBusToInit()
    {
        static const std::map<EtherCATState, std::function<bool(Bus*)>> transitions = {
            {EtherCATState::preOp, &Bus::_preOpToInit},
            {EtherCATState::safeOp, &Bus::_safeOpToInit},
            {EtherCATState::op, &Bus::_opToInit},
            {EtherCATState::init,
             [](Bus* b)
             {
                 BUS_WARNING(b->iterationCount, "Bus is already in init");
                 return true;
             }}};
 
        ARMARX_VERBOSE << "Switching bus to init";
 
        return transitions.at(busState)(this);
    }
 
    bool
    Bus::switchBusToPreOp()
    {
        static const std::map<EtherCATState, std::function<bool(Bus*)>> transitions = {
            {EtherCATState::init, &Bus::_initToPreOp},
            {EtherCATState::safeOp, &Bus::_safeOpToPreOp},
            {EtherCATState::op, &Bus::_opToPreOp},
            {EtherCATState::preOp,
             [](Bus* b)
             {
                 BUS_WARNING(b->iterationCount, "Bus is already in preOp");
                 return true;
             }},
        };
 
        ARMARX_VERBOSE << "Switching bus to preOp";
 
        return transitions.at(busState)(this);
    }
 
    bool
    Bus::switchBusToSafeOp()
    {
        static const std::map<EtherCATState, std::function<bool(Bus*)>> transitions = {
            {EtherCATState::init, &Bus::_initToSafeOp},
            {EtherCATState::preOp, &Bus::_preOpToSafeOp},
            {EtherCATState::op, &Bus::_opToSafeOp},
            {EtherCATState::safeOp,
             [](Bus* b)
             {
                 BUS_WARNING(b->iterationCount, "Bus is already in safeOp");
                 return true;
             }},
        };
 
        ARMARX_VERBOSE << "Switching bus to safeOp";
 
        return transitions.at(busState)(this);
    }
 
    bool
    Bus::switchBusToOp()
    {
        static const std::map<EtherCATState, std::function<bool(Bus*)>> transitions = {
            {EtherCATState::init, &Bus::_initToOp},
            {EtherCATState::preOp, &Bus::_preOpToOp},
            {EtherCATState::safeOp, &Bus::_safeOpToOp},
            {EtherCATState::op,
             [](Bus* b)
             {
                 BUS_WARNING(b->iterationCount, "Bus is already in op");
                 return true;
             }}};
 
        return transitions.at(busState)(this);
    }
 
    bool
    Bus::_initToPreOp()
    {
        ARMARX_VERBOSE << "Start switching bus from 'Init' to 'PreOp'";
        ARMARX_ON_SCOPE_EXIT
        {
            ARMARX_VERBOSE << "Finish switching bus from 'Init' to 'PreOp'";
        };
 
        ARMARX_CHECK_EQUAL(busState, EtherCATState::init);
 
        if (!socketInitialized && !initSocket())
        {
            BUS_ERROR(iterationCount, "Could not init socket. Abort.");
            return false;
        }
 
        if (ec_config_init(FALSE) <= 0)
        {
            BUS_ERROR(iterationCount, "No slaves found on bus");
            return false;
        }
 
        bool ret = changeBusState(EtherCATState::preOp);
        if (ret)
        {
            // SDOs can now be read and written (flag is used by SDOHandler)
            sdoAccessAvailable = true;
            ARMARX_INFO << ec_slavecount << " slaves found and set from Init to PreOp";
        }
 
        return ret;
    }
 
    bool
    Bus::_initToSafeOp()
    {
        ARMARX_CHECK_EQUAL(busState, EtherCATState::init);
 
        return _initToPreOp() and _preOpToSafeOp();
    }
 
    bool
    Bus::_initToOp()
    {
        ARMARX_CHECK_EQUAL(busState, EtherCATState::init);
 
        return _initToPreOp() and _preOpToSafeOp() and _safeOpToOp();
    }
 
    bool
    Bus::_preOpToInit()
    {
        ARMARX_VERBOSE << "Start switching bus from 'PreOp' to 'Init'";
        ARMARX_ON_SCOPE_EXIT
        {
            ARMARX_VERBOSE << "Finish switching bus from 'PreOp' to 'Init'";
        };
 
        ARMARX_CHECK_EQUAL(busState, EtherCATState::preOp);
 
        return changeBusState(EtherCATState::init);
    }
 
    bool
    Bus::_preOpToSafeOp()
    {
        ARMARX_VERBOSE << "Start switching bus from 'PreOp' to 'SafeOp'";
        ARMARX_ON_SCOPE_EXIT
        {
            ARMARX_VERBOSE << "Finish switching bus from 'PreOp' to 'SafeOp'";
        };
 
        ARMARX_CHECK_EQUAL(busState, EtherCATState::preOp);
 
        //prepare Devices to be read to switch to Safe-Op
        if (slaves.size() > 0)
        {
            if (!validateBus())
            {
                shutdown();
                BUS_FATAL_AND_THROW(
                    iterationCount,
                    "Bus could not be validated. This indicates a severe hardware-error, "
                    "since the amount of slaves on the bus changed during the initialization "
                    "of the bus which can only happen if a PCB is behaving incorrectly or "
                    "(most likely) a cable is broken. Shutdown bus!");
                return false;
            }
        }
        else
        {
            // Create devices and populate members.
            if (!this->createDevices())
            {
                BUS_ERROR(iterationCount, "Error during slave and device creation");
                return false;
            }
 
            if (slaves.size() < 1)
            {
                BUS_ERROR(iterationCount, "There are no usable devices on the bus!");
                return false;
            }
            ARMARX_INFO << "Devices were created";
        }
 
        for (std::unique_ptr<SlaveInterface>& slave : slaves)
        {
            slave->doMappings();
        }
 
        ARMARX_TRACE;
 
        for (std::unique_ptr<SlaveInterface>& slave : slaves)
        {
            slave->prepareForSafeOp();
        }
 
        ARMARX_TRACE;
 
        for (std::unique_ptr<SlaveInterface>& slave : slaves)
        {
            slave->finishPreparingForSafeOp();
        }
 
        ARMARX_TRACE;
 
        int actualMappedSize = ec_config_map(ioMap.data());
 
        //calculating Workcounter after mapping to have an error indication later
        int expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
        ARMARX_VERBOSE << "Calculated workcounter: " << expectedWKC << std::endl;
 
        errorHandler.init(expectedWKC);
 
        ///give the devices their mapping
        if (!setPDOMappings())
        {
            BUS_ERROR(
                iterationCount,
                "Couldn't map the PDO, maybe the the pc is under to much load. Check if there are "
                "other performance hungry programs running.\nOr just try to start again");
            return false;
        }
 
        bool ret = changeBusState(EtherCATState::safeOp);
 
        if (ret)
        {
            ARMARX_INFO << "IOmapping done, size: " << actualMappedSize
                        << " - all Slaves are in SAFE-OP now\n";
        }
 
        // Run further initialization code for devices
        for (const auto& device : devices)
        {
            device->postSwitchToSafeOp();
        }
 
        ARMARX_TRACE;
 
        // Slaves inputs (EtherCAT naming convention) are transfered, outputs NOT.
        // This means, the slave objects can receive updated data but cannot yet send
        // data to the slaves via PDO
        rtUpdatePDO();
 
        pdoValidity = PDOValidity::OnlyInputs;
 
        return ret;
    }
 
    bool
    Bus::_preOpToOp()
    {
        return _preOpToSafeOp() and _safeOpToOp();
    }
 
    bool
    Bus::_safeOpToInit()
    {
        ARMARX_VERBOSE << "Start switching bus from 'SafeOp' to 'Init'";
        ARMARX_ON_SCOPE_EXIT
        {
            ARMARX_VERBOSE << "Finish switching bus from 'SafeOp' to 'Init'";
        };
 
        ARMARX_CHECK_EQUAL(busState, EtherCATState::safeOp);
 
        return changeBusState(EtherCATState::init);
    }
 
    bool
    Bus::_safeOpToPreOp()
    {
        ARMARX_VERBOSE << "Start switching bus from 'SafeOp' to 'PreOp'";
        ARMARX_ON_SCOPE_EXIT
        {
            ARMARX_VERBOSE << "Finish switching bus from 'SafeOp' to 'PreOp'";
        };
 
        ARMARX_CHECK_EQUAL(busState, EtherCATState::safeOp);
 
        return changeBusState(EtherCATState::preOp);
    }
 
    bool
    Bus::_safeOpToOp()
    {
        ARMARX_VERBOSE << "Start switching bus from 'SafeOp' to 'Op'";
        ARMARX_ON_SCOPE_EXIT
        {
            ARMARX_VERBOSE << "Finish switching bus from 'SafeOp' to 'Op'";
        };
 
        ARMARX_CHECK_EQUAL(busState, EtherCATState::safeOp);
 
        // Give the slaves some time to prepare.
        for (std::unique_ptr<SlaveInterface>& slave : slaves)
        {
            slave->prepareForOp();
        }
 
        for (std::unique_ptr<SlaveInterface>& slave : slaves)
        {
            slave->finishPreparingForOp();
        }
 
        // Send one valid process data to make outputs in slaves happy
        rtUpdatePDO();
 
        if (changeBusState(EtherCATState::op))
        {
            ARMARX_INFO << "All slaves in op!";
        }
        else
        {
            return false;
        }
 
        pdoValidity = PDOValidity::Both;
        pdoAccessAvailable = true;
 
        ARMARX_TRACE;
 
        // Some devices might have additional initialization to do after all slaves
        // are in op.
        for (std::shared_ptr<DeviceInterface>& device : devices)
        {
            device->postSwitchToOp();
        }
 
        ARMARX_TRACE;
 
 
        //preparing devices to run
        size_t slaveReadyCounter = 0;
        while (slaveReadyCounter != slaves.size())
        {
            rtUpdatePDO();
            slaveReadyCounter = 0;
            std::string missingSlaves;
            for (std::unique_ptr<SlaveInterface>& slave : slaves)
            {
                if (slave->prepareForRun())
                {
                    slaveReadyCounter++;
                }
                else
                {
                    missingSlaves += slave->getSlaveIdentifier().getName() +
                                     " (idx: " + std::to_string(slave->getSlaveNumber()) + "), ";
                }
            }
            ARMARX_INFO << deactivateSpam(1) << "Waiting for "
                        << (slaves.size() - slaveReadyCounter) << "/" << slaves.size()
                        << " slaves to get ready: " << missingSlaves;
        }
 
        std::stringstream slaveInfo;
        for (std::unique_ptr<SlaveInterface>& slave : slaves)
        {
            slaveInfo << "#" << slave->getSlaveNumber() << ": "
                      << slave->getSlaveIdentifier().getName() << "\n";
        }
        ARMARX_IMPORTANT << "Used slaves: \n" << slaveInfo.str();
 
        busUpdateLastUpdateTime = armarx::rtNow();
 
        functionalState = BusFunctionalState::Running;
 
        return true;
    }
 
    bool
    Bus::_opToInit()
    {
        ARMARX_VERBOSE << "Start switching bus from 'Op' to 'Init'";
        ARMARX_ON_SCOPE_EXIT
        {
            ARMARX_VERBOSE << "Finish switching bus from 'Op' to 'Init'";
        };
 
        ARMARX_CHECK_EQUAL(busState, EtherCATState::op);
 
        return changeBusState(EtherCATState::init);
    }
 
    bool
    Bus::_opToPreOp()
    {
        ARMARX_VERBOSE << "Start switching bus from 'Op' to 'PreOp'";
        ARMARX_ON_SCOPE_EXIT
        {
            ARMARX_VERBOSE << "Finish switching bus from 'Op' to 'PreOp'";
        };
 
        ARMARX_CHECK_EQUAL(busState, EtherCATState::op);
 
        return changeBusState(EtherCATState::preOp);
    }
 
    bool
    Bus::_opToSafeOp()
    {
        ARMARX_VERBOSE << "Start switching bus from 'Op' to 'SafeOp'";
        ARMARX_ON_SCOPE_EXIT
        {
            ARMARX_VERBOSE << "Finish switching bus from 'Op' to 'SafeOp'";
        };
 
        ARMARX_CHECK_EQUAL(busState, EtherCATState::op);
 
 
        return changeBusState(EtherCATState::safeOp);
    }
 
    bool
    Bus::changeBusState(EtherCATState state)
    {
        ARMARX_TRACE;
        ARMARX_ON_SCOPE_EXIT
        {
            busState = readStates();
        };
 
        if (state <= EtherCATState::preOp)
        {
            pdoValidity = PDOValidity::None;
        }
 
        if (state != EtherCATState::op)
        {
            pdoAccessAvailable = false;
        }
 
        EtherCATState retState = readStates();
        if (retState == state)
        {
            ARMARX_VERBOSE << "All slaves are already in requested state " << state;
            return true;
        }
        else
        {
            IceUtil::Time const start = armarx::rtNow();
            EtherCATState const retState = changeStateOfBus(state, true);
            IceUtil::Time const end = armarx::rtNow();
            IceUtil::Time const elapsed = (end - start);
 
            if (retState != state)
            {
                ARMARX_INFO << "One or more slaves appear to not have reached the desired state. "
                            << "If the desired state is lower than the actual one this is fine.";
            }
 
            if (retState < state)
            {
                readStates();
                for (std::uint16_t i = 1; i < ec_slavecount + 1; i++)
                {
                    EtherCATState const actualState = ec_slave[i].state;
                    if (actualState != state)
                    {
                        SLAVE_WARNING(
                            getSlaveAtIndex(i)->getSlaveIdentifier(),
                            "Slave did not reach requested state %s after %ld µs (start = "
                            "%ld µs since epoch, end = %ld µs since epoch). Actual state: %s AL: "
                            "%s",
                            state.c_str(),
                            elapsed.toMicroSeconds(),
                            start.toMicroSeconds(),
                            end.toMicroSeconds(),
                            actualState.c_str(),
                            ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
                    }
                }
 
                return false;
            }
        }
 
        return true;
    }
 
    bool
    Bus::validateBus() const
    {
        if (slaves.empty())
        {
            BUS_WARNING(iterationCount, "No slaves have been created. Cannot validate the bus.");
            return false;
        }
 
        int w = 0x0000;
        int slaveCount = ec_BRD(0x0000, ECT_REG_TYPE, sizeof(w), &w, EC_TIMEOUTSAFE);
        if (slaveCount == EC_NOFRAME)
        {
            BUS_ERROR(iterationCount, "EC_NOFRAME when trying to get slave count.");
            return false;
        }
 
        if (static_cast<std::uint16_t>(slaveCount) != slaves.size())
        {
            BUS_ERROR(iterationCount,
                      "Could not validate bus! Amount of found slaves is not equal to the previous "
                      "created slaves. (Found: %u, Expected: %u)",
                      slaveCount,
                      slaves.size());
            return false;
        }
 
        // If the amount of found slaves is the expected amount, we assume that the bus is fine
        // and every slaves is still available.
        // It is extremly unlikely, that the physical position of two or more slaves
        // on the bus have changed.
        return true;
    }
 
    bool
    Bus::initSocket()
    {
        if (socketInitialized)
        {
            BUS_ERROR(iterationCount, "Socket is already initialized.");
            return false;
        }
 
        if (socketFileDescriptor == -1)
        {
            BUS_WARNING(iterationCount, "socketFileDescriptor is -1 - did you forget to set it?");
            return false;
        }
 
        if (!ifname.empty())
        {
            ARMARX_IMPORTANT << "ec_init(" << ifname << ")";
            if (!ec_init(ifname.c_str()))
            {
                BUS_ERROR(iterationCount,
                          "Could not init EtherCAT on %s\nExecute as root\n",
                          ifname.c_str());
                return false;
            }
        }
        else if (socketFileDescriptor != -1)
        {
            ARMARX_INFO << "Using socketFileDescriptor " << socketFileDescriptor
                        << " to open raw socket";
            //initialise SOEM, open socket
            if (!ec_init_wsock(socketFileDescriptor))
            {
                BUS_WARNING(iterationCount,
                            "No socket connection on %u\nExecute as root\n",
                            socketFileDescriptor);
                return false;
            }
        }
        else
        {
            BUS_WARNING(iterationCount, "Either socketFileDescriptor or ifname need to be set");
            return false;
        }
 
        //We succeed
        ARMARX_INFO << "Started SOEM with socketFileDescriptor: " << socketFileDescriptor;
 
        socketInitialized = true;
        return true;
    }
 
    void
    Bus::readAllErrorCounters()
    {
        std::vector<RegisterDataList> registerData;
        for (const auto& slave : slaves)
        {
            registerData.push_back(
                RegisterDataList{static_cast<std::uint16_t>(slave->getSlaveIdentifier().slaveIndex),
                                 {
                                     {datatypes::RegisterEnum::LOST_LINK_COUNTER_PORT_0,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::LOST_LINK_COUNTER_PORT_1,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::LOST_LINK_COUNTER_PORT_2,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::LOST_LINK_COUNTER_PORT_3,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::FRAME_ERROR_COUNTER_PORT_0,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::FRAME_ERROR_COUNTER_PORT_1,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::FRAME_ERROR_COUNTER_PORT_2,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::FRAME_ERROR_COUNTER_PORT_3,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::PHYSICAL_ERROR_COUNTER_PORT_0,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::PHYSICAL_ERROR_COUNTER_PORT_1,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::PHYSICAL_ERROR_COUNTER_PORT_2,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::PHYSICAL_ERROR_COUNTER_PORT_3,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::PREVIOUS_ERROR_COUNTER_PORT_0,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::PREVIOUS_ERROR_COUNTER_PORT_1,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::PREVIOUS_ERROR_COUNTER_PORT_2,
                                      datatypes::RegisterEnumTypeContainer()},
                                     {datatypes::RegisterEnum::PREVIOUS_ERROR_COUNTER_PORT_3,
                                      datatypes::RegisterEnumTypeContainer()},
                                 }});
        }
 
 
        EtherCATFrameList* frameList =
            SlaveRegisterReadingScheduler::createEtherCATFrameListFromRegisterDataList(
                &registerData);
 
        int leftRetries = 3;
        bool readRegistersSuccessful = false;
 
        do
        {
            readRegistersSuccessful = readRegisters(frameList);
 
            if (not readRegistersSuccessful)
            {
                leftRetries -= 1;
 
                if (leftRetries == 0)
                {
                    ARMARX_ERROR << "Repeatedly failed reading error registers.  Giving up.";
                    return;
                }
                else
                {
                    ARMARX_ERROR << "Failed reading error registers.  Left retries: "
                                 << leftRetries;
                }
            }
 
        } while (not readRegistersSuccessful);
 
        SlaveRegisterReadingScheduler::updateRegisterDataFromEtherCATFrameList(frameList,
                                                                               &registerData);
 
        struct ErrorCounters
        {
            std::uint8_t invalidFrame = 0;
            std::uint8_t rxError = 0; // Or physical error count.
            std::uint8_t forwardedRxError = 0; // Error counter of a predecessor.
            std::uint8_t linkLost = 0;
        };
 
        std::vector<ErrorCounters> counts(4);
 
        for (const auto& d : registerData)
        {
            auto copyCounts = [&](datatypes::RegisterEnum type) -> std::uint8_t
            { return std::get<datatypes::EtherCATDataType::UNSIGNED8>(d.registerData.at(type)); };
 
            using RE = datatypes::RegisterEnum;
 
            counts[0].invalidFrame = copyCounts(RE::FRAME_ERROR_COUNTER_PORT_0);
            counts[0].rxError = copyCounts(RE::PHYSICAL_ERROR_COUNTER_PORT_0);
            counts[0].forwardedRxError = copyCounts(RE::PREVIOUS_ERROR_COUNTER_PORT_0);
            counts[0].linkLost = copyCounts(RE::LOST_LINK_COUNTER_PORT_0);
 
            counts[1].invalidFrame = copyCounts(RE::FRAME_ERROR_COUNTER_PORT_1);
            counts[1].rxError = copyCounts(RE::PHYSICAL_ERROR_COUNTER_PORT_1);
            counts[1].forwardedRxError = copyCounts(RE::PREVIOUS_ERROR_COUNTER_PORT_1);
            counts[1].linkLost = copyCounts(RE::LOST_LINK_COUNTER_PORT_1);
 
            counts[2].invalidFrame = copyCounts(RE::FRAME_ERROR_COUNTER_PORT_2);
            counts[2].rxError = copyCounts(RE::PHYSICAL_ERROR_COUNTER_PORT_2);
            counts[2].forwardedRxError = copyCounts(RE::PREVIOUS_ERROR_COUNTER_PORT_2);
            counts[2].linkLost = copyCounts(RE::LOST_LINK_COUNTER_PORT_2);
 
            counts[3].invalidFrame = copyCounts(RE::FRAME_ERROR_COUNTER_PORT_3);
            counts[3].rxError = copyCounts(RE::PHYSICAL_ERROR_COUNTER_PORT_3);
            counts[3].forwardedRxError = copyCounts(RE::PREVIOUS_ERROR_COUNTER_PORT_3);
            counts[3].linkLost = copyCounts(RE::LOST_LINK_COUNTER_PORT_3);
 
            auto slave = getSlaveAtIndex(d.slaveIndex);
            for (unsigned int i = 0; i < 4; i++)
            {
                if (counts[i].invalidFrame > 0 || counts[i].rxError > 0 ||
                    counts[i].forwardedRxError > 0 || counts[i].linkLost > 0)
                {
                    SLAVE_ERROR(slave->getSlaveIdentifier(),
                                "Error counters on port %u\n"
                                " - invalid frame: \t\t%u\n"
                                " - rx error: \t\t%u\n"
                                " - forwarded rx error: \t%u\n"
                                " - link lost: \t\t%u",
                                i,
                                counts[i].invalidFrame,
                                counts[i].rxError,
                                counts[i].forwardedRxError,
                                counts[i].linkLost);
                }
            }
        }
    }
 
    EtherCATState
    Bus::rtGetEtherCATState() const
    {
        return busState;
    }
 
    BusFunctionalState
    Bus::rtGetFunctionalState() const
    {
        return functionalState;
    }
 
    void
    Bus::registerSlaveFactory(const SlaveFactory& factory)
    {
        slaveFactories.push_back(factory);
    }
 
    void
    Bus::registerDeviceFactory(const std::string& name, const DeviceFactory& factory)
    {
        deviceFactories[name] = factory;
    }
 
    void
    Bus::setAndParseHardwareConfig(const MultiNodeRapidXMLReader& hwconfig)
    {
        hardware_config::ConfigParser parser(hwconfig);
        parser.parse();
        this->hwconfig = parser.getHardwareConfig();
    }
 
    std::vector<std::experimental::observer_ptr<SlaveInterface>>
    Bus::getSlaves() const
    {
        return slavesView;
        std::vector<SlaveInterface*> slaves_raw;
        //        std::transform(slaves.begin(),
        //                       slaves.end(),
        //                       std::back_inserter(slaves_raw),
        //                       [](auto& slave) { return slave.get(); });
        //        return slaves_raw;
    }
 
    const std::vector<std::shared_ptr<DeviceInterface>>&
    Bus::getDevices() const
    {
        return devices;
    }
 
    Bus::~Bus()
    {
    }
 
    std::experimental::observer_ptr<SlaveInterface>
    Bus::getSlaveAtIndex(std::uint16_t slaveIndex) const
    {
        for (const std::unique_ptr<SlaveInterface>& slave : slaves)
        {
            if (slave->getSlaveNumber() == slaveIndex)
            {
                return std::experimental::make_observer(slave.get());
            }
        }
 
        return nullptr;
    }
 
    bool
    Bus::rtIsEmergencyStopActive() const
    {
        bool found = false;
        for (const std::unique_ptr<SlaveInterface>& slave : slaves)
        {
            if (!errorHandler.isSlaveLostOrDuringReinitialization(slave.get()) &&
                slave->isEmergencyStopActive())
            {
                // dont break so that isEmergencyStopActive executed for each operational slave
                found = true;
            }
        }
        return found;
    }
 
    bool
    Bus::rtHasError() const
    {
        BUS_TIMING_START(bus_rtHasError)
        const bool hasError = errorHandler.hasError();
        BUS_TIMING_CEND(bus_rtHasError, iterationCount, 0.5)
        return hasError;
    }
 
    void
    Bus::setRobot(const VirtualRobot::RobotPtr& robot)
    {
        this->robot = robot;
    }
 
    void
    Bus::configureErrorCountersReading(bool enable, unsigned int periodInMS)
    {
        if (!enable)
        {
            errorRegisterReadingScheduler.reset();
        }
        else
        {
            errorRegisterReadingScheduler = std::make_unique<SlaveRegisterReadingScheduler>(
                static_cast<std::uint16_t>(slaves.size()),
                periodInMS,
                std::vector<datatypes::RegisterEnum>{
                    datatypes::RegisterEnum::LOST_LINK_COUNTER_PORT_0,
                    datatypes::RegisterEnum::LOST_LINK_COUNTER_PORT_1,
                    datatypes::RegisterEnum::FRAME_ERROR_COUNTER_PORT_0,
                    datatypes::RegisterEnum::FRAME_ERROR_COUNTER_PORT_1,
                    datatypes::RegisterEnum::PHYSICAL_ERROR_COUNTER_PORT_0,
                    datatypes::RegisterEnum::PHYSICAL_ERROR_COUNTER_PORT_1,
                    datatypes::RegisterEnum::PREVIOUS_ERROR_COUNTER_PORT_0,
                    datatypes::RegisterEnum::PREVIOUS_ERROR_COUNTER_PORT_1});
        }
    }
 
    std::uint64_t
    Bus::getIterationCount() const
    {
        return iterationCount;
    }
 
} // namespace armarx::control::ethercat