Skill.cpp

Go to the documentation of this file.

#include "Skill.h"
 
#include <functional>
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <thread>
#include <utility>
 
#include <Ice/LocalException.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/Metronome.h>
 
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/skills/core/ProviderID.h>
#include <RobotAPI/libraries/skills/core/SkillDescription.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionHandle.h>
#include <RobotAPI/libraries/skills/core/SkillExecutionID.h>
#include <RobotAPI/libraries/skills/core/SkillID.h>
#include <RobotAPI/libraries/skills/core/SkillProxy.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/core/error/Exception.h>
 
namespace armarx
{
    namespace skills
    {
        Skill::Skill(const SkillDescription& desc) : description(desc), constructing(true)
        {
            // replace constructor if you want to have a specific logging tag
            Logging::setTag("armarx::skills::" + description.skillId.toString());
 
            ARMARX_CHECK_NOT_NULL(desc.rootProfileDefaults);
            if (!desc.rootProfileDefaults->fullfillsType(desc.parametersType))
            {
                ARMARX_WARNING << "Missing root profile defaults for skill " << desc.skillId;
            }
        }
 
        // install a local condition via a lambda
        void
        Skill::installConditionWithCallback(std::function<bool()>&& f, std::function<void()>&& cb)
        {
            std::scoped_lock l(conditionCallbacksMutex);
            conditionCallbacks.push_back({f, cb});
        }
 
        void
        Skill::waitFor(const armarx::Duration& duration, const armarx::Duration& interval) const
        {
            double n = duration / interval;
            auto metronome = armarx::Metronome(interval);
            while (n > 1 and not shouldSkillTerminate())
            {
                n--;
                metronome.waitForNextTick();
            }
            if (n > 0.05 and not shouldSkillTerminate())
            {
                // Wait for the remaining time
                armarx::Clock::WaitFor(interval * n);
            }
        }
 
        std::optional<TerminatedSkillStatusUpdate>
        Skill::callSubskill(SkillProxyPtr proxy)
        {
            auto parameters = proxy->getRootProfileParameters();
            return callSubskill(std::move(proxy), parameters);
        }
 
        std::optional<TerminatedSkillStatusUpdate>
        Skill::callSubskill(SkillProxyPtr proxy, const aron::data::DictPtr& parameters)
        {
            auto handle = callSubskillAsync(std::move(proxy), parameters);
            ARMARX_CHECK_NOT_NULL(handle);
            auto ret = handle->join();
 
            // While the sub skill was running, our skill might also have been aborted.
            // In this case, the correct behavour would be aborting ourselves.
            // The caller of callSubskill() can catch the thrown error::SkillAbortedException
            // if necessary.
            throwIfSkillShouldTerminate();
            return ret;
        }
 
        SkillExecutionHandlePtr
        Skill::callSubskillAsync(Skill::SkillProxyPtr prx)
        {
            aron::data::DictPtr params = prx->getRootProfileParameters();
            return callSubskillAsync(std::move(prx), params);
        }
 
        SkillExecutionHandlePtr
        Skill::callSubskillAsync(Skill::SkillProxyPtr prx, const aron::data::DictPtr& params)
        {
            std::string executorHistory = this->executorName + "->" + getSkillId().toString();
 
            throwIfSkillShouldTerminate();
            auto eid = prx->executeSkillAsync(executorHistory, params);
 
            std::unique_lock l(subskillsMutex);
            throwIfSkillShouldTerminate(
                [&]()
                {
                    prx->abortSkillAsync(eid);
                }); // also notify newly added skill as it was not added to subskills list yet
            this->subskills.push_back(eid);
 
            return std::make_unique<SkillExecutionHandle>(std::move(prx), eid);
        }
 
        std::optional<TerminatedSkillStatusUpdate>
        Skill::callSubskill(const SkillID& skillId)
        {
            return callSubskill(std::make_unique<SkillProxy>(manager, skillId));
        }
 
        std::optional<TerminatedSkillStatusUpdate>
        Skill::callSubskill(const SkillID& skillId, const aron::data::DictPtr& parameters)
        {
            return callSubskill(std::make_unique<SkillProxy>(manager, skillId), parameters);
        }
 
