Skill.cpp

Go to the documentation of this file.

#include "Skill.h"
 
namespace armarx
{
    namespace skills
    {
        Skill::Skill(const SkillDescription& desc):
            description(desc)
        {
            // replace constructor if you want to have a specific logging tag
            Logging::setTag("armarx::skills::" + description.skillName);
        }
 
        // install a local condition via a lambda
        void Skill::installConditionWithCallback(std::function<bool()>&& f, std::function<void()>&& cb)
        {
            std::lock_guard l(callbacksMutex);
            callbacks.push_back({f, cb});
        }
 
        bool Skill::isSkillAvailable(const InitInput &in) const
        {
            return this->isAvailable(in);
        }
 
        bool Skill::isAvailable(const InitInput& in) const
        {
            (void) in;
            return true;
        }
 
        void Skill::resetSkill()
        {
            //ARMARX_IMPORTANT << "Resetting skill '" << description.skillName << "'";
 
            // resetting log times
            started = armarx::core::time::DateTime::Invalid();
            exited = armarx::core::time::DateTime::Invalid();
 
            // resetting master running variable
            running = false;
 
            // resetting conditional variables
            stopped = false;
            timeoutReached = false;
 
            this->reset();
        }
 
        void Skill::waitForDependenciesOfSkill()
        {
            //ARMARX_IMPORTANT << "Waiting for dependencies of skill '" << description.skillName << "'";
            this->waitForDependencies();
        }
 
        void Skill::_init()
        {
            //ARMARX_IMPORTANT << "Initializing skill '" << description.skillName << "'";
            callbacks.clear();
            running = true;
            started = armarx::core::time::DateTime::Now();
 
            // install timeout condition
            installConditionWithCallback(
                        [&](){ return (armarx::core::time::DateTime::Now() >= (started + description.timeout)); },
                        [&](){ notifyTimeoutReached(); }
            );
 
            conditionCheckingThread = std::thread([&]()
            {
                armarx::core::time::Metronome metronome(conditionCheckingThreadFrequency);
                while (running) // when the skill ends/aborts this variable will be set to false
                {
                    {
                        std::scoped_lock l(callbacksMutex);
                        for (auto& p : callbacks)
                        {
                            auto& f = p.first;
                            auto& cb = p.second;
                            if (f())
                            {
                                cb();
                            }
                        }
                    }
                    const auto sleepDuration = metronome.waitForNextTick();
                    if (not sleepDuration.isPositive())
                    {
                        ARMARX_WARNING << deactivateSpam() << "PeriodicSkill: execution took too long (" << -sleepDuration << " vs " << conditionCheckingThreadFrequency.toCycleDuration() << ")";
                    }
                }
            });
        }
        Skill::InitResult Skill::initSkill(const InitInput& in)
        {
            this->_init();
            return this->init(in);
        }
 
        void Skill::_main()
        {
            // Nothing here yet...
        }
        Skill::MainResult Skill::mainOfSkill(const MainInput& in)
        {
            this->_main();
            return this->main(in);
        }
 
        void Skill::_exit()
        {
            // ARMARX_IMPORTANT << "Exiting Skill '" << description.skillName << "'";
            running = false; // stop checking conditions
 
            if (conditionCheckingThread.joinable())
            {
                conditionCheckingThread.join();
            }
            exited = armarx::core::time::DateTime::Now();
        }
        Skill::ExitResult Skill::exitSkill(const ExitInput& in)
        {
            auto ret = this->exit(in);
            this->_exit();
            return ret;
        }
 
        void Skill::notifyTimeoutReached()
        {
            timeoutReached = true;
            onTimeoutReached();
        }
 
        Skill::MainResult Skill::MakeAbortedResult(aron::data::DictPtr data)
        {
            return MainResult{
                .status = TerminatedSkillStatus::Aborted,
                .data = data,
            };
        }
 
        void Skill::notifySkillToStopASAP()
        {
            stopped = true;
            onStopRequested();
        }
 
        bool Skill::checkWhetherSkillShouldStopASAP() const
        {
            return stopped || timeoutReached;
        }
 
        // condition effects
        void Skill::onTimeoutReached()
        {
        }
        void Skill::onStopRequested()
        {
        }
 
        // reset all local variables
        void Skill::reset()
        {
            // Default nothing to reset
        }
 
        // Wait if needed
        void Skill::waitForDependencies()
        {
            // Default wait for nothing
        }
 
        // always called before execute (should not take longer than 100ms)
        Skill::InitResult Skill::init(const InitInput&)
        {
            // Default nothing to init
            return {.status = TerminatedSkillStatus::Succeeded};
        }
 
        // always called after execute or if skill fails (should not take longer than 100ms)
        Skill::ExitResult Skill::exit(const ExitInput&)
        {
            // Default nothing to exit
            return {.status = TerminatedSkillStatus::Succeeded};
        }
 
        Skill::MainResult Skill::main(const MainInput& in)
        {
            // This is just a dummy implementation
            ARMARX_IMPORTANT << "Dummy executing skill '" << description.skillName << "'. Please overwrite this method.";
            return {.status = TerminatedSkillStatus::Succeeded, .data = nullptr};
        }
 
        Skill::MainResult Skill::executeFullSkill(const MainInput& in)
        {
            this->resetSkill();
            this->initSkill(InitInput{.executorName = in.executorName, .params = in.params});
            auto ret = this->mainOfSkill(in);
            this->exitSkill(ExitInput{.executorName = in.executorName, .params = in.params});
            return ret;
        }
    }
}