FluxioCompositeExecutor.cpp

Go to the documentation of this file.

#include "FluxioCompositeExecutor.h"
 
#include <algorithm>
#include <atomic>
#include <chrono>
#include <cstddef>
#include <experimental/memory>
#include <functional>
#include <iterator>
#include <list>
#include <memory>
#include <mutex>
#include <optional>
#include <shared_mutex>
#include <string>
#include <thread>
#include <utility>
#include <vector>
 
#include <IceUtil/UUID.h>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
 
#include "RobotAPI/libraries/aron/core/data/variant/container/Dict.h"
#include "RobotAPI/libraries/aron/core/data/variant/forward_declarations.h"
#include "RobotAPI/libraries/skills/core/FluxioNode.h"
#include "RobotAPI/libraries/skills/core/FluxioParameterNode.h"
#include "RobotAPI/libraries/skills/core/FluxioProfile.h"
#include "RobotAPI/libraries/skills/core/FluxioResult.h"
#include "RobotAPI/libraries/skills/core/FluxioSkill.h"
#include "RobotAPI/libraries/skills/core/FluxioSkillStatusUpdate.h"
#include "RobotAPI/libraries/skills/core/FluxioSubSkillNode.h"
#include "RobotAPI/libraries/skills/core/SkillStatusUpdate.h"
#include "RobotAPI/libraries/skills/core/error/FluxioException.h"
#include "RobotAPI/libraries/skills/core/executor/FluxioExecutor.h"
 
#include "../FluxioControlNode.h"
#include "../FluxioEdge.h"
#include "../FluxioParameter.h"
#include "FluxioLoopExecutor.h"
#include "FluxioMergerExecutor.h"
 
namespace armarx::skills
{
    FluxioCompositeExecutor::FluxioCompositeExecutor(
        const std::string& id,
        const skills::FluxioSkill& skill,
        const std::function<skills::Result<std::experimental::observer_ptr<FluxioExecutor>,
                                           skills::error::FluxioException>(
            const std::string& skillId,
            const std::string& profileId,
            const std::string& executorName,
            armarx::aron::data::DictPtr parameters)>&& executeFluxioSkillFunc,
        const std::function<std::experimental::observer_ptr<FluxioExecutor>(
            const std::vector<std::string>& parameterIds)>&& addMergerExecutorToDCFunc,
        const std::function<std::experimental::observer_ptr<FluxioExecutor>(
            const std::string& id,
            const skills::FluxioSkill& skill,
            bool isRetry)>&& addLoopExecutorToDCFunc) :
        FluxioExecutor(id, false),
        executeFluxioSkill(executeFluxioSkillFunc),
        addMergerExecutorToDC(addMergerExecutorToDCFunc),
        addLoopExecutorToDC(addLoopExecutorToDCFunc),
        skill(skill)
    {
        std::scoped_lock l(possibleInputsMutex);
        possibleInputs[skill.id] = nullptr;
        for (const auto& node : skill.nodes)
        {
            possibleInputs[node.first] = nullptr;
        }
 
        std::scoped_lock resultLock(resultMutex);
        result = std::make_shared<armarx::aron::data::Dict>();
    }
 
    void
    FluxioCompositeExecutor::run(const std::string executorName,
                                 armarx::aron::data::DictPtr parameters,
                                 std::experimental::observer_ptr<const FluxioProfile> profilePtr)
    {
        ARMARX_INFO << "Running skill " << skill.name;
        setStatus(skills::SkillStatus::Constructing);
        this->executorName = executorName;
 
        std::unique_lock possibleInputsLock(possibleInputsMutex);
        this->possibleInputs[skill.id] = parameters;
 
        // fill in missing with default values
        for (const auto& [key, param] : this->skill.parameters)
        {
            if (param.type->getShortName() == "Object<Event>" || !param.isInput ||
                this->possibleInputs[this->skill.id]->hasElement(key))
            {
                continue;
            }
            const auto& fallBack = this->findParameterValue(profilePtr, param);
 
            if (fallBack == nullptr)
            {
                ARMARX_INFO << "No fallback value found for parameter " << param.name;
                if (!param.required)
                {
                    ARMARX_INFO
                        << "Parameter is not required. Using nullptr (std::nullopt) instead.";
                }
                else
                {
                    ARMARX_INFO << "Can't execute skill due to incomplete params.";
                    ARMARX_INFO << "Aborting skill execution.";
                    this->setStatus(skills::SkillStatus::Aborted);
                    return;
                }
            }
 
            this->possibleInputs[this->skill.id]->addElement(key, fallBack);
        }
        possibleInputsLock.unlock();
 
        skills::FluxioEdge startEdge;
        if (!validateSkill(startEdge))
        {
 
            ARMARX_WARNING << "Skill execution cancelled.";
            setStatus(skills::SkillStatus::Failed);
            return;
        }
 
        setStatus(skills::SkillStatus::Initializing);
 
        std::unique_lock executionsLock(subExecutionsMapMutex);
        subExecutionsMap.clear();
        executionsLock.unlock();
        std::unique_lock resultLock(resultMutex);
        result = std::make_shared<armarx::aron::data::Dict>();
        resultLock.unlock();
 
        setStatus(skills::SkillStatus::Preparing);
 
        std::atomic_bool skillRunning = true;
 
        // thread for polling status updates
        std::thread(
            [this, &skillRunning]
            {
                while (skillRunning)
                {
                    this->pollSubStatuses();
                    std::this_thread::sleep_for(
                        std::chrono::milliseconds(pollingFrequency.toMilliSeconds()));
                }
            })
            .detach();
 
        // thread for the sub routines
        const std::string newExecutorName = executorName + "/" + skill.name;
        std::thread(
            [this, &startEdge, &skillRunning, newExecutorName, profilePtr]
            {
                this->startSubRoutine(startEdge.toNodePtr,
                                      startEdge.toParameterPtr,
                                      skillRunning,
                                      newExecutorName,
                                      profilePtr);
            })
            .detach();
 
        setStatus(skills::SkillStatus::Running);
        const bool useTimeout = skill.timeout.isPositive();
        const auto timeStarted = armarx::DateTime::Now();
 
        // if the skill has a timout smaller than the polling frequency of the executor, use the skill´s timeout
        // as pollingfrequency instead to avoid missing the timeout ('overshoot')
        const auto sleepTime =
            useTimeout ? std::min(pollingFrequency, skill.timeout) : pollingFrequency;
 
        // main loop
        while (skillRunning)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(sleepTime.toMilliSeconds()));
 
