RTUnit.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2017, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ArmarX
 * @author     Mirko Waechter( mirko.waechter at kit dot edu)
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#include "RTUnit.h"
 
#include <chrono>
#include <filesystem>
#include <sstream>
#include <thread>
 
#include <sys/mman.h>
 
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/Tools/Gravity.h>
 
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/rapidxml/wrapper/MultiNodeRapidXMLReader.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/core/util/OnScopeExit.h>
 
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointController.h>
#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
#include <RobotAPI/components/units/RobotUnit/util/RtTiming.h>
#include <RobotAPI/libraries/core/Pose.h>
 
#include <armarx/control/ethercat/Bus.h>
#include <armarx/control/ethercat/DeviceInterface.h>
#include <armarx/control/ethercat/RTUtility.h>
#include <armarx/control/ethercat/SlaveErrorRegistersDevice.h>
#include <armarx/control/ethercat/SlaveInterface.h>
#include <armarx/control/ethercat/Timing.h>
 
namespace armarx::control::ethercat
{
    // The maximum stack size which is guaranteed safe to access without faulting
    static constexpr unsigned int MAX_SAFE_STACK = (8 * 1024);
 
    RTUnit::RTUnit()
    {
        ;
    }
 
    void
    RTUnit::onInitRobotUnit()
    {
        ARMARX_TRACE;
 
        ARMARX_INFO << "Locking memory";
 
        if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
        {
            ARMARX_WARNING << "Could not lock memory (mlockall failed)";
        }
 
        ARMARX_DEBUG << "Pre-fault our stack";
        unsigned char dummy[MAX_SAFE_STACK];
        memset(dummy, 0, MAX_SAFE_STACK);
 
        ARMARX_INFO << "RTUnit initializing now";
        robot = rtGetRobot()->clone();
        ARMARX_CHECK_EXPRESSION(rtGetRobot()->hasRobotNodeSet(properties.robotJointsNodeSet));
        ARMARX_CHECK_EXPRESSION(rtGetRobot()->hasRobotNodeSet(properties.robotColNodeSet));
        rtRobotJointSet = rtGetRobot()->getRobotNodeSet(properties.robotJointsNodeSet);
        rtRobotBodySet = rtGetRobot()->getRobotNodeSet(properties.robotColNodeSet);
        ARMARX_CHECK_NOT_NULL(rtRobotJointSet);
        ARMARX_CHECK_NOT_NULL(rtRobotBodySet);
        for (auto& node : *rtRobotJointSet)
        {
            node->setEnforceJointLimits(false);
        }
        std::stringstream massesInfo;
        for (int i = 0; i < static_cast<int>(rtRobotBodySet->getSize()); ++i)
        {
            auto node = rtRobotBodySet->getNode(i);
            massesInfo << "\t" << node->getName() << ": " << node->getMass() << " kg\n";
        }
        ARMARX_DEBUG << "Masses info: " << massesInfo.str();
 
        ARMARX_CHECK_EXPRESSION(
            std::filesystem::path(properties.periodicLoggingDirectory).is_absolute());
 
        //setting the bus
        Bus& bus = Bus::getBus();
        bus.setRobot(robot);
        bus.setLocalMinimumLoggingLevel(this->getEffectiveLoggingLevel());
        bus.setTimeouts(ethercatTimeouts); // Set configured timeouts.
    }
 
