SkillImplementationWrapper.cpp

Go to the documentation of this file.

#include "SkillImplementationWrapper.h"
 
#include <chrono>
#include <exception>
#include <mutex>
#include <string>
#include <thread>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
 
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/interface/skills/SkillProviderInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/skills/core/Skill.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionID.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/core/aron/SkillErrorResult.aron.generated.h>
#include <RobotAPI/libraries/skills/core/error/Exception.h>
#include <RobotAPI/libraries/skills/provider/SkillFactory.h>
 
namespace armarx
{
    namespace skills::detail
    {
        SkillRuntime::SkillRuntime(
            const skills::SkillBlueprint* fac,
            const skills::SkillExecutionID& execId,
            const aron::data::DictPtr& initial_parameters,
            const skills::callback::dti::SkillProviderCallbackInterfacePrx& callbackInterface) :
            factory(fac), statusUpdate{{execId, initial_parameters, callbackInterface}}
        {
            ;
        }
 
        // ask a skill to stop
        void
        SkillRuntime::stopSkill()
        {
            while (this->skill == nullptr)
            {
                ARMARX_INFO << deactivateSpam()
                            << "Trying to stop a skill that has not been constructed yet... "
                               "Waiting until construction...";
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
            }
 
            this->skill->notifySkillToStop();
        }
 
        void
        SkillRuntime::updateSkillParameters(const aron::data::DictPtr& i)
        {
            while (this->skill == nullptr)
            {
                ARMARX_INFO << deactivateSpam()
                            << "Trying to add params to a skill that has not been constructed "
                               "yet... Waiting until construction...";
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
            }
 
            this->skill->updateParameters(i);
        }
 