        std::optional<TerminatedSkillStatusUpdate>
        Skill::callSubskill(const SkillID& skillId,
                            std::function<void(aron::data::DictPtr&)> parametersFunction)
        {
            SkillProxyPtr proxy = std::make_unique<SkillProxy>(manager, skillId);
 
            aron::data::DictPtr parameters = proxy->getRootProfileParameters();
            if (not parameters)
            {
                parameters = armarx::aron::make_dict();
            }
 
            parametersFunction(parameters);
 
            return callSubskill(std::move(proxy), parameters);
        }
 
        SkillExecutionHandlePtr
        Skill::callSubskillAsync(const SkillID& skillId,
                                 std::function<void(aron::data::DictPtr&)> parametersFunction)
        {
            SkillProxyPtr proxy = std::make_unique<SkillProxy>(manager, skillId);
 
            aron::data::DictPtr parameters = proxy->getRootProfileParameters();
            if (not parameters)
            {
                parameters = armarx::aron::make_dict();
            }
 
            parametersFunction(parameters);
 
            return callSubskillAsync(std::move(proxy), parameters);
        }
 
        SkillExecutionHandlePtr
        Skill::callSubskillAsync(const SkillID& skillId)
        {
            SkillProxyPtr proxy = std::make_unique<SkillProxy>(manager, skillId);
 
            aron::data::DictPtr parameters = proxy->getRootProfileParameters();
            if (not parameters)
            {
                parameters = armarx::aron::make_dict();
            }
 
            return callSubskillAsync(std::move(proxy), parameters);
        }
 
        std::optional<TerminatedSkillStatusUpdate>
        Skill::executeSkill(
            const ::armarx::skills::SkillID& skillID,
            std::function<aron::data::DictPtr(aron::data::DictPtr&)> const& parametersFunction)
        {
            SkillProxyPtr proxy = std::make_unique<SkillProxy>(manager, skillID);
 
            aron::data::DictPtr parameter_defaults = proxy->getRootProfileParameters();
            return callSubskill(std::move(proxy), parametersFunction(parameter_defaults));
        }
 
        void
        Skill::updateParameters(const aron::data::DictPtr& d)
        {
            std::scoped_lock l(this->parametersMutex);
            if (this->parameters == nullptr)
            {
                // set params as there has been no update before.
                this->parameters = d;
            }
            else
            {
                // merge params into existing. Note that this may update already set params.
                this->parameters->mergeAndReplaceCopy(d);
            }
        }
 
        void
        Skill::setParameters(const aron::data::DictPtr& d)
        {
            // we only set the params if the skill is not already running
            if (running or exiting or finished)
            {
                return;
            }
 
            std::scoped_lock l(this->parametersMutex);
            this->parameters = d;
        }
 
        aron::data::DictPtr
        Skill::getParameters() const
        {
            return this->parameters;
        }
 
        Skill::InitResult
        Skill::_init()
        {
            // ARMARX_IMPORTANT << "Initializing skill '" << description.skillName << "'";
            this->initializing = true;
            this->constructing = false;
            this->preparing = false;
            this->running = false;
            this->exiting = false;
            this->finished = false;
 
            // install timeout condition
            installConditionWithCallback(
                [&]() {
                    return (armarx::core::time::DateTime::Now() >= (started + description.timeout));
                },
                [&]() { notifyTimeoutReached(); });
 
            conditionCheckingThread = std::thread(
                [&]()
                {
                    armarx::core::time::Metronome metronome(conditionCheckingThreadFrequency);
                    while (initializing or preparing or
                           running) // when the skill ends/aborts this variable will be set to false
                    {
                        {
                            std::scoped_lock l(conditionCallbacksMutex);
                            for (auto& p : conditionCallbacks)
                            {
                                auto& f = p.first;
                                auto& cb = p.second;
                                if (f())
                                {
                                    cb();
                                }
                            }
                        }
 
                        const auto sleepDuration = metronome.waitForNextTick();
                        if (not sleepDuration.isPositive())
                        {
                            ARMARX_WARNING << deactivateSpam()
                                           << "ConditionCheckingThread: execution took too long ("
                                           << -sleepDuration << " vs "
                                           << conditionCheckingThreadFrequency.toCycleDuration()
                                           << ")";
                        }
                    }
                });
            return {.status = TerminatedSkillStatus::Succeeded};
        }
 
