FluxioNativeExecutor.cpp

Go to the documentation of this file.

#include "FluxioNativeExecutor.h"
 
#include <experimental/memory>
#include <memory>
#include <mutex>
#include <utility>
#include <vector>
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include "RobotAPI/libraries/aron/core/data/variant/container/Dict.h"
#include "RobotAPI/libraries/aron/core/type/variant/container/Object.h"
#include "RobotAPI/libraries/skills/core/FluxioParameter.h"
#include "RobotAPI/libraries/skills/core/SkillStatusUpdate.h"
#include "RobotAPI/libraries/skills/core/executor/FluxioExecutor.h"
#include <RobotAPI/libraries/aron/converter/json/NLohmannJSONConverter.h>
 
#include "../ProviderID.h"
#include "../SkillExecutionRequest.h"
#include "../SkillID.h"
 
namespace armarx::skills
{
    FluxioNativeExecutor::FluxioNativeExecutor(
        const std::string& id,
        const skills::SkillID& skillId,
        const FluxioSkill& fluxioSkill,
        const std::function<bool(const skills::SkillExecutionID&)>&& abortSkillFunc,
        const std::function<skills::SkillExecutionID(const skills::SkillExecutionRequest&)>&&
            executeSkillAsyncFunc,
        const std::function<std::optional<skills::SkillStatusUpdate>(
            const skills::SkillExecutionID&)>&& getSkillExecutionStatusFunc) :
        FluxioExecutor(id, true),
        skillId(skillId),
        fluxioSkill(fluxioSkill),
        abortSkill(abortSkillFunc),
        executeSkillAsync(executeSkillAsyncFunc),
        getSkillExecutionStatus(getSkillExecutionStatusFunc)
    {
        std::scoped_lock resultLock(resultMutex);
        result = std::make_shared<armarx::aron::data::Dict>();
    }
 
    void
    FluxioNativeExecutor::run(const std::string executorName,
                              armarx::aron::data::DictPtr parameters,
                              std::experimental::observer_ptr<const FluxioProfile> profilePtr)
    {
        // reset results
        std::unique_lock resultLock(resultMutex);
        result = std::make_shared<armarx::aron::data::Dict>();
        resultLock.unlock();
 
        std::unique_lock l(possibleInputsMutex);
        this->possibleInputs[this->fluxioSkill.id] = parameters;
 
        const auto& type = std::make_shared<armarx::aron::type::Object>();
 
        // fill in missing with default values
        for (const auto& [key, param] : this->fluxioSkill.parameters)
        {
            if (param.type->getShortName() == "Object<Event>" || !param.isInput ||
                this->possibleInputs[this->fluxioSkill.id]->hasElement(key))
            {
                continue;
            }
            const auto& fallBack = 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 (MAYBE type). 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->fluxioSkill.id]->addElement(key, fallBack);
 
            type->addMemberType(param.name, param.type);
        }
 
        const auto& params = std::make_shared<armarx::aron::data::Dict>();
 
        for (const auto& [key, value] : this->possibleInputs[this->fluxioSkill.id]->getElements())
        {
            const auto& paramIt = this->fluxioSkill.parameters.find(key);
            if (paramIt == this->fluxioSkill.parameters.end())
            {
                ARMARX_WARNING << "Parameter with ID " << key << " not found in skill "
                               << this->fluxioSkill.id;
                ARMARX_WARNING << "Can't execute skill due to incomplete params.";
                this->setStatus(skills::SkillStatus::Failed);
                return;
            }
            if (value == nullptr && paramIt->second.required)
            {
                ARMARX_INFO << "Value for parameter " << paramIt->second.name << " is null.";
                continue;
            }
            params->addElement(paramIt->second.name, value);
        }
        l.unlock();
 
        const auto& isValid = params->fullfillsType(type);
 
        if (!isValid)
        {
            ARMARX_WARNING << "Parameters do not fullfill the type of the skill";
            ARMARX_WARNING << "Can't execute skill due to incompatible params.";
            this->setStatus(skills::SkillStatus::Failed);
            return;
        }
 
        skills::SkillExecutionRequest req;
        req.skillId = skillId;
        req.parameters = params;
        req.executorName = executorName;
        this->executorName = executorName;
 
        ARMARX_WARNING << "Executing skill with the following parameters: "
                       << aron::data::converter::AronNlohmannJSONConverter::ConvertToNlohmannJSON(
                              req.parameters);
        auto eid = executeSkillAsync(req);
 
        this->executionId = eid;
    }
 
    void
    FluxioNativeExecutor::abort()
    {
        std::scoped_lock l(statusMutex);
        if (status->status == skills::SkillStatus::Aborted ||
            status->status == skills::SkillStatus::Succeeded ||
            status->status == skills::SkillStatus::Failed)
        {
            // already terminated
            return;
        }
 
        if (!this->executionId.has_value())
        {
            // error
            ARMARX_WARNING << "Execution ID not set. Cannot abort skill (it should not be running "
                              "at this point).";
            return;
        }
 
        abortSkill(this->executionId.value());
    }
 
    std::optional<std::vector<skills::FluxioSkillStatusUpdate>>
    FluxioNativeExecutor::getStatusUpdate()
    {
        if (!this->executionId.has_value())
        {
            // error
            ARMARX_WARNING << "Execution ID or plugin is not set";
            return std::vector<skills::FluxioSkillStatusUpdate>(this->statusUpdates.begin(),
                                                                this->statusUpdates.end());
        }
 
        auto executionId = this->executionId.value();
 
        auto status = getSkillExecutionStatus(executionId);
 
        if (!status.has_value())
        {
            // error
            ARMARX_WARNING << "No status with execution ID " << this->executionId->toString()
                           << " found";
            return std::nullopt;
        }
 
        skills::FluxioSkillStatusUpdate update = {
            armarx::DateTime::Now(), this->id, this->fluxioSkill.id, status.value().status};
 
        // newest statusupdate is aways at the end of the vector
        if (this->statusUpdates.empty() || update.status != this->statusUpdates.front().status)
        {
            this->setStatus(update.status);
        }
 
        return std::vector<skills::FluxioSkillStatusUpdate>(this->statusUpdates.begin(),
                                                            this->statusUpdates.end());
    }
 
    void
    FluxioNativeExecutor::setStatus(skills::SkillStatus status, const std::string& /*nodeId*/)
    {
        FluxioExecutor::setStatus(status, this->fluxioSkill.id);
    }
 
    armarx::aron::data::DictPtr
    FluxioNativeExecutor::getResultsCopy()
    {
        // grab latest status update and check if it contains a result
        if (!executionId.has_value())
        {
            ARMARX_WARNING << "No execution ID available for skill " << this->skillId;
            return nullptr;
        }
        std::scoped_lock l(resultMutex);
        const auto& s = getSkillExecutionStatus(executionId.value());
        if (!s.has_value() || s->result == nullptr)
        {
            ARMARX_WARNING << "No result available for skill " << this->skillId;
            return nullptr;
        }
 
        for (const auto& [key, value] : s->result->getElements())
        {
            // get param by name
            const auto& params = this->fluxioSkill.parameters;
            const auto& param =
                std::find_if(params.begin(),
                             params.end(),
                             [&key](const std::pair<std::string, FluxioParameter>& it)
                             { return !it.second.isInput && it.second.name == key; });
 
            if (param == params.end())
            {
                ARMARX_WARNING << "Parameter with name " << key << " not found in skill "
                               << this->skillId;
                return nullptr;
            }
            this->result->addElement(param->second.id, value);
        }
 
        return this->result->clone();
    }
} // namespace armarx::skills