        TerminatedSkillStatusUpdate
        SkillRuntime::executeSkill()
        {
            std::scoped_lock l(this->executionMutex);
            // -------------------------------------------------------------------------------------
            // sanity check
            // -------------------------------------------------------------------------------------
            ARMARX_CHECK(this->skill == nullptr)
                << "A SKILL HAS ALREADY BEEN CONSTRUCTED. THIS SHOULD NOT HAPPEN!!";
 
            // -------------------------------------------------------------------------------------
            // setup basic vars and lambdas
            // -------------------------------------------------------------------------------------
            // actually we should lock... however this is only read access and the members are const throughout the execution...
            ARMARX_CHECK(statusUpdate.executionId.skillId.isFullySpecified());
 
            const auto& initial_aron_params = statusUpdate.parameters;
            const auto& callback_interface = statusUpdate.callbackInterface;
            const auto& skillId = statusUpdate.executionId.skillId;
            const auto& skillName = skillId.skillName;
            const auto& providerId = *skillId.providerId;
            const auto& executorName = statusUpdate.executionId.executorName;
            const auto& manager =
                skills::manager::dti::SkillManagerInterfacePrx::checkedCast(callback_interface);
 
            ARMARX_INFO << "Executing skill: " << skillName;
 
            auto updateStatus =
                [&](const SkillStatus status, const aron::data::DictPtr& data = nullptr)
            {
                std::unique_lock l(skillStatusesMutex);
                statusUpdate.status = status;
 
                statusUpdate.result = data;
                if (skill) // if skill has been constructed
                {
                    // update parameterization
                    statusUpdate.parameters = skill->getParameters();
                }
 
                if (callback_interface) // if callback interface is used
                {
                    auto pid = providerId.toCallbackIce();
                    callback_interface->updateStatusForSkill(statusUpdate.toProviderIce(), pid);
                }
            };
 
            {
                std::unique_lock l(skillStatusesMutex);
 
                // ---------------------------------------------------------------------------------
                // check times
                // ---------------------------------------------------------------------------------
                if ((armarx::core::time::DateTime::Now() -
                     statusUpdate.executionId.executionStartedTime) >
                    armarx::core::time::Duration::Seconds(1))
                {
                    ARMARX_WARNING << "SkillError 001: For some reason the skill '"
                                   << skillId.toString()
                                   << "' has been scheduled > 1s ago. The execution should start "
                                      "much faster! Continue, but this is bad behavior..";
                }
            }
 
            // -------------------------------------------------------------------------------------
            // construct skill
            // -------------------------------------------------------------------------------------
            ARMARX_INFO << "Construct skill `" << skillName << "`.";
 
            updateStatus(SkillStatus::Constructing);
            ARMARX_CHECK_NOT_NULL(factory) << "Skill Factory is a nullptr";
            this->skill = this->factory->createSkill(providerId, getEffectiveLoggingLevel());
            this->skill->setExecutorName(executorName);
            this->skill->setManager(manager);
            this->skill->setCallback([&](const SkillStatus s, const armarx::aron::data::DictPtr& d)
                                     { updateStatus(s, d); });
 
            // set initial parameters that were attached to the execution request (only add as we are not sure whether some updates already arrived)
            this->updateSkillParameters(initial_aron_params);
 
            auto makeAbortedResult = [&](const std::string& errorCode,
                                         const aron::data::DictPtr& data,
                                         const std::string& message)
            {
                armarx::skills::arondto::SkillErrorResult errorResult;
                errorResult.errorCode = errorCode;
                errorResult.data = data;
                errorResult.errorMessage = message;
 
                updateStatus(SkillStatus::Aborted, errorResult.toAron());
 
                std::unique_lock l(skillStatusesMutex);
                auto terminated = TerminatedSkillStatusUpdate{
                    {.executionId = statusUpdate.executionId,
                     .parameters = statusUpdate.parameters,
                     .callbackInterface = statusUpdate.callbackInterface,
                     .result = statusUpdate.result}};
                terminated.status = TerminatedSkillStatus::Aborted;
                return terminated;
            };
 
            auto makeFailedResult = [&](const std::string& errorCode,
                                        const aron::data::DictPtr& data,
                                        const std::string& message)
            {
                armarx::skills::arondto::SkillErrorResult errorResult;
                errorResult.errorCode = errorCode;
                errorResult.data = data;
                errorResult.errorMessage = message;
 
                updateStatus(SkillStatus::Failed, errorResult.toAron());
 
                std::unique_lock l(skillStatusesMutex);
                auto terminated = TerminatedSkillStatusUpdate{
                    {.executionId = statusUpdate.executionId,
                     .parameters = statusUpdate.parameters,
                     .callbackInterface = statusUpdate.callbackInterface,
                     .result = statusUpdate.result}};
                terminated.status = TerminatedSkillStatus::Failed;
                return terminated;
            };
 
            // -------------------------------------------------------------------------------------
            // Check params
            // -------------------------------------------------------------------------------------
            // NOT RELEVANT ANYMORE!!
            //            if (skill->getSkillDescription().acceptedType && initial_aron_params &&
            //                not(initial_aron_params->fullfillsType(skill->getSkillDescription().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 << message;
            //                return makeTerminationResult(message);
            //            }
 
            auto exitAndMakeFailedResult = [&](const std::string& errorCode,
                                               const aron::data::DictPtr& data,
                                               const std::string& message)
            {
                skill->exitSkill(); // try to exit skill. Ignore return value
                return makeFailedResult(errorCode, data, message);
            };
            auto exitAndMakeAbortedResult = [&](const std::string& errorCode,
                                                const aron::data::DictPtr& data,
                                                const std::string& message)
            {
                skill->exitSkill(); // try to exit skill. Ignore return value
                return makeAbortedResult(errorCode, data, message);
            };
 
            // Construction succeeded!
            // -------------------------------------------------------------------------------------
            // Init skill
            // -------------------------------------------------------------------------------------
            ARMARX_INFO << "Init skill: " << skillName;
            updateStatus(SkillStatus::Initializing);
 
            try
            {
                Skill::InitResult initRet = skill->initSkill();
                if (initRet.status != TerminatedSkillStatus::Succeeded)
                {
                    std::string message = "SkillError 101: The initialization of skill '" +
                                          skillName + "' did not succeed.";
                    return exitAndMakeFailedResult("101", nullptr, message);
                }
            }
            catch (const error::SkillAbortedException& ex)
            {
                return exitAndMakeAbortedResult("101x", nullptr, GetHandledExceptionString());
            }
            catch (const error::SkillFailedException& ex)
            {
                return exitAndMakeFailedResult("101x", nullptr, GetHandledExceptionString());
            }
            catch (const std::exception& ex)
            {
                std::string message =
                    "SkillError 101e: An error occured during the initialization of skill '" +
                    skillName + "'. The error was: " + GetHandledExceptionString();
                ARMARX_ERROR << message;
                return exitAndMakeFailedResult("101e", nullptr, message);
            }
 
            // Init succeeded!
            // -------------------------------------------------------------------------------------
            // Prepare skill
            // -------------------------------------------------------------------------------------
            ARMARX_INFO << "Prepare skill: " << skillName;
            updateStatus(SkillStatus::Preparing);
 
            try
            {
                auto prepareRet = skill->prepareSkill();
                while (prepareRet.status == ActiveOrTerminatedSkillStatus::Running)
                {
                    ARMARX_INFO << deactivateSpam() << "Not all requirements for skill "
                                << skillName << " fulfilled. Waiting...";
 
                    // wait...
                    std::this_thread::sleep_for(std::chrono::milliseconds(50));
                    prepareRet = skill->prepareSkill();
                }
 
                if (prepareRet.status != ActiveOrTerminatedSkillStatus::Succeeded)
                {
                    std::string message = "SkillError 201: The prepare method of skill '" +
                                          skillName + "' did not succeed. ";
                    const bool shouldSkillTerminate = skill->shouldSkillTerminate();
                    ARMARX_ERROR << message << VAROUT(shouldSkillTerminate);
                    return exitAndMakeFailedResult("201", nullptr, message);
                }
            }
            catch (const error::SkillAbortedException& ex)
            {
                return exitAndMakeAbortedResult("201x", nullptr, GetHandledExceptionString());
            }
            catch (const error::SkillFailedException& ex)
            {
                return exitAndMakeFailedResult("201x", nullptr, GetHandledExceptionString());
            }
            catch (const std::exception& ex)
            {
                std::string message = "SkillError 201e: An error occured during waiting for skill "
                                      "dependencies of skill '" +
                                      skillName +
                                      "'. The error was: " + GetHandledExceptionString();
                ARMARX_ERROR << message;
                return exitAndMakeFailedResult("201e", nullptr, message);
            }
 
            // Prepare succeeded!
            // -------------------------------------------------------------------------------------
            // Main of skill
            // -------------------------------------------------------------------------------------
            // execute. If the skill fails for some reason, from this point it will always execute its exit function.
            ARMARX_INFO << "Main skill: " << skillName;
            updateStatus(SkillStatus::Running);
 
            Skill::MainResult mainRet;
            try
            {
                mainRet = skill->mainOfSkill();
                if (mainRet.status == TerminatedSkillStatus::Failed)
                {
                    std::string message =
                        "SkillError 501: The main method of skill '" + skillName + "' did fail.";
 
                    return exitAndMakeFailedResult("501", mainRet.data, message);
                }
                else if (mainRet.status == TerminatedSkillStatus::Aborted)
                {
                    std::string message =
                        "SkillError 501: The main method of skill '" + skillName + "' got aborted.";
                    return exitAndMakeAbortedResult("501", mainRet.data, message);
                }
            }
            catch (const error::SkillAbortedException& ex)
            {
                return exitAndMakeAbortedResult("501x", mainRet.data, GetHandledExceptionString());
            }
            catch (const error::SkillFailedException& ex)
            {
                return exitAndMakeFailedResult("501x", mainRet.data, GetHandledExceptionString());
            }
            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 << message;
                return exitAndMakeFailedResult("501e", mainRet.data, message);
            }
 