        Skill::PrepareResult
        Skill::_prepare()
        {
            this->preparing = true;
            this->initializing = false;
            this->constructing = false;
            this->running = false;
            this->exiting = false;
            this->finished = false;
 
            if (shouldSkillTerminate())
            {
                return {.status = ActiveOrTerminatedSkillStatus::Aborted};
            }
 
            // Default nothing to prepare
            if (not description.parametersType)
            {
                return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
            }
            if (this->parameters && this->parameters->fullfillsType(description.parametersType))
            {
                // wait until parameters fulfill type
                return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
            }
 
            // false if we have to wait for parameters
            return {.status = ActiveOrTerminatedSkillStatus::Running};
        }
 
        Skill::MainResult
        Skill::_main()
        {
            this->running = true;
            this->initializing = false;
            this->constructing = false;
            this->preparing = false;
            this->exiting = false;
            this->finished = false;
            return {.status = TerminatedSkillStatus::Succeeded};
        }
 
        Skill::ExitResult
        Skill::_exit()
        {
            // ARMARX_IMPORTANT << "Exiting Skill '" << description.skillName << "'";
            this->exiting = true;
            this->running = false;
            this->initializing = false;
            this->constructing = false;
            this->preparing = false;
            this->finished = false;
 
            if (conditionCheckingThread.joinable())
            {
                conditionCheckingThread.join();
            }
            exited = armarx::core::time::DateTime::Now();
 
            this->finished = true;
            this->exiting = false;
            return {.status = TerminatedSkillStatus::Succeeded};
        }
 
        Skill::InitResult
        Skill::initSkill()
        {
            std::scoped_lock l(parametersMutex);
            auto _res = this->_init();
            auto res = this->init();
            return {.status = skills::mergeSkillStatuseses(_res.status, res.status)};
        }
 
        Skill::PrepareResult
        Skill::prepareSkill()
        {
            std::scoped_lock l(parametersMutex);
            auto _res = this->_prepare();
            auto res = this->prepare();
            return {.status = skills::mergeSkillStatuseses(_res.status, res.status)};
        }
 
        Skill::MainResult
        Skill::mainOfSkill()
        {
            std::scoped_lock l(parametersMutex);
            auto _res = this->_main();
            auto res = this->main();
            return {.status = skills::mergeSkillStatuseses(_res.status, res.status),
                    .data = res.data};
        }
 
        Skill::ExitResult
        Skill::exitSkill()
        {
            std::scoped_lock l(parametersMutex);
            auto res = this->exit();
            auto _res = this->_exit();
            return {.status = skills::mergeSkillStatuseses(_res.status, res.status)};
        }
 
        void
        Skill::throwIfSkillShouldTerminate(const std::function<void()>& do_before,
                                           const std::string& abortedMessage) const
        {
            if (shouldSkillTerminate())
            {
                do_before();
                throwIfSkillShouldTerminate(abortedMessage);
            }
        }
 
        void
        Skill::throwIfSkillShouldTerminate(const std::string& abortedMessage) const
        {
            if (stopped)
            {
                std::string message =
                    std::string("The skill '" + getSkillId().toString() + "' was asked to stop.");
                message += abortedMessage.empty() ? "" : " Additional message: " + abortedMessage;
 
                throw error::SkillAbortedException(__PRETTY_FUNCTION__, message);
                return;
            }
 
            if (timeoutReached)
            {
                std::string message =
                    std::string("The skill '" + getSkillId().toString() + "' reached timeout.");
                message += abortedMessage.empty() ? "" : " Additional message: " + abortedMessage;
 
                ARMARX_WARNING << message;
                throw error::SkillFailedException(__PRETTY_FUNCTION__, message);
            }
        }
 
        Skill::MainResult
        Skill::MakeSucceededResult(aron::data::DictPtr data)
        {
            return MainResult{
                .status = TerminatedSkillStatus::Succeeded,
                .data = data,
            };
        }
 
        Skill::MainResult
        Skill::MakeFailedResult(aron::data::DictPtr data)
        {
            return MainResult{
                .status = TerminatedSkillStatus::Failed,
                .data = data,
            };
        }
 