            if (useTimeout && armarx::DateTime::Now() >= timeStarted + skill.timeout)
            {
                ARMARX_WARNING << "Skill " << skill.name << " timed out.";
                abortSubExecutions();
                setStatus(skills::SkillStatus::Aborted);
 
                // wait for the thread to update the status map one last time
                std::this_thread::sleep_for(
                    std::chrono::milliseconds(pollingFrequency.toMilliSeconds()));
                skillRunning = false;
                return;
            }
 
            std::shared_lock l(statusMutex);
            const auto s = this->status;
            l.unlock();
 
            if (s->status == skills::SkillStatus::Aborted ||
                s->status == skills::SkillStatus::Failed ||
                s->status == skills::SkillStatus::Succeeded)
            {
                // the skill was aborted with the abort method, no need to abort the
                // SubExecutions twice
                if (s->status != skills::SkillStatus::Aborted)
                {
                    abortSubExecutions();
                }
 
                // wait for the thread to update the status map one last time
                std::this_thread::sleep_for(
                    std::chrono::milliseconds(pollingFrequency.toMilliSeconds()));
                skillRunning = false;
                return;
            }
        }
    }
 
    void
    FluxioCompositeExecutor::startSubRoutine(
        const std::experimental::observer_ptr<const skills::FluxioNode>& startNode,
        const std::experimental::observer_ptr<const skills::FluxioParameter>& startParameter,
        std::atomic_bool& running,
        const std::string& executorName,
        const std::experimental::observer_ptr<const FluxioProfile> profilePtr)
    {
        if (!running)
        {
            return;
        }
 
        if (startNode == nullptr)
        {
            ARMARX_WARNING << "Unexpected nullptr";
            setStatus(skills::SkillStatus::Failed);
            return;
        }
 
        if (startNode->nodeType == skills::FluxioNodeType::PARAMETER)
        {
            // cast to parameter node
            const auto& paramNodePtr = std::experimental::make_observer(
                dynamic_cast<const skills::FluxioParameterNode*>(startNode.get()));
            if (paramNodePtr == nullptr)
            {
                ARMARX_WARNING << "Unexpected nullptr. Dynamic cast from FluxioNodePtr to "
                                  "FluxioParameterNodePtr failed.";
                setStatus(skills::SkillStatus::Failed);
                return;
            }
 
            this->handleParameterRoutine(paramNodePtr, running, executorName);
        }
        else if (startNode->nodeType == skills::FluxioNodeType::CONTROL)
        {
            // cast to control node
            const auto& controlNodePtr = std::experimental::make_observer(
                dynamic_cast<const skills::FluxioControlNode*>(startNode.get()));
 
            if (controlNodePtr == nullptr)
            {
                ARMARX_WARNING << "Unexpected nullptr. Dynamic cast from FluxioNodePtr to "
                                  "FluxioControlNodePtr failed.";
                setStatus(skills::SkillStatus::Failed);
                return;
            }
 
            this->handleControlRoutine(
                controlNodePtr, startParameter, running, executorName, profilePtr);
        }
        else if (startNode->nodeType == skills::FluxioNodeType::SUBSKILL)
        {
            // cast to subskill node
            const auto& subSkillNodePtr = std::experimental::make_observer(
                dynamic_cast<const skills::FluxioSubSkillNode*>(startNode.get()));
            if (subSkillNodePtr == nullptr || subSkillNodePtr->skillPtr == nullptr)
            {
                ARMARX_WARNING << "Unexpected nullptr. Dynamic cast from FluxioNodePtr to "
                                  "FluxioSubSkillNodePtr failed.";
                setStatus(skills::SkillStatus::Failed);
                return;
            }
 
            this->handleSubSkillRoutine(subSkillNodePtr, running, executorName, profilePtr);
        }
        else
        {
            ARMARX_WARNING
                << "Unexpected node type '"
                << skills::FluxioNodeTypeToString(startNode->nodeType).value_or("UNKNOWN")
                << "' for Node with id " << startNode->nodeId;
            setStatus(skills::SkillStatus::Failed);
            return;
        }
    }
 
    void
    FluxioCompositeExecutor::handleParameterRoutine(
        std::experimental::observer_ptr<const skills::FluxioParameterNode> parameterNode,
        std::atomic_bool& /*running*/,
        const std::string& /*executorName*/)
    {
        if (parameterNode == nullptr || parameterNode->parameterPtr == nullptr)
        {
            ARMARX_WARNING << "Unexpected nullptr";
            setStatus(skills::SkillStatus::Failed);
            return;
        }
 
        // make sure it is an event parameter
        if (parameterNode->parameterPtr->type->getShortName() != "Object<Event>")
        {
            ARMARX_WARNING << "Unexpected parameter type "
                           << parameterNode->parameterPtr->type->getShortName();
            setStatus(skills::SkillStatus::Failed);
            return;
        }
 
        const auto& skill = this->skill;
        std::list<FluxioEdge> resEdges;
        std::copy_if(skill.edges.begin(),
                     skill.edges.end(),
                     std::back_inserter(resEdges),
                     [](const skills::FluxioEdge& edge)
                     {
                         return (edge.isValid() &&
                                 edge.toNodePtr->nodeType == FluxioNodeType::PARAMETER &&
                                 edge.fromParameterPtr->type->getShortName() != "Object<Event>");
                     });
 
        std::unique_lock l(possibleInputsMutex);
        for (const auto& e : resEdges)
        {
            const auto& fromParam = e.fromParameterPtr;
            const auto& toParam = e.toParameterPtr;
            auto fromNodeId = e.fromNodePtr->nodeId;
            if (e.fromNodePtr->nodeType == FluxioNodeType::PARAMETER)
            {
                fromNodeId = skill.id;
            }
 
            l.unlock();
            armarx::aron::data::VariantPtr value =
                this->getPossibleInputCopy(fromNodeId, fromParam->id);
            l.lock();
 
            this->result->addElement(toParam->id, value);
        }
        l.unlock();
 
        // get the event type and set the status accordingly
        const std::string& eventType = parameterNode->parameterPtr->name;
        if (eventType == "Succeeded")
        {
            setStatus(skills::SkillStatus::Succeeded);
        }
        else if (eventType == "Failed")
        {
            setStatus(skills::SkillStatus::Failed);
        }
        else if (eventType == "Aborted")
        {
            setStatus(skills::SkillStatus::Aborted);
        }
        else
        {
            ARMARX_WARNING << "Unexpected event type " << eventType << " for parameter "
                           << parameterNode->parameterPtr->name;
            setStatus(skills::SkillStatus::Failed);
        }
    }
 
    void
    FluxioCompositeExecutor::handleSubSkillRoutine(
        std::experimental::observer_ptr<const skills::FluxioSubSkillNode> subSkillNode,
        std::atomic_bool& running,
        const std::string& executorName,
        const std::experimental::observer_ptr<const FluxioProfile> profilePtr)
    {
        if (subSkillNode == nullptr || subSkillNode->skillPtr == nullptr)
        {
            ARMARX_WARNING << "Unexpected nullptr";
            setStatus(skills::SkillStatus::Failed);
            return;
        }
 
        // gather parameters for the subskill
        armarx::aron::data::DictPtr params = std::make_shared<armarx::aron::data::Dict>();
 
        std::list<FluxioEdge> paramEdges;
        std::copy_if(skill.edges.begin(),
                     skill.edges.end(),
                     std::back_inserter(paramEdges),
                     [&subSkillNode](const skills::FluxioEdge& edge)
                     {
                         return (edge.isValid() && edge.toNodePtr->nodeId == subSkillNode->nodeId &&
                                 edge.fromParameterPtr->type->getShortName() != "Object<Event>");
                     });
 
        for (const auto& e : paramEdges)
        {
            const auto& fromParam = e.fromParameterPtr;
            const auto& toParam = e.toParameterPtr;
            auto fromNodeId = e.fromNodePtr->nodeId;
            if (e.fromNodePtr->nodeType == FluxioNodeType::PARAMETER)
            {
                fromNodeId = skill.id;
            }
            armarx::aron::data::VariantPtr value = getPossibleInputCopy(fromNodeId, fromParam->id);
            if (value == nullptr)
            {
                ARMARX_WARNING << "Failed to get possible input for parameter " << fromParam->id
                               << " " << fromNodeId;
            }
            params->addElement(toParam->id, value);
        }
 
        // start skill execution
        const auto& executorRes =
            executeFluxioSkill(subSkillNode->skillPtr->id, profilePtr->id, executorName, params);
        if (!executorRes.isSuccess())
        {
            ARMARX_WARNING << "Failed to execute subskill " << subSkillNode->skillPtr->id;
            setStatus(skills::SkillStatus::Failed);
            return;
        }
        auto executorPtr = executorRes.getResult();
        if (executorPtr == nullptr)
        {
            ARMARX_WARNING << "Failed to execute subskill " << subSkillNode->skillPtr->id;
            setStatus(skills::SkillStatus::Failed);
            return;
        }
 
        std::unique_lock executionsLock(subExecutionsMapMutex);
        subExecutionsMap[subSkillNode->nodeId] = executorPtr;
        executionsLock.unlock();
 
        // wait until the skill has finished (or the super skill is finished)
        skills::FluxioSkillStatusUpdate statusUpdate;
        while (running)
        {
            // sleep for a while
            std::this_thread::sleep_for(std::chrono::milliseconds(250));
            executorPtr->getStatusUpdate(); // FIXME: bad design
            const auto& statusUpdateIt = executorPtr->getStatus();
 
            if (!statusUpdateIt.has_value())
            {
                ARMARX_INFO << "No status update from skill " << subSkillNode->skillPtr->name
                            << " yet. Waiting...";
                continue;
            }
 
            statusUpdate = statusUpdateIt.value();
            // did the status change? update statusUpdates list
            std::unique_lock statusMapLock(statusUpdatesMutex);
            const auto& lastUpdate =
                std::find_if(statusUpdates.begin(),
                             statusUpdates.end(),
                             [&subSkillNode](const skills::FluxioSkillStatusUpdate& statusUpdate)
                             { return statusUpdate.subSkillNodeId == subSkillNode->nodeId; });
 
            if (lastUpdate == statusUpdates.end() || lastUpdate->status != statusUpdate.status)
            {
                statusUpdates.push_front({armarx::DateTime::Now(),
                                          executorPtr->id,
                                          subSkillNode->nodeId,
                                          statusUpdate.status});
            }
 
            statusMapLock.unlock();
 
            // check subskill is finished
            if (statusUpdate.status == skills::SkillStatus::Succeeded ||
                statusUpdate.status == skills::SkillStatus::Failed ||
                statusUpdate.status == skills::SkillStatus::Aborted)
            {
                break;
            }
        }
 
        // check if the parent skill is still running
        if (!running)
        {
            return;
        }
 
        std::unique_lock possibleInputsLock(this->possibleInputsMutex);
        this->possibleInputs[subSkillNode->nodeId] = executorPtr->getResultsCopy();
        possibleInputsLock.unlock();
 
        //  check the final skill status get the output event parameter
        const std::string& outputEventName =
            statusUpdate.status == skills::SkillStatus::Succeeded ? "Succeeded"
            : statusUpdate.status == skills::SkillStatus::Failed  ? "Failed"
            : statusUpdate.status == skills::SkillStatus::Aborted ? "Aborted"
                                                                  : "Undefined";
        const auto& outputParam = std::find_if(
            subSkillNode->skillPtr->parameters.begin(),
            subSkillNode->skillPtr->parameters.end(),
            [&outputEventName](const std::pair<std::string, skills::FluxioParameter>& param)
            {
                return (param.second.type->getShortName() == "Object<Event>" &&
                        !param.second.isInput && param.second.name == outputEventName);
            });
 
        if (outputParam == subSkillNode->skillPtr->parameters.end())
        {
            ARMARX_WARNING << "Skill " << subSkillNode->skillPtr->name
                           << " is missing the output event parameter " << outputEventName;
            setStatus(skills::SkillStatus::Failed);
            return;
        }
 
        // find the connected edge
        const auto& edge =
            std::find_if(skill.edges.begin(),
                         skill.edges.end(),
                         [&subSkillNode, &outputParam](const skills::FluxioEdge& edge)
                         {
                             return (edge.fromNodePtr->nodeId == subSkillNode->nodeId &&
                                     edge.fromParameterPtr->id == outputParam->second.id);
                         });
 
        if (edge == skill.edges.end())
        {
            if (outputEventName == "Failed")
            {
                setStatus(skills::SkillStatus::Failed);
                return;
            }
 
            if (outputEventName == "Aborted")
            {
                setStatus(skills::SkillStatus::Aborted);
                return;
            }
 
            ARMARX_WARNING << "Skill " << skill.name
                           << " has no edge connected to the output event parameter "
                           << outputEventName;
            setStatus(skills::SkillStatus::Failed);
            return;
        }
 
        // start new subroutine
        const std::string& nextExecutorName = executorName + "/" + subSkillNode->skillPtr->name;
        startSubRoutine(
            edge->toNodePtr, edge->toParameterPtr, running, nextExecutorName, profilePtr);
    }
 
    void
    FluxioCompositeExecutor::handleControlRoutine(
        std::experimental::observer_ptr<const skills::FluxioControlNode> controlNode,
        const std::experimental::observer_ptr<const skills::FluxioParameter>& startParameter,
        std::atomic_bool& running,
        const std::string& executorName,
        const std::experimental::observer_ptr<const FluxioProfile> profilePtr)
    {
        if (controlNode == nullptr)
        {
            ARMARX_WARNING << "Unexpected nullptr";
            setStatus(skills::SkillStatus::Failed);
            return;
        }
 
        // check the controlType
        if (controlNode->controlType == skills::FluxioControlNodeType::SPLITTER)
        {
            // find connected nodes and store the relevant edges
            std::vector<std::experimental::observer_ptr<const skills::FluxioEdge>> edgePtrs = {};
            for (const auto& [id, param] : controlNode->parametersMap)
            {
                // ignore the input parameters
                if (param.isInput)
                {
                    continue;
                }
 
                for (const auto& edge : skill.edges)
                {
                    if (edge.isValid() && edge.fromNodePtr->nodeId == controlNode->nodeId &&
                        edge.fromParameterPtr->id == id)
                    {
                        edgePtrs.push_back(std::experimental::make_observer(&edge));
                    }
                }
            }
 
            const size_t allParams = edgePtrs.size();
            int param = 1;
 
            // start subroutines in separate threads
            for (std::experimental::observer_ptr<const skills::FluxioEdge> edgePtr : edgePtrs)
            {
                const std::string newExecutorName = executorName + "/Splitter" + "(" +
                                                    std::to_string(param) + "/" +
                                                    std::to_string(allParams) + ")";
                std::thread(
                    [this, edgePtr, &running, newExecutorName, profilePtr]
                    {
                        startSubRoutine(edgePtr->toNodePtr,
                                        edgePtr->toParameterPtr,
                                        running,
                                        newExecutorName,
                                        profilePtr);
                    })
                    .detach();
                param++;
            }
        }
        else if (controlNode->controlType == skills::FluxioControlNodeType::AND_MERGER)
        {
            // check the list of subexecutions for the node id
            std::experimental::observer_ptr<FluxioMergerExecutor> mergerExecutorPtr = nullptr;
            std::unique_lock executionsLock(subExecutionsMapMutex);
            const auto& executorPtr = subExecutionsMap.find(controlNode->nodeId);
            if (executorPtr == subExecutionsMap.end())
            {
                // assemble paramId vector
                std::vector<std::string> paramIds = {};
                for (const auto& [id, param] : controlNode->parametersMap)
                {
                    if (param.isInput)
                    {
                        paramIds.push_back(id);
                    }
                }
 
                // there is no execution for the merger yet, let´s start one
                mergerExecutorPtr = std::experimental::make_observer(
                    dynamic_cast<FluxioMergerExecutor*>(addMergerExecutorToDC(paramIds).get()));
                subExecutionsMap.emplace(controlNode->nodeId, mergerExecutorPtr);
                executionsLock.unlock();
            }
            else
            {
                executionsLock.unlock();
 
                mergerExecutorPtr = std::experimental::make_observer(
                    dynamic_cast<FluxioMergerExecutor*>(executorPtr->second.get()));
            }
 
            if (mergerExecutorPtr == nullptr)
            {
                ARMARX_WARNING << "Unexpected nullptr. Dynamic cast from FluxioExecutorPtr to "
                                  "FluxioMergerExecutorPtr failed.";
                setStatus(skills::SkillStatus::Failed);
                return;
            }
 
            // check in the token
            mergerExecutorPtr->checkInToken(startParameter->id);
 
            if (mergerExecutorPtr->getStatus() != std::nullopt)
            {
                return;
            }
 
            // reuse this thread
            mergerExecutorPtr->run(executorName, nullptr, profilePtr);
 
            const auto& outputParam =
                std::find_if(controlNode->parametersMap.begin(),
                             controlNode->parametersMap.end(),
                             [](const std::pair<std::string, skills::FluxioParameter>& param)
                             { return !param.second.isInput; });
 
            if (outputParam == controlNode->parametersMap.end())
            {
                ARMARX_WARNING << "Control node " << controlNode->nodeId
                               << " has no output parameter";
                setStatus(skills::SkillStatus::Failed);
                return;
            }
 
            // find the connected edge
            const auto& edge =
                std::find_if(skill.edges.begin(),
                             skill.edges.end(),
                             [&controlNode, &outputParam](const skills::FluxioEdge& edge)
                             {
                                 return (edge.fromNodePtr->nodeId == controlNode->nodeId &&
                                         edge.fromParameterPtr->id == outputParam->second.id);
                             });
 
            if (edge == skill.edges.end())
            {
                ARMARX_WARNING
                    << "Skill " << skill.name
                    << " has no edge connected to the output event parameter of the AND merger";
                setStatus(skills::SkillStatus::Failed);
                return;
            }
 
            // start new subroutine
            startSubRoutine(
                edge->toNodePtr, edge->toParameterPtr, running, executorName, profilePtr);
        }
        else if (controlNode->controlType == skills::FluxioControlNodeType::LOOP_REPEAT ||
                 controlNode->controlType == skills::FluxioControlNodeType::LOOP_RETRY)
        {
            if (controlNode->slottedNode == nullptr)
            {
                ARMARX_INFO << "Slot is empty, Can not execute loop node.";
                setStatus(skills::SkillStatus::Failed);
                return;
            }
 
            if (controlNode->slottedNode->nodeType != skills::FluxioNodeType::SUBSKILL)
            {
                ARMARX_INFO << "Slotted node is not a subskill node, can not execute loop node.";
                setStatus(skills::SkillStatus::Failed);
                return;
            }
 
            armarx::aron::data::DictPtr params = std::make_shared<armarx::aron::data::Dict>();
            std::list<FluxioEdge> paramEdges;
            std::copy_if(skill.edges.begin(),
                         skill.edges.end(),
                         std::back_inserter(paramEdges),
                         [&controlNode](const skills::FluxioEdge& edge)
                         {
                             return (edge.isValid() &&
                                     edge.fromParameterPtr->type->getShortName() !=
                                         "Object<Event>" &&
                                     (edge.toNodePtr->nodeId == controlNode->slottedNode->nodeId ||
                                      edge.toNodePtr->nodeId == controlNode->nodeId));
                         });
 
            for (const auto& e : paramEdges)
            {
                const auto& fromParam = e.fromParameterPtr;
                const auto& toParam = e.toParameterPtr;
                const auto& toNode = e.toNodePtr;
 
                if (!e.isValid())
                {
                    // skip
                }
 
                auto fromNodeId = e.fromNodePtr->nodeId;
                if (e.fromNodePtr->nodeType == FluxioNodeType::PARAMETER)
                {
                    fromNodeId = skill.id;
                }
                armarx::aron::data::VariantPtr value =
                    getPossibleInputCopy(fromNodeId, fromParam->id);
                if (value == nullptr)
                {
                    ARMARX_WARNING << "Failed to get possible input for parameter " << fromParam->id
                                   << " " << fromNodeId;
                }
 
                if (toNode->nodeId == controlNode->nodeId &&
                    toParam->name.find("Iterations") != std::string::npos)
                {
                    params->addElement("Iterations", value);
                }
                else
                {
                    params->addElement(toParam->id, value);
                }
            }
 
            const bool isRetry =
                controlNode->controlType == skills::FluxioControlNodeType::LOOP_RETRY;
            auto loopExecutionId = IceUtil::generateUUID();
            const auto& slottedSubskillNodePtr = std::experimental::make_observer(
                dynamic_cast<const skills::FluxioSubSkillNode*>(controlNode->slottedNode.get()));
 
            if (slottedSubskillNodePtr == nullptr)
            {
                ARMARX_INFO << "Unexpected nullptr. Dynamic cast from FluxioNodePtr to "
                               "FluxioSubSkillNodePtr failed.";
                setStatus(skills::SkillStatus::Failed);
                return;
            }
 
            if (slottedSubskillNodePtr->skillPtr == nullptr)
            {
                ARMARX_INFO << "Unexpected nullptr. Subskill node has no skill pointer.";
                setStatus(skills::SkillStatus::Failed);
                return;
            }
 
            std::shared_lock executionsLock(subExecutionsMapMutex);
            const auto& loopExecutorPtr =
                std::experimental::make_observer(dynamic_cast<FluxioLoopExecutor*>(
                    addLoopExecutorToDC(loopExecutionId, *slottedSubskillNodePtr->skillPtr, isRetry)
                        .get()));
            subExecutionsMap.emplace(controlNode->nodeId, loopExecutorPtr);
            executionsLock.unlock();
 
            std::thread([&] { loopExecutorPtr->run(executorName, params, profilePtr); }).detach();
 
            skills::FluxioSkillStatusUpdate statusUpdate;
            skills::FluxioSkillStatusUpdate slottedStatusUpdate;
            while (running)
            {
                // sleep for a while
                std::this_thread::sleep_for(std::chrono::milliseconds(250));
                loopExecutorPtr->getStatusUpdate();
 
                const auto& statusUpdateOpt = loopExecutorPtr->getStatus();
                if (!statusUpdateOpt.has_value())
                {
                    ARMARX_INFO << "No status update from loop execution yet. Waiting...";
                    continue;
                }
                statusUpdate = statusUpdateOpt.value();
 
                // did the status change? update statusUpdates list
                std::unique_lock statusMapLock(statusUpdatesMutex);
                const auto& lastUpdate =
                    std::find_if(statusUpdates.begin(),
                                 statusUpdates.end(),
                                 [&controlNode](const skills::FluxioSkillStatusUpdate& statusUpdate)
                                 { return statusUpdate.subSkillNodeId == controlNode->nodeId; });
 
                if (lastUpdate == statusUpdates.end() || lastUpdate->status != statusUpdate.status)
                {
                    statusUpdates.push_front({armarx::DateTime::Now(),
                                              loopExecutorPtr->id,
                                              controlNode->nodeId,
                                              statusUpdate.status});
                }
 
                const auto& slottedStatusUpdateOpt = loopExecutorPtr->getSlottedStatus();
                const auto& currentSlottedExecutionId = loopExecutorPtr->slottedExecutionId;
                if (!slottedStatusUpdateOpt.has_value() || !currentSlottedExecutionId.has_value())
                {
                    if (statusUpdate.status == skills::SkillStatus::Failed ||
                        statusUpdate.status == skills::SkillStatus::Aborted)
                    {
                        break;
                    }
 
                    // ARMARX_INFO << "No status update from slotted subskill yet. Waiting...";
                    continue;
                }
                slottedStatusUpdate = slottedStatusUpdateOpt.value();
 
                const auto& lastSlottedUpdate = std::find_if(
                    statusUpdates.begin(),
                    statusUpdates.end(),
                    [&controlNode](const skills::FluxioSkillStatusUpdate& statusUpdate)
                    { return statusUpdate.subSkillNodeId == controlNode->slottedNode->nodeId; });
 
                if (lastSlottedUpdate == statusUpdates.end() ||
                    lastSlottedUpdate->status != statusUpdate.status ||
                    slottedStatusUpdate.executionId != currentSlottedExecutionId.value())
                {
                    statusUpdates.push_front({armarx::DateTime::Now(),
                                              currentSlottedExecutionId.value(),
                                              controlNode->slottedNode->nodeId,
                                              slottedStatusUpdate.status});
                }
 
                statusMapLock.unlock();
 
                // check loop is finished
                if (statusUpdate.status == skills::SkillStatus::Succeeded ||
                    statusUpdate.status == skills::SkillStatus::Failed ||
                    statusUpdate.status == skills::SkillStatus::Aborted)
                {
                    break;
                }
            }
 
            if (!running)
            {
                return;
            }
 
            std::unique_lock possibleInputsLock(this->possibleInputsMutex);
            this->possibleInputs[controlNode->slottedNode->nodeId] =
                loopExecutorPtr->getResultsCopy();
            possibleInputsLock.unlock();
 
            const std::string& outputEventName =
                statusUpdate.status == skills::SkillStatus::Succeeded ? "Succeeded"
                : statusUpdate.status == skills::SkillStatus::Failed  ? "Failed"
                : statusUpdate.status == skills::SkillStatus::Aborted ? "Aborted"
                                                                      : "Undefined";
 
            const auto& outputParam = std::find_if(
                controlNode->parametersMap.begin(),
                controlNode->parametersMap.end(),
                [&outputEventName](const std::pair<std::string, skills::FluxioParameter>& param)
                {
                    return (param.second.type->getShortName() == "Object<Event>" &&
                            !param.second.isInput && param.second.name == outputEventName);
                });
 
            if (outputParam == controlNode->parametersMap.end())
            {
                ARMARX_WARNING << "Loop with nodeId " << controlNode->nodeId
                               << " is missing the output event parameter " << outputEventName;
                setStatus(skills::SkillStatus::Failed);
                return;
            }
 
            // find the connected edge
            const auto& edge =
                std::find_if(skill.edges.begin(),
                             skill.edges.end(),
                             [&controlNode, &outputParam](const skills::FluxioEdge& edge)
                             {
                                 return (edge.fromNodePtr->nodeId == controlNode->nodeId &&
                                         edge.fromParameterPtr->id == outputParam->second.id);
                             });
 
            if (edge == skill.edges.end())
            {
                if (outputEventName == "Failed")
                {
                    setStatus(skills::SkillStatus::Failed);
                    return;
                }
 
                if (outputEventName == "Aborted")
                {
                    setStatus(skills::SkillStatus::Aborted);
                    return;
                }
 
                ARMARX_WARNING << "Skill " << skill.name
                               << " has no edge connected to the output event parameter "
                               << outputEventName;
                setStatus(skills::SkillStatus::Failed);
                return;
            }
 
            // start new subroutine
            const std::string& nextExecutorName = executorName + "/Loop";
            startSubRoutine(
                edge->toNodePtr, edge->toParameterPtr, running, nextExecutorName, profilePtr);
        }
 
        else
        {
            ARMARX_WARNING << "Unexpected control type ";
            setStatus(skills::SkillStatus::Failed);
            return;
        }
    }
 
    void
    FluxioCompositeExecutor::abort()
    {
        ARMARX_INFO << "Aborting skill " << skill.name;
        setStatus(skills::SkillStatus::Aborted);
        abortSubExecutions();
    }
 
    void
    FluxioCompositeExecutor::abortSubExecutions()
    {
        std::shared_lock executionsLock(subExecutionsMapMutex);
        for (const auto& [nodeId, executorPtr] : subExecutionsMap)
        {
            auto s = executorPtr->getStatus();
            if (!s.has_value() || s->status == skills::SkillStatus::Succeeded ||
                s->status == skills::SkillStatus::Failed ||
                s->status == skills::SkillStatus::Aborted)
            {
                continue;
            }
 
            executorPtr->abort();
            std::unique_lock statusMapLock(statusUpdatesMutex);
            statusUpdates.push_front(
                {armarx::DateTime::Now(), executorPtr->id, nodeId, skills::SkillStatus::Aborted});
            statusMapLock.unlock();
        }
        executionsLock.unlock();
    }
 
    std::optional<std::vector<skills::FluxioSkillStatusUpdate>>
    FluxioCompositeExecutor::getStatusUpdate()
    {
        ARMARX_INFO << "Getting status updates for skill " << skill.name;
        // convert statusupdates list to vector
        std::shared_lock statusMapLock(statusUpdatesMutex);
        auto ret = std::vector<skills::FluxioSkillStatusUpdate>(statusUpdates.begin(),
                                                                statusUpdates.end());
        statusMapLock.unlock();
        return ret;
    }
 
    void
    FluxioCompositeExecutor::pollSubStatuses()
    {
        std::scoped_lock l(subExecutionsMapMutex, statusUpdatesMutex);
        for (const auto& [nodeId, executorPtr] : subExecutionsMap)
        {
            executorPtr->getStatusUpdate();
            auto s = executorPtr->getStatus();
            if (!s.has_value())
            {
                continue;
            }
 
            const auto& lastStatus =
                find_if(statusUpdates.begin(),
                        statusUpdates.end(),
                        [&](const skills::FluxioSkillStatusUpdate& statusUpdate)
                        { return statusUpdate.subSkillNodeId == nodeId; })
                    ->status;
 
            if (lastStatus != s->status)
            {
                statusUpdates.push_front(
                    {armarx::DateTime::Now(), executorPtr->id, nodeId, s->status});
            }
        }
    }
 
    bool
    FluxioCompositeExecutor::validateSkill(skills::FluxioEdge& ret) const
    {
 
        // get start parameter
        const auto& startParam =
            std::find_if(skill.parameters.begin(),
                         skill.parameters.end(),
                         [](const std::pair<std::string, skills::FluxioParameter>& param)
                         {
                             return (param.second.type->getShortName() == "Object<Event>" &&
                                     param.second.isInput && param.second.name == "Start");
                         });
 
        if (startParam == skill.parameters.end())
        {
            ARMARX_WARNING << "Skill has no start parameter";
            return false;
        }
 
        // get all parameter nodes for the start parameter
        const auto& startNode = std::find_if(
            skill.nodes.begin(),
            skill.nodes.end(),
            [startParam](const std::pair<const std::string,
                                         const std::unique_ptr<skills::FluxioNode>>& nodeEntry)
            {
                if (nodeEntry.second->nodeType != skills::FluxioNodeType::PARAMETER)
                {
                    return false;
                }
 
                const auto& paramNode =
                    dynamic_cast<const skills::FluxioParameterNode*>(nodeEntry.second.get());
                return (paramNode->parameterPtr->id == startParam->second.id);
            });
 
        // there can only be one
        if (startNode == skill.nodes.end())
        {
            ARMARX_WARNING << "Skill has no start node";
            return false;
        }
 
        // check if the start node is connected
        const auto& startEdge =
            std::find_if(skill.edges.begin(),
                         skill.edges.end(),
                         [startNode](const skills::FluxioEdge& edge)
                         { return (edge.fromNodePtr->nodeId == startNode->second->nodeId); });
 
        // there can only be one
        if (startEdge == skill.edges.end())
        {
            ARMARX_WARNING << "Skill has no edge connected to the start node";
            return false;
        }
 
        // get the output event parameters
        const auto& outputParamsSuccess =
            std::find_if(skill.parameters.begin(),
                         skill.parameters.end(),
                         [](const std::pair<std::string, skills::FluxioParameter>& param)
                         {
                             return (param.second.type->getShortName() == "Object<Event>" &&
                                     !param.second.isInput && param.second.name == "Succeeded");
                         });
        const auto& outputParamsFailed =
            std::find_if(skill.parameters.begin(),
                         skill.parameters.end(),
                         [](const std::pair<std::string, skills::FluxioParameter>& param)
                         {
                             return (param.second.type->getShortName() == "Object<Event>" &&
                                     !param.second.isInput && param.second.name == "Failed");
                         });
        const auto& outputParamsAborted =
            std::find_if(skill.parameters.begin(),
                         skill.parameters.end(),
                         [](const std::pair<std::string, skills::FluxioParameter>& param)
                         {
                             return (param.second.type->getShortName() == "Object<Event>" &&
                                     !param.second.isInput && param.second.name == "Aborted");
                         });
 
        if (outputParamsSuccess == skill.parameters.end() ||
            outputParamsFailed == skill.parameters.end() ||
            outputParamsAborted == skill.parameters.end())
        {
            ARMARX_WARNING << "Skill is missing one or more output event parameters";
            return false;
        }
 
        // TODO: the rest
 
        ARMARX_INFO << "Skill validation is not fully implemented yet.";
        ret = *startEdge;
        return true;
    }
 
    void
    FluxioCompositeExecutor::setStatus(skills::SkillStatus status, const std::string& /*nodeId*/)
    {
        FluxioExecutor::setStatus(status, skill.id);
    }
} // namespace armarx::skills