FluxioLoopExecutor.cpp

Go to the documentation of this file.

#include "FluxioLoopExecutor.h"
 
#include <algorithm>
#include <chrono>
#include <experimental/memory>
#include <functional>
#include <memory>
#include <mutex>
#include <optional>
#include <shared_mutex>
#include <string>
#include <thread>
#include <vector>
 
#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/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/SkillStatusUpdate.h"
#include "RobotAPI/libraries/skills/core/error/FluxioException.h"
#include "RobotAPI/libraries/skills/core/executor/FluxioExecutor.h"
 
namespace armarx::skills
{
    FluxioLoopExecutor::FluxioLoopExecutor(
        const std::string& id,
        const skills::FluxioSkill& skill,
        bool isRetry,
        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) :
        FluxioExecutor(id, false),
        executeFluxioSkill(executeFluxioSkillFunc),
        skill(skill),
        isRetry(isRetry)
    {
        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
    FluxioLoopExecutor::run(std::string executorName,
                            armarx::aron::data::DictPtr parameters,
                            std::experimental::observer_ptr<const FluxioProfile> profilePtr)
    {
        ARMARX_INFO << "Running loop with slotted skill " << skill.name;
        setStatus(skills::SkillStatus::Constructing);
        this->executorName = executorName;
        this->slottedExecutionId = std::nullopt;
 
        // get loop parameter
        int iterations = -1; // default value
        bool infiniteLoop = false;
        if (!parameters->hasElement("Iterations"))
        {
            if (isRetry)
            {
                ARMARX_INFO << "max Iterations not set, using default (infinite)";
                infiniteLoop = true;
            }
            else
            {
                ARMARX_INFO << "Missing 'Iterations' parameter for loop node.";
                setStatus(SkillStatus::Failed);
                return;
            }
        }
        else
        {
            // retrieve iterations value
            const auto& valuePtr =
                armarx::aron::data::Int::DynamicCastAndCheck(parameters->getElement("Iterations"));
            if (valuePtr == nullptr)
            {
                ARMARX_INFO << "Invalid 'Iterations' parameter for loop node.";
                setStatus(SkillStatus::Failed);
                return;
            }
            iterations = valuePtr->getValue();
        }
 
        // gather parameters for the slotted skill
        armarx::aron::data::DictPtr slottedSkillParams =
            std::make_shared<armarx::aron::data::Dict>();
        for (const auto& paramIt : skill.parameters)
        {
            const auto& paramId = paramIt.first;
            if (!parameters->hasElement(paramId))
            {
                ARMARX_WARNING << "Failed to get possible input for parameter " << paramId;
                // slottedSkillParams->addElement(paramId, nullptr);
            }
            else
            {
                const auto& value = parameters->getElement(paramId);
                slottedSkillParams->addElement(paramId, value->cloneAsVariant());
            }
        }
 
        setStatus(skills::SkillStatus::Initializing);
 
 
        setStatus(skills::SkillStatus::Preparing);
        const std::string newExecutorName =
            executorName + "/Loop (" + (isRetry ? "Retry" : "Repeat") + ")";
        bool loopRunning = true;
        bool allIterationsSucceeded = true;
        loopIndex = 0;
 
        // main loop
        setStatus(skills::SkillStatus::Running);
        while (loopRunning)
        {
            const auto& ret =
                executeSlottedSkill(profilePtr->id, newExecutorName, slottedSkillParams);
 
            if (!ret.has_value())
            {
                ARMARX_INFO << "Failed to execute slotted skill " << skill.name;
                setStatus(SkillStatus::Failed);
                return;
            }
 
            const auto& skillStatus = ret->status;
 
            if (skillStatus == SkillStatus::Succeeded && isRetry)
            {
                setStatus(SkillStatus::Succeeded);
                loopRunning = false;
                break;
            }
 
            if (skillStatus == SkillStatus::Aborted)
            {
                setStatus(SkillStatus::Aborted);
                return;
            }
 
            if (skillStatus == SkillStatus::Failed)
            {
                allIterationsSucceeded = false;
            }
 
            if (!infiniteLoop && loopIndex >= iterations - 1)
            {
                if (isRetry)
                {
                    setStatus(SkillStatus::Failed);
                }
                else
                {
                    setStatus(allIterationsSucceeded ? SkillStatus::Succeeded
                                                     : SkillStatus::Failed);
                }
                loopRunning = false;
                break;
            }
 
            loopIndex += 1;
        }
 
        const auto& res = subExecutionsMap.find(std::to_string(loopIndex));
        if (res == subExecutionsMap.end() || res->second == nullptr)
        {
            setStatus(SkillStatus::Failed);
            return;
        }
        const auto& resultsCopy = res->second->getResultsCopy();
        if (resultsCopy != nullptr)
        {
            for (const auto& [k, v] : resultsCopy->getElements())
            {
                result->setElementCopy(k, v);
            }
        }
    }
 
    std::optional<FluxioSkillStatusUpdate>
    FluxioLoopExecutor::executeSlottedSkill(const std::string& profileId,
                                            const std::string& executorName,
                                            armarx::aron::data::DictPtr& parameters)
    {
        const std::string loopIndexStr = std::to_string(loopIndex);
 
        // start slotted skill execution
        const auto& executorRes = executeFluxioSkill(skill.id, profileId, executorName, parameters);
        if (!executorRes.isSuccess())
        {
            ARMARX_INFO << "Failed to execute slotted skill " << skill.name;
            return std::nullopt;
        }
        auto executorPtr = executorRes.getResult();
        if (executorPtr == nullptr)
        {
            ARMARX_INFO << "Failed to get executor pointer";
            return std::nullopt;
        }
 
        slottedExecutionId = executorPtr->id;
        std::unique_lock executionsLock(subExecutionsMapMutex);
        subExecutionsMap[loopIndexStr] = executorPtr;
        executionsLock.unlock();
        bool skillRunning = true;
        skills::FluxioSkillStatusUpdate statusUpdate;
        while (skillRunning)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(250));
            executorPtr->getStatusUpdate(); // FIXME: bad design (use call-backs instead)
            const auto& statusUpdateIt = executorPtr->getStatus();
 
            if (!statusUpdateIt.has_value())
            {
                ARMARX_INFO << "No status update from slotted skill " << skill.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(),
                             [&loopIndexStr](const skills::FluxioSkillStatusUpdate& statusUpdate)
                             { return statusUpdate.subSkillNodeId == loopIndexStr; });
 
            if (lastUpdate == statusUpdates.end() || lastUpdate->status != statusUpdate.status)
            {
                statusUpdates.push_front(
                    {armarx::DateTime::Now(), executorPtr->id, loopIndexStr, statusUpdate.status});
            }
 
            statusMapLock.unlock();
 
            // check subskill is finished
            if (statusUpdate.status == skills::SkillStatus::Succeeded ||
                statusUpdate.status == skills::SkillStatus::Failed ||
                statusUpdate.status == skills::SkillStatus::Aborted)
            {
                skillRunning = false;
                break;
            }
        }
        return statusUpdate;
    }
 
    void
    FluxioLoopExecutor::abort()
    {
        ARMARX_INFO << "Aborting loop ";
        setStatus(skills::SkillStatus::Aborted);
        abortSubExecutions();
    }
 
    std::optional<std::vector<skills::FluxioSkillStatusUpdate>>
    FluxioLoopExecutor::getStatusUpdate()
    {
        // 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
    FluxioLoopExecutor::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<skills::FluxioSkillStatusUpdate>
    FluxioLoopExecutor::getSlottedStatus()
    {
        std::scoped_lock l(subExecutionsMapMutex);
 
        const auto& loopIndexStr = std::to_string(loopIndex);
        const auto& ret = subExecutionsMap.find(loopIndexStr);
        if (ret == subExecutionsMap.end() || ret->second == nullptr)
        {
            return std::nullopt;
        }
 
        return ret->second->getStatus();
    }
} // namespace armarx::skills