        Skill::MainResult
        Skill::MakeAbortedResult(aron::data::DictPtr data)
        {
            return MainResult{
                .status = TerminatedSkillStatus::Aborted,
                .data = data,
            };
        }
 
        void
        Skill::notifySkillToStop()
        {
            if (stopped)
            {
                // skill already got stopped. Ignore
                return;
            }
            std::scoped_lock l(subskillsMutex);
            stopped = true;
            _onStopRequested();
            onStopRequested();
        }
 
        void
        Skill::notifyTimeoutReached()
        {
            if (stopped || timeoutReached)
            {
                // skill already got timeoutReached. Ignore
                return;
            }
 
            std::scoped_lock l(subskillsMutex);
            timeoutReached = true;
            _onTimeoutReached();
            onTimeoutReached();
        }
 
        bool
        Skill::shouldSkillTerminate() const
        {
            return stopped || timeoutReached;
        }
 
        // condition effects
        void
        Skill::_onTimeoutReached()
        {
            // WE ASSUME THAT THE LOCK IS ALREADY TAKEN
 
            if (!manager)
            {
                return;
            }
 
            for (const auto& execId : subskills)
            {
                try
                {
                    manager->abortSkillAsync(execId.toManagerIce());
                }
                catch (const Ice::LocalException& e)
                {
                    ARMARX_WARNING << "Failed to abort subskill '" << execId.skillId
                                   << "': " << e.what();
                }
            }
        }
 
        void
        Skill::_onStopRequested()
        {
            // WE ASSUME THAT THE LOCK IS ALREADY TAKEN
 
            if (!manager)
            {
                return;
            }
 
            for (const auto& execId : subskills)
            {
                try
                {
                    manager->abortSkillAsync(execId.toManagerIce());
                }
                catch (const Ice::LocalException& e)
                {
                    ARMARX_WARNING << "Failed to abort subskill '" << execId.skillId
                                   << "': " << e.what();
                }
            }
        }
 
        void
        Skill::onTimeoutReached()
        {
        }
 
        void
        Skill::onStopRequested()
        {
        }
 
        // always called before prepare (should not take longer than 100ms)
        Skill::InitResult
        Skill::init()
        {
            // Default nothing to init
            return {.status = TerminatedSkillStatus::Succeeded};
        }
 
        // always called before main (should not take longer than 100ms)
        Skill::PrepareResult
        Skill::prepare()
        {
            // Default nothing to prepare
            return {.status = ActiveOrTerminatedSkillStatus::Succeeded};
        }
 
        // always called after main or if skill fails (should not take longer than 100ms)
        Skill::ExitResult
        Skill::exit()
        {
            // Default nothing to exit
            return {.status = TerminatedSkillStatus::Succeeded};
        }
 
        Skill::MainResult
        Skill::main()
        {
            // This is just a dummy implementation
            ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillId
                             << "'. Please overwrite this method.";
            return {.status = TerminatedSkillStatus::Succeeded, .data = nullptr};
        }
 
        void
        Skill::setProviderId(const skills::ProviderID& pid)
        {
            description.skillId.providerId = pid;
        }
 
        void
        Skill::setCallback(const CallbackT& callback)
        {
            this->callback = callback;
        }
 
        void
        Skill::setManager(const manager::dti::SkillManagerInterfacePrx& manager)
        {
            this->manager = manager;
        }
 
        void
        Skill::setExecutorName(const std::string& executorName)
        {
            this->executorName = executorName;
        }
 
        void
        Skill::updateSubSkillStatus(const skills::SkillStatusUpdate& statusUpdate)
        {
            GlobalSkillUpdateManager.update(statusUpdate);
        }
 
        SkillDescription
        Skill::getSkillDescription() const
        {
            return description;
        }
 
        SkillID
        Skill::getSkillId() const
        {
            return description.skillId;
        }
 
        Skill::~Skill()
        {
            // ARMARX_IMPORTANT << "DESTROY SKILL " << getSkillId();
        }
 
        SkillExecutionHandlePtr
        Skill::callSubskillAsync(const SkillID& skillId, const aron::data::DictPtr& parameters)
        {
            SkillProxyPtr proxy = std::make_unique<SkillProxy>(manager, skillId);
            return callSubskillAsync(std::move(proxy), parameters);
        }
 
    } // namespace skills
} // namespace armarx