    void
    RTUnit::onConnectRobotUnit()
    {
        ARMARX_INFO << "RTUnit connects now";
 
        startRTThread();
        while (this->getRobotUnitState() < RobotUnitState::Running)
        {
            usleep(100'000);
        }
    }
 
    void
    RTUnit::onDisconnectRobotUnit()
    {
        ARMARX_INFO << "RTUnit disconnects now";
    }
 
    void
    RTUnit::onExitRobotUnit()
    {
        ARMARX_INFO << "RTUnit is exiting now ";
 
        RTUtility::stopLowLatencyMode();
 
        ARMARX_INFO << "RTUnit exit complete";
    }
 
    void
    RTUnit::initializeKinematicUnit()
    {
        using IfaceT = KinematicUnitInterface;
 
        auto guard = getGuard();
        throwIfStateIsNot(RobotUnitState::InitializingUnits, __FUNCTION__);
        //check if unit is already added
        if (getUnit(IfaceT::ice_staticId()))
        {
            return;
        }
 
        auto unit = createKinematicSubUnit(getIceProperties()->clone(),
                                           ControlModes::Position1DoF,
                                           properties.useTorqueVelocityModeAsDefault
                                               ? ControlModes::VelocityTorque
                                               : ControlModes::Velocity1DoF);
        //add
        if (unit)
        {
            addUnit(std::move(unit));
        }
    }
 
    void
    RTUnit::startRTThread()
    {
        ARMARX_INFO << "starting control task";
 
        if (rtTask.joinable())
        {
            rtTask.join();
        }
 
        rtTask = std::thread{[this]
                             {
                                 taskRunning = true;
                                 try
                                 {
                                     this->rtRun();
                                 }
                                 catch (...)
                                 {
                                     ARMARX_FATAL << "RT Thread caused an uncaught exception:\n"
                                                  << GetHandledExceptionString();
                                 }
                             }};
    }
 
    void
    RTUnit::joinControlThread()
    {
        ARMARX_INFO << "stopping control task";
        taskRunning = false;
        rtTask.join();
        ARMARX_INFO << "rt task stopped";
    }
 
    void
    RTUnit::publish(StringVariantBaseMap debugObserverMap, StringVariantBaseMap timingMap)
    {
        RobotUnit::publish(std::move(debugObserverMap), std::move(timingMap));
    }
 
    armarx::PropertyDefinitionsPtr
    RTUnit::createPropertyDefinitions()
    {
        PropertyDefinitionsPtr def = RobotUnit::createPropertyDefinitions();
 
        def->optional(
            properties.busConfigFilePath, "BusConfigFilePath", "Location of the BusConfigFile");
        def->optional(properties.socketFileDescriptor,
                      "SocketFileDescriptor",
                      "Socketfiledescriptor on which the ethercat connection is running");
        def->optional(
            properties.ethercatInterfaceName,
            "EthercatInterfaceName",
            "Name of the ethercat socket. If set to non-empty string, this will be used over "
            "SocketFileDescriptor");
        def->optional(properties.useTorqueVelocityModeAsDefault,
                      "UseTorqueVelocityModeAsDefault",
                      "If true, the KinematicUnit will use TorqueVelocity mode for velocity mode");
        def->optional(properties.rtLoopTimeInUS,
                      "RTLoopTimeInUS",
                      "Desired duration of one interation in the real-time control loop");
        def->optional(properties.rTLoopTimingCheckToleranceFactor,
                      "RTLoopTimingCheckToleranceFactor",
                      "Factor by which the timing checks are multiplied. Higher value -> less "
                      "warning outputs");
 
 
        // Periodic Logging
        def->optional(properties.periodicLoggingDirectory,
                      "PeriodicLoggingDirectory",
                      "This absolute path to a directory is used for storing periodic log-files.");
        def->optional(
            properties.periodicLoggingHistorySize,
            "PeriodicLoggingHistorySize",
            "The amount of older iterations of the periodic logging files that are kept.");
        def->optional(properties.periodicLoggingIntervalInSeconds,
                      "PeriodicLoggingIntervalInSeconds",
                      "The duration in seconds of one periodic logging interval. After that "
                      "duration, a new set of log-files will be created");
        def->optional(properties.periodicLoggingMode,
                      "PeriodicLoggingMode",
                      "Select in which mode the periodic logging should be executed.")
            .map("None", PeriodicLoggingMode::None)
            .map("OnlyBusErrors", PeriodicLoggingMode::OnlyBusErrors)
            .map("OnlyRtLogging", PeriodicLoggingMode::OnlyRtLogging)
            .map("Both", PeriodicLoggingMode::Both);
 
 
        // EtherCAT error counters
        def->optional(properties.enableErrorCountersReading,
                      "ErrorCountersReading.Enable",
                      "Whether to periodically read the error counters from all slaves and "
                      "provide them as sensor values");
        def->optional(properties.errorCountersReadingPeriodInMS,
                      "ErrorCountersReading.PeriodInMS",
                      "Wait time in milliseconds between two full reads of the error counters of "
                      "all slaves.");
        def->optional(properties.resetErrorRegistersAtStartup,
                      "ErrorCountersReading.ResetAtStartup",
                      "Whether to reset all error counters on all slaves after starting the bus.");
 
        // node sets
        def->optional(properties.robotJointsNodeSet, "nodeSet.robotJoints", "The node set which contains the actuated joints.");
        def->optional(properties.robotColNodeSet, "nodeSet.robotCol", "The node set for e.g. gravity computation.");
 
        def->optional(ethercatTimeouts.registerRead_us,
                      "ethercat.timeouts.register_read",
                      "EtherCAT timeout for a register frame to read certain registers in µs.");
        def->optional(ethercatTimeouts.stateCheck_us,
                      "ethercat.timeouts.state_check",
                      "EtherCAT timeout to check for the current EtherCAT state of slaves in µs.");
        def->optional(ethercatTimeouts.sdoRead_us,
                      "ethercat.timeouts.sdo_read",
                      "EtherCAT timeout to read an SDO in µs.");
        def->optional(ethercatTimeouts.sdoWrite_us,
                      "ethercat.timeouts.sdo_write",
                      "EtherCAT timeout to write an SDO in µs.");
 
        return def;
    }
 
    RTUnit::~RTUnit()
    {
        ARMARX_INFO << "DTOR of RTUnit";
 
        backupLoggingRunning.store(false);
        if (backupLoggingTask.joinable())
        {
            backupLoggingTask.join();
        }
    }
 
    void
    RTUnit::rtRun()
    {
        ARMARX_INFO << "Control task running";
        Bus& bus = Bus::getBus();
 
        // Start periodic logging of rt-values and bus errors
        startBackupLogging();
 
        ARMARX_ON_SCOPE_EXIT
        {
            ARMARX_INFO << "Leaving rtRun scope";
            if (bus.rtGetEtherCATState() != EtherCATState::init)
            {
                bus.shutdown();
            }
            backupLoggingRunning.store(false); // This will automatically dump the latest BusErrors
            if (backupLoggingTask.joinable())
            {
                backupLoggingTask.join();
            }
        };
 
        bool busStartSucceeded = false;
 
 
        try
        {
            ARMARX_DEBUG << "initBus";
            busStartSucceeded = initBusRTThread();
        }
        catch (...)
        {
            ARMARX_ERROR << "Initializing EtherCAT bus RT thread failed with exception: "
                         << armarx::GetHandledExceptionString();
            return;
        }
 
        ARMARX_INFO << "initBus finished with: " << busStartSucceeded;
        if (!busStartSucceeded)
        {
            return;
        }
 
        bool initializationDone = false;
        bool initializationFailed = false;
        ARMARX_DEBUG << "Async init starting now";
        std::thread unitInitTask = std::thread{
            [&, this]
            {
                try
                {
                    finishDeviceInitialization();
                    // rtReadSensorDeviceValues(TimeUtil::GetTime(), IceUtil::Time::microSeconds(1)); // initialize sensor values
                    initializeDefaultUnits();
                    ARMARX_IMPORTANT << "Setting up custom Units";
                    finishUnitInitialization();
                    ARMARX_IMPORTANT << "Finishing setting up custom Units...DONE";
                    ARMARX_INFO << "Sleeping a moment to let everything settle in";
                    usleep(500000);
                    initializationDone = true;
                }
                catch (...)
                {
                    ARMARX_FATAL
                        << "Shutting down - RobotUnit Init Thread caused an uncaught exception:\n"
                        << GetHandledExceptionString();
                    initializationFailed = true;
                }
            }};
        unitInitTask.detach();
        if (initializationFailed)
        {
            return;
        }
        if (busStartSucceeded)
        {
            //setting the timestamps for the pdo update, at the moment we only have on
            currentPDOUpdateTimestamp = armarx::rtNow();
            CycleUtil cycleStats(IceUtil::Time::microSeconds(properties.rtLoopTimeInUS));
            cycleStats.setBusyWaitShare(0.1f);
            while (!initializationDone)
            {
                // auto realDeltaT = currentPDOUpdateTimestamp - lastPDOUpdateTimestamp;
                // auto cappedDeltaT = IceUtil::Time::microSeconds(boost::algorithm::clamp(realDeltaT.toMicroSeconds(), 1, 2000)); // TODO: Make property
                //                bus.updateBus(false);
                // rtReadSensorDeviceValues(currentPDOUpdateTimestamp, cappedDeltaT);
                // lastPDOUpdateTimestamp = currentPDOUpdateTimestamp;
                // currentPDOUpdateTimestamp = TimeUtil::GetTime();
                cycleStats.waitForCycleDuration();
                ARMARX_INFO << deactivateSpam(1) << "Waiting for unit initialization!";
            }
            ARMARX_IMPORTANT << "RobotUnit is now ready";
        }
        else
        {
            ARMARX_WARNING << "Bus was not started!";
            return;
        }
 
        ARMARX_DEBUG << "Setting up gravity calculation";
        // init data structs for gravity calculation
        for (int i = 0; i < static_cast<int>(rtRobotJointSet->getSize()); i++)
        {
            auto selectedJoint = getSensorDevice(rtRobotJointSet->getNode(i)->getName());
            if (selectedJoint)
            {
                auto sensorValue = const_cast<SensorValue1DoFGravityTorque*>(
                    selectedJoint->getSensorValue()->asA<SensorValue1DoFGravityTorque>());
                ARMARX_DEBUG << "will calculate gravity for robot node "
                             << rtRobotJointSet->getNode(i)->getName();
                nodeJointDataVec.push_back(
                    std::make_pair(rtRobotJointSet->getNode(i), sensorValue));
            }
            else
            {
                ARMARX_INFO << "Joint " << rtRobotJointSet->getNode(i)->getName() << " not found";
                nodeJointDataVec.push_back(std::make_pair(rtRobotJointSet->getNode(i), nullptr));
            }
        }
 
        controlLoopRTThread();
 
        while (getObjectScheduler() && !getObjectScheduler()->isTerminationRequested())
        {
            ARMARX_INFO << deactivateSpam() << "Waiting for shutdown";
            usleep(10000);
        }
    }
 
    MultiNodeRapidXMLReader
    RTUnit::ReadHardwareConfigFile(std::string busConfigFilePath, std::string robotName)
    {
        ARMARX_TRACE;
        if (!ArmarXDataPath::getAbsolutePath(busConfigFilePath, busConfigFilePath))
        {
            throw LocalException("could not find BusConfigFilePath: ") << busConfigFilePath;
        }
        ARMARX_CHECK(std::filesystem::is_regular_file(busConfigFilePath))
            << "The bus config file `" << busConfigFilePath << "` does not exist!";
 
        ARMARX_INFO_S << "Read the hw config from " << busConfigFilePath;
 
        //reading the config for the Bus and create all the robot objects in the robot container
        auto busConfigFilePathDir = std::filesystem::path(busConfigFilePath).parent_path();
 
        ARMARX_TRACE;
        ARMARX_INFO_S << "Reading file `" << busConfigFilePath << "`";
        auto rapidXmlReaderPtr = RapidXmlReader::FromFile(busConfigFilePath);
        ARMARX_CHECK_NOT_NULL(rapidXmlReaderPtr) << "Failed to read `" << busConfigFilePath << "`";
 
        ARMARX_TRACE;
        auto rootNode = rapidXmlReaderPtr->getRoot("HardwareConfig");
        ARMARX_CHECK(rootNode.is_valid());
        ARMARX_CHECK(rootNode.attribute_value("robotName") == robotName);
 
        MultiNodeRapidXMLReader multiNode({rootNode});
        for (RapidXmlReaderNode& includeNode : rootNode.nodes("include"))
        {
            ARMARX_CHECK(includeNode.has_attribute("file"))
                << "File `" << busConfigFilePath
                << "` contains invalid `include` tag. The `file` attribute is missing.";
            const auto relPath = includeNode.attribute_value("file");
 
            std::filesystem::path filepath = (busConfigFilePathDir / relPath);
            if (!std::filesystem::exists(filepath))
            {
                std::string absPath;
                if (!ArmarXDataPath::getAbsolutePath(relPath, absPath))
                {
                    throw LocalException("Could not find config file at path ") << relPath;
                }
            }
 
            ARMARX_TRACE;
            auto includedNode = RapidXmlReader::FromFile(filepath.string());
            ARMARX_CHECK(includedNode->getRoot("HardwareConfig").is_valid())
                << "Invalid root node for XML file `" << filepath.string() << "`";
            ARMARX_CHECK(includedNode->getRoot("HardwareConfig").attribute_value("robotName") ==
                         robotName);
            multiNode.addNode(includedNode->getRoot("HardwareConfig"));
        }
        return multiNode;
    }
 
    bool
    RTUnit::initBusRTThread()
    {
        // init EtherCAT
        Bus& bus = Bus::getBus();
        bus.setSocketFileDescriptor(properties.socketFileDescriptor);
        bus.setIfName(properties.ethercatInterfaceName);
        if (!bus.switchBusToSafeOp())
        {
            ARMARX_ERROR << "Bringing bus to EtherCAT state 'safe-operational' failed.";
            return false;
        }
 
        auto addDevices = [&](auto device)
        {
            auto cd = std::dynamic_pointer_cast<ControlDevice>(device);
            if (cd)
            {
                addControlDevice(cd);
            }
            auto sd = std::dynamic_pointer_cast<SensorDevice>(device);
            if (sd)
            {
                addSensorDevice(sd);
            }
        };
 
        for (auto& device : bus.getDevices())
        {
            addDevices(device);
            for (const auto& subDevice : device->getSubDevices())
            {
                addDevices(subDevice);
            }
        }
 
        if (properties.resetErrorRegistersAtStartup)
        {
            for (const auto& slave : bus.getSlaves())
            {
                bus.resetErrorRegisters(
                    static_cast<std::uint16_t>(slave->getSlaveIdentifier().slaveIndex));
            }
        }
 
        if (properties.enableErrorCountersReading)
        {
            bus.configureErrorCountersReading(properties.enableErrorCountersReading,
                                              properties.errorCountersReadingPeriodInMS);
            // Add special slave error counters sensor devices
            for (const auto& slave : bus.getSlaves())
            {
                auto sd = std::dynamic_pointer_cast<SensorDevice>(slave->getErrorRegistersDevice());
                if (sd)
                {
                    addSensorDevice(sd);
                }
            }
        }
 
        ARMARX_INFO << "EtherCAT bus is started";
        return true;
    }
 
    void
    RTUnit::controlLoopRTThread()
    {
        Bus& bus = Bus::getBus();
        try
        {
            finishControlThreadInitialization();
 
            RTUtility::pinThreadToCPU(0);
            RTUtility::elevateThreadPriority(RTUtility::RT_THREAD_PRIORITY);
            RTUtility::startLowLatencyMode();
 
            //to not get any strange start values
            currentPDOUpdateTimestamp = armarx::rtNow();
            CycleUtil cycleKeeper(IceUtil::Time::microSeconds(properties.rtLoopTimeInUS));
            cycleKeeper.setBusyWaitShare(1.0f);
            ARMARX_CHECK_EXPRESSION(rtGetRobot());
            ARMARX_CHECK_EXPRESSION(rtRobotJointSet);
            ARMARX_CHECK_EXPRESSION(rtRobotBodySet);
            ARMARX_CHECK_EXPRESSION(rtRobotJointSet->getSize() > 0);
            VirtualRobot::Gravity gravity(rtGetRobot(), rtRobotJointSet, rtRobotBodySet);
 
            const std::uint64_t loopDurationThreshold =
                properties.rtLoopTimeInUS / 1000 * properties.rTLoopTimingCheckToleranceFactor;
 
            //#if 0
            std::vector<float> gravityValues(rtRobotJointSet->getSize());
            ARMARX_CHECK_EQUAL(rtRobotJointSet->getSize(), nodeJointDataVec.size());
            //#endif
 
 
            if (!bus.switchBusToOp())
            {
                BUS_FATAL_AND_THROW(bus.getIterationCount(), "Switching bus to EtherCAT state 'operational' failed");
            }
 
 
            ARMARX_INFO << "RT control loop started";
            EmergencyStopState lastSoftwareEmergencyStopState = rtGetEmergencyStopState();
 
            auto lastMonoticTimestamp = armarx::rtNow();
            auto currentMonotonicTimestamp = lastMonoticTimestamp;
            currentPDOUpdateTimestamp = armarx::rtNow();
 
            for (; taskRunning; ++_iterationCount)
            {
                const auto loopStartTime = armarx::rtNow();
                rtGetRTThreadTimingsSensorDevice().rtMarkRtLoopStart();
 
                auto realDeltaT = currentMonotonicTimestamp - lastMonoticTimestamp;
                auto cappedDeltaT = IceUtil::Time::microSeconds(
                    std::clamp<long>(realDeltaT.toMicroSeconds(), 1, 2000)); // TODO: Make property
                if (rtIsCalibrating())
                {
                    BUS_TIMING_START(calibrateActivateControlers)
                    rtCalibrateActivateControlers();
                    BUS_TIMING_CEND(calibrateActivateControlers,
                                    bus.getIterationCount(),
                                    0.3 * loopDurationThreshold)
 
                    BUS_TIMING_START(switchControllerSetup)
                    rtSwitchControllerSetup(SwitchControllerMode::RTThread);
                    BUS_TIMING_CEND(
                        switchControllerSetup, bus.getIterationCount(), 0.3 * loopDurationThreshold)
 
                    BUS_TIMING_START(resetAllTargets)
                    rtResetAllTargets();
                    BUS_TIMING_CEND(
                        resetAllTargets, bus.getIterationCount(), 0.3 * loopDurationThreshold)
 
                    BUS_TIMING_START(calibrateInRt)
                    _calibrationStatus = rtCalibrate();
                    BUS_TIMING_CEND(
                        calibrateInRt, bus.getIterationCount(), 0.3 * loopDurationThreshold)
 
                    BUS_TIMING_START(handleInvalidTargets)
                    rtHandleInvalidTargets();
                    BUS_TIMING_CEND(
                        handleInvalidTargets, bus.getIterationCount(), 0.3 * loopDurationThreshold)
 
                    BUS_TIMING_START(runJointControllers)
                    rtRunJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
                    BUS_TIMING_CEND(
                        runJointControllers, bus.getIterationCount(), 0.3 * loopDurationThreshold)
                }
                else
                {
                    BUS_TIMING_START(RunControllers)
                    BUS_TIMING_START(SwitchControllers)
                    rtSwitchControllerSetup();
                    BUS_TIMING_CEND(
                        SwitchControllers, bus.getIterationCount(), 0.3 * loopDurationThreshold)
                    BUS_TIMING_START(ResettingTargets)
                    rtResetAllTargets();
                    BUS_TIMING_CEND(
                        ResettingTargets, bus.getIterationCount(), 0.3 * loopDurationThreshold)
                    BUS_TIMING_START(RunNJointControllers)
                    rtRunNJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
                    BUS_TIMING_CEND(
                        RunNJointControllers, bus.getIterationCount(), 0.3 * loopDurationThreshold)
                    BUS_TIMING_START(CheckInvalidTargets)
                    rtHandleInvalidTargets();
                    BUS_TIMING_CEND(
                        CheckInvalidTargets, bus.getIterationCount(), 0.3 * loopDurationThreshold)
 
                    BUS_TIMING_START(RunJointControllers)
                    rtRunJointControllers(currentPDOUpdateTimestamp, cappedDeltaT);
                    BUS_TIMING_CEND(
                        RunJointControllers, bus.getIterationCount(), 0.3 * loopDurationThreshold)
                    BUS_TIMING_CEND(
                        RunControllers, bus.getIterationCount(), 0.3 * loopDurationThreshold)
                }
 
                //bus update
                BUS_TIMING_START(rtBusSendReceive)
                rtGetRTThreadTimingsSensorDevice().rtMarkRtBusSendReceiveStart();
                if (bus.rtGetEtherCATState() == EtherCATState::op)
                {
                    // error correction
                    BUS_TIMING_START(getEmergencyStopState)
                    auto currentSoftwareEmergencyStopState = rtGetEmergencyStopState();
                    BUS_TIMING_CEND(
                        getEmergencyStopState, bus.getIterationCount(), 0.5 * loopDurationThreshold)
                    if (lastSoftwareEmergencyStopState ==
                            EmergencyStopState::eEmergencyStopActive &&
                        currentSoftwareEmergencyStopState ==
                            EmergencyStopState::eEmergencyStopInactive)
                    {
                        //                    bus.rebootBus();
                    }
                    lastSoftwareEmergencyStopState = currentSoftwareEmergencyStopState;
 
                    BUS_TIMING_START(rtUpdateBus)
                    bus.rtUpdateBus(true, _iterationCount);
                    BUS_TIMING_CEND(
                        rtUpdateBus, bus.getIterationCount(), 0.6 * loopDurationThreshold)
                    if (bus.rtIsEmergencyStopActive() || bus.rtHasError())
                    {
                        rtSetEmergencyStopState(EmergencyStopState::eEmergencyStopActive);
                    }
                }
                currentPDOUpdateTimestamp = armarx::rtNow();
 
                rtGetRTThreadTimingsSensorDevice().rtMarkRtBusSendReceiveEnd();
                BUS_TIMING_CEND(rtBusSendReceive, bus.getIterationCount(), 0.55)
                BUS_TIMING_START(ReadSensorValues)
                rtReadSensorDeviceValues(currentPDOUpdateTimestamp, cappedDeltaT);
                BUS_TIMING_CEND(
                    ReadSensorValues, bus.getIterationCount(), 0.7 * loopDurationThreshold)
                lastMonoticTimestamp = currentMonotonicTimestamp;
                currentMonotonicTimestamp = armarx::rtNow();
 
                BUS_TIMING_START(Publish)
                rtUpdateSensorAndControlBuffer(currentPDOUpdateTimestamp, cappedDeltaT);
                BUS_TIMING_CEND(Publish, bus.getIterationCount(), 0.15 * loopDurationThreshold)
 
                BUS_TIMING_START(ComputeGravityTorques)
                gravity.computeGravityTorque(gravityValues);
                size_t i = 0;
                for (auto& node : nodeJointDataVec)
                {
                    auto torque = gravityValues.at(i);
                    if (node.second)
                    {
                        node.second->gravityTorque = -torque;
                    }
 
                    i++;
                }
                BUS_TIMING_CEND(
                    ComputeGravityTorques, bus.getIterationCount(), 0.2 * loopDurationThreshold)
 
 
                rtGetRTThreadTimingsSensorDevice().rtMarkRtLoopPreSleep();
 
                const auto loopPreSleepTime = armarx::rtNow();
                BUS_TIMING_START(RTLoopWaiting)
                cycleKeeper.waitForCycleDuration();
                BUS_TIMING_CEND(RTLoopWaiting, bus.getIterationCount(), loopDurationThreshold)
                const auto loopPostSleepTime = armarx::rtNow();
 
                const auto loopDuration = armarx::rtNow() - loopStartTime;
 
                if (bus.rtGetFunctionalState() == BusFunctionalState::Running &&
                    loopDuration.toMicroSecondsDouble() >
                        (properties.rtLoopTimeInUS + 500) *
                            properties.rTLoopTimingCheckToleranceFactor)
                {
                    const SensorValueRTThreadTimings* sval =
                        rtGetRTThreadTimingsSensorDevice()
                            .getSensorValue()
                            ->asA<SensorValueRTThreadTimings>();
                    ARMARX_CHECK_NOT_NULL(sval);
                    BUS_WARNING(
                        bus.getIterationCount(),
                        "RT loop is under 1kHz control frequency: %.1f \n"
                        "-- cycle time PDO timestamp %.1f µs\n"
                        "-- sleep                    %.1f µs\n"
                        "-- thread timing sensor value: \n"
                        "---- rtSwitchControllerSetup        Duration %.1f µs\t"
                        " RoundTripTime %.1f µs\n"
                        "---- rtRunNJointControllers         Duration %.1f µs\t"
                        " RoundTripTime %.1f µs\n"
                        "---- rtHandleInvalidTargets         Duration %.1f µs\t"
                        " RoundTripTime %.1f µs\n"
                        "---- rtRunJointControllers          Duration %.1f µs\t"
                        " RoundTripTime %.1f µs\n"
                        "---- rtBusSendReceive               Duration %.1f µs\t"
                        " RoundTripTime %.1f µs\n"
                        "---- rtReadSensorDeviceValues       Duration %.1f µs\t"
                        " RoundTripTime %.1f µs\n"
                        "---- rtUpdateSensorAndControlBuffer Duration %.1f µs\t"
                        " RoundTripTime %.1f µs\n"
                        "---- rtResetAllTargets              Duration %.1f µs\t"
                        " RoundTripTime %.1f µs\n"
                        "---- rtLoop                         Duration %.1f µs\t"
                        " RoundTripTime %.1f µs\n",
                        realDeltaT.toMicroSecondsDouble(),
                        loopDuration.toMicroSecondsDouble(),
                        (loopPostSleepTime - loopPreSleepTime).toMicroSecondsDouble(),
                        sval->rtSwitchControllerSetupDuration.toMicroSecondsDouble(),
                        sval->rtSwitchControllerSetupRoundTripTime.toMicroSecondsDouble(),
                        sval->rtRunNJointControllersDuration.toMicroSecondsDouble(),
                        sval->rtRunNJointControllersRoundTripTime.toMicroSecondsDouble(),
                        sval->rtHandleInvalidTargetsDuration.toMicroSecondsDouble(),
                        sval->rtHandleInvalidTargetsRoundTripTime.toMicroSecondsDouble(),
                        sval->rtRunJointControllersDuration.toMicroSecondsDouble(),
                        sval->rtRunJointControllersRoundTripTime.toMicroSecondsDouble(),
                        sval->rtBusSendReceiveDuration.toMicroSecondsDouble(),
                        sval->rtBusSendReceiveRoundTripTime.toMicroSecondsDouble(),
                        sval->rtReadSensorDeviceValuesDuration.toMicroSecondsDouble(),
                        sval->rtReadSensorDeviceValuesRoundTripTime.toMicroSecondsDouble(),
                        sval->rtUpdateSensorAndControlBufferDuration.toMicroSecondsDouble(),
                        sval->rtUpdateSensorAndControlBufferRoundTripTime.toMicroSecondsDouble(),
                        sval->rtResetAllTargetsDuration.toMicroSecondsDouble(),
                        sval->rtResetAllTargetsRoundTripTime.toMicroSecondsDouble(),
                        sval->rtLoopDuration.toMicroSecondsDouble(),
                        sval->rtLoopRoundTripTime.toMicroSecondsDouble());
                }
            }
            ARMARX_IMPORTANT << "RT loop stopped";
            ARMARX_INFO << "Execution stats: Average: "
                        << cycleKeeper.getAverageDuration().toMilliSecondsDouble()
                        << " max: " << cycleKeeper.getMaximumDuration().toMilliSecondsDouble()
                        << " min: " << cycleKeeper.getMinimumDuration().toMilliSecondsDouble();
            //switching off the bus and be sure that the bus will receive
            bus.shutdown();
        }
        catch (UserException& e)
        {
            ARMARX_FATAL << "exception in control thread!"
                         << "\nwhat:\n"
                         << e.what() << "\nreason:\n"
                         << e.reason << "\n\tname: " << e.ice_id() << "\n\tfile: " << e.ice_file()
                         << "\n\tline: " << e.ice_line() << "\n\tstack: " << e.ice_stackTrace()
                         << std::flush;
            bus.shutdown();
        }
        catch (Ice::Exception& e)
        {
            ARMARX_FATAL << "exception in control thread!\nwhat:\n"
                         << e.what() << "\n\tname: " << e.ice_id() << "\n\tfile: " << e.ice_file()
                         << "\n\tline: " << e.ice_line() << "\n\tstack: " << e.ice_stackTrace()
                         << std::flush;
            bus.shutdown();
        }
        catch (std::exception& e)
        {
            ARMARX_FATAL << "exception in control thread!\nwhat:\n" << e.what() << std::flush;
            bus.shutdown();
        }
        catch (...)
        {
            ARMARX_FATAL << "exception in control thread!" << std::flush;
            bus.shutdown();
        }
        ARMARX_INFO << "Leaving control loop function";
    }
 
    void
    RTUnit::startBackupLogging()
    {
        ARMARX_IMPORTANT << "startBackupLogging()";
 
        if (properties.periodicLoggingMode == PeriodicLoggingMode::None)
        {
            ARMARX_IMPORTANT << "Periodic backup logging deactivated.";
            return;
        }
 
        backupLoggingTask = std::thread{[this]
                                        {
                                            try
                                            {
                                                backupLoggingRunning.store(true);
                                                periodicBackupLoggingRun();
                                            }
                                            catch (...)
                                            {
                                                handleExceptions();
                                            }
                                        }};
    }
 
    void
    RTUnit::periodicBackupLoggingRun()
    {
        // This loop is responsible for keeping the last x minutes of Sensor- and ControlData and BusErrors in temporary files
        // Each x minutes a new instance of rtLogging is started and therefore a new file generated.
        // Similary each x minutes the BusErrors from that timeframe are dumped into a file.
 
        struct FilePaths
        {
            std::filesystem::path rtLogPath;
            std::filesystem::path busErrorLogPath;
        };
 
        std::queue<FilePaths> fileHistory;
        IceUtil::Time lastIterationStart = armarx::rtNow();
 
        std::string robotUnitLogPathFormat =
            std::filesystem::path(properties.periodicLoggingDirectory)
                .append("rt_log_{DateTime}.csv")
                .string();
        std::filesystem::path busErrorLogPath =
            std::filesystem::path(properties.periodicLoggingDirectory).append("bus_errors.txt");
 
 
        SimpleRemoteReferenceCounterBasePtr token;
        while (backupLoggingRunning.load())
        {
            ARMARX_TRACE;
            if (fileHistory.size() > properties.periodicLoggingHistorySize)
            {
                FilePaths p = fileHistory.front();
                std::filesystem::remove(p.rtLogPath);
                std::filesystem::remove(p.busErrorLogPath);
                fileHistory.pop();
            }
 
            lastIterationStart = armarx::rtNow();
            while (backupLoggingRunning.load() &&
                   armarx::rtNow() <
                       lastIterationStart +
                           IceUtil::Time::seconds(properties.periodicLoggingIntervalInSeconds))
            {
                if (!token && this->getRobotUnitState() == RobotUnitState::Running &&
                    (properties.periodicLoggingMode == PeriodicLoggingMode::OnlyRtLogging ||
                     properties.periodicLoggingMode == PeriodicLoggingMode::Both))
                {
                    ARMARX_DEBUG << "Start new periodic log iteration";
                    token = this->startRtLogging(robotUnitLogPathFormat, {"*"});
                }
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
            }
 
            ARMARX_TRACE;
 
            std::string rtLogActualPath;
            if (backupLoggingRunning.load() && token)
            {
                rtLogActualPath = token->getId();
                this->stopRtLogging(token);
                token = nullptr;
            }
 
            ARMARX_TRACE;
 
            std::string busErrorLogActualPath;
            busErrorLogActualPath = Bus::getBus().logErrorsToFile(
                busErrorLogPath, properties.periodicLoggingIntervalInSeconds);
 
            if (properties.periodicLoggingMode == PeriodicLoggingMode::OnlyRtLogging ||
                properties.periodicLoggingMode == PeriodicLoggingMode::None)
            {
                std::filesystem::remove(busErrorLogActualPath);
            }
 
            if (busErrorLogActualPath.empty())
            {
                // No errors happend in the last interval therefore we can delete the
                // rtLog as well to save disk space
                std::filesystem::remove(rtLogActualPath);
            }
            else
            {
                fileHistory.push({rtLogActualPath, busErrorLogActualPath});
            }
        }
 
        // Special case for last rtLog in case of shutdown
        // This rt log gets always saved no matter if the corresponding bus error log is empty or not
        // We have to check it by hand and delete if no bus errors happend
        if (token && (fileHistory.empty() || fileHistory.back().rtLogPath != token->getId()))
        {
            std::filesystem::remove(token->getId());
        }
    }
} // namespace armarx::control::ethercat