SkillImplementationWrapper.cpp

Go to the documentation of this file.

#include "SkillImplementationWrapper.h"
 
namespace armarx
{
    namespace skills::detail
    {
        SkillImplementationWrapper::SkillImplementationWrapper(std::unique_ptr<skills::Skill>&& skill) :
            skill(std::move(skill))
        {
            ARMARX_CHECK_NOT_NULL(this->skill);
        }
 
        TerminatedSkillStatusUpdate SkillImplementationWrapper::setupAndExecuteSkill(const std::string& executorName, const skills::SkillParameterization parameterization)
        {
            std::unique_lock l(executingMutex);
 
            const std::string skillName = skill->description.skillName;
            ARMARX_INFO_S << "Executing skill: " << skillName;
 
            // reset execution params. This func is also used to clean up once this method returns
            auto resetExecParam = [&](){
                std::unique_lock l2(skillStatusMutex); // skill is not updating
                //statusUpdate.status = skills::provider::dto::Execution::Status::Idle; I decided to not update the status to idle every time the skill stops.
                statusUpdate.data = nullptr;
                statusUpdate.executorName = "";
            };
 
            // set params and setup variables
            auto setExecParams = [&](){
                std::lock_guard l(skillStatusMutex);
                statusUpdate.usedParameterization = parameterization;
                statusUpdate.executorName = executorName;
            };
 
            resetExecParam();
            setExecParams();
 
            auto& aron_params = parameterization.usedInputParams;
            auto updateStatus = [&](const SkillStatus status, const aron::data::DictPtr& data = nullptr){
                std::lock_guard l(skillStatusMutex);
                statusUpdate.status = status;
                statusUpdate.data = data;
 
                auto& callbackInterface = statusUpdate.usedParameterization.usedCallbackInterface;
 
                if (callbackInterface)
                {
                    callbackInterface->updateStatusForSkill(statusUpdate.toIce());
                }
            };
 
            auto createErrorMessage = [](const std::string& message){
                auto obj = aron::make_dict();
                auto m = aron::make_string(message, aron::Path({"errormessage"}));
                obj->addElement("errormessage", m);
                return obj;
            };
 
            // Check params
            if (skill->description.acceptedType && not(aron_params))
            {
                std::string message = "SkillError 001: The Skill '" + skillName + "' requires a type but params are NULL.";
                ARMARX_ERROR_S << message;
                resetExecParam();
                return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
            }
 
            if (skill->description.acceptedType && aron_params && not(aron_params->fullfillsType(skill->description.acceptedType)))
            {
                std::string message = "SkillError 002: The Skill '" + skillName + "' has a type and got parameters but the input does not match the type.";
                ARMARX_ERROR_S << message;
                resetExecParam();
                return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
            }
 
            // Check if skill is available with the given parameterization
            try
            {
                if (not skill->isSkillAvailable(Skill::InitInput{.executorName = executorName, .params = aron_params}))
                {
                    std::string message = "SkillError 101: The Skill '" + skillName + "' is not available.";
                    ARMARX_WARNING << message;
                    resetExecParam();
                    return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
                }
            }
            catch (const std::exception& ex)
            {
                std::string message = "SkillError 101e: An error occured during the check whether skill '" + skillName + "' is available. The error was: " + GetHandledExceptionString();
                ARMARX_ERROR_S << message;
                resetExecParam();
                return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
            }
 
            // set scheduled
            updateStatus(SkillStatus::Scheduled);
 
            // reset skill and perhaps wait for dependencies
            try
            {
                skill->resetSkill();
            }
            catch (const std::exception& ex)
            {
                std::string message = "SkillError 201e: An error occured during the reset of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
                ARMARX_ERROR_S << message;
 
                updateStatus(SkillStatus::Failed);
                resetExecParam();
                return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
            }
 
            try
            {
                skill->waitForDependenciesOfSkill();
            }
            catch (const std::exception& ex)
            {
                std::string message = "SkillError 301e: An error occured during waiting for skill dependencies of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
                ARMARX_ERROR_S << message;
 
                updateStatus(SkillStatus::Failed);
                resetExecParam();
                return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
            }
 
            // execute. If the skill fails for some reason, from this point it will always execute its exit function.
            updateStatus(SkillStatus::Running);
 
 
            try
            {
                Skill::InitResult initRet = skill->initSkill({executorName, aron_params});
                if (initRet.status != TerminatedSkillStatus::Succeeded)
                {
                    std::string message = "SkillError 401: The initialization of skill '" + skillName + "' did not succeed.";
                    skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
 
                    updateStatus(skills::toSkillStatus(initRet.status));
                    resetExecParam();
                    return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, initRet.status});
                }
            }
            catch (const std::exception& ex)
            {
                std::string message = "SkillError 401e: An error occured during the initialization of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
                ARMARX_ERROR_S << message;
                skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
 
                updateStatus(SkillStatus::Failed);
                resetExecParam();
                return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
            }
            // Init succeeded!
 
 
            Skill::MainResult mainRet;
            try
            {
                mainRet = skill->mainOfSkill({executorName, aron_params, [&updateStatus](const aron::data::DictPtr& update)
                {
                    // during execution the statusUpdate.status is always RUNNING
                    updateStatus(SkillStatus::Running, update);
                }});
                if (mainRet.status != TerminatedSkillStatus::Succeeded)
                {
                    std::string message = "SkillError 501: The main method of skill '" + skillName + "' did not succeed.";
                    skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
 
                    updateStatus(skills::toSkillStatus(mainRet.status));
                    resetExecParam();
                    return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, mainRet.status});
                }
            }
            catch (const std::exception& ex)
            {
                std::string message = "SkillError 501e: An error occured during the main method of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
                ARMARX_ERROR_S << message;
                skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
 
                updateStatus(SkillStatus::Failed);
                resetExecParam();
                return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
            }
            // Main succeeded!
 
            try
            {
                Skill::ExitResult exitRet = skill->exitSkill({executorName, aron_params});
                if (exitRet.status != TerminatedSkillStatus::Succeeded)
                {
                    std::string message = "SkillError 601: The exit method of skill '" + skillName + "' did not succeed.";
                    skill->exitSkill({executorName, aron_params}); // try to exit skill. Ignore return value
 
                    updateStatus(skills::toSkillStatus(exitRet.status));
                    resetExecParam();
                    return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, exitRet.status});
                }
            }
            catch (const std::exception& ex)
            {
                std::string message = "SkillError 601e: An error occured during the exit method of skill '" + skillName + "'. The error was: " + GetHandledExceptionString();
                ARMARX_ERROR_S << message;
 
                updateStatus(SkillStatus::Failed);
                resetExecParam();
                return TerminatedSkillStatusUpdate({{skill->getSkillId(), executorName, parameterization, createErrorMessage(message)}, TerminatedSkillStatus::Failed});
            }
            // Exit succeeded!
 
 
            // All succeeded!
            updateStatus(SkillStatus::Succeeded);
 
            // Tidy up
            resetExecParam();
 
            // return result of main method
            return {{skill->getSkillId(), executorName, parameterization, mainRet.data}, TerminatedSkillStatus::Succeeded};
        }
    }
}