            // Main succeeded!
            // -------------------------------------------------------------------------------------
            // Exit of skill
            // -------------------------------------------------------------------------------------
            ARMARX_INFO << "Exit skill: " << skillName;
            try
            {
                Skill::ExitResult exitRet = skill->exitSkill();
                if (exitRet.status != TerminatedSkillStatus::Succeeded)
                {
                    std::string message = "SkillError 601: The exit method of skill '" + skillName +
                                          "' did not succeed.";
                    return makeFailedResult("601", mainRet.data, message);
                }
            }
            catch (const error::SkillAbortedException& ex)
            {
                return makeAbortedResult("601x", mainRet.data, GetHandledExceptionString());
            }
            catch (const error::SkillFailedException& ex)
            {
                return makeFailedResult("601x", mainRet.data, GetHandledExceptionString());
            }
            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 << message;
                return makeFailedResult("601e", mainRet.data, message);
            }
 
            // Exit succeeded!
            // All succeeded!
            {
                updateStatus(SkillStatus::Succeeded, mainRet.data);
 
                // return result of main method
                std::unique_lock l(skillStatusesMutex);
                TerminatedSkillStatusUpdate ret{
                    {.executionId = statusUpdate.executionId,
                     .parameters = statusUpdate.parameters,
                     .callbackInterface = statusUpdate.callbackInterface}};
                ret.result = mainRet.data;
                ret.status = TerminatedSkillStatus::Succeeded;
                return ret;
            }
        }
    } // namespace skills::detail
} // namespace armarx