SimplePeriodicSkill.cpp

Go to the documentation of this file.

#include "SimplePeriodicSkill.h"
 
#include <ArmarXCore/core/time/Metronome.h>
 
#include <RobotAPI/libraries/skills/core/error/Exception.h>
 
namespace armarx::skills
{
    SimplePeriodicSkill::SimplePeriodicSkill(const SkillDescription& skillDescription,
                                             const armarx::Frequency& frequency) :
        SimpleSkill(skillDescription), frequency(frequency)
    {
    }
 
    Skill::MainResult
    SimplePeriodicSkill::main(const MainInput& in)
    {
        armarx::core::time::Metronome metronome(frequency);
 
        while (true)
        {
            this->throwIfSkillShouldTerminate();
 
            const auto res = step(in);
            switch (res.status)
            {
                case ActiveOrTerminatedSkillStatus::Running:
                    // nothing to do here. break switch
                    break;
                case ActiveOrTerminatedSkillStatus::Aborted:
                    return MakeAbortedResult();
                case ActiveOrTerminatedSkillStatus::Succeeded:
                    return MakeSucceededResult(res.data);
                case ActiveOrTerminatedSkillStatus::Failed:
                    return MakeFailedResult();
            }
 
            const auto sleepDuration = metronome.waitForNextTick();
            if (not sleepDuration.isPositive())
            {
                ARMARX_INFO << deactivateSpam() << __PRETTY_FUNCTION__
                            << ": execution took too long (" << -sleepDuration
                            << " too long. Expected " << frequency.toCycleDuration() << ")";
            }
        }
 
        // never happens
        throw skills::error::SkillException(__PRETTY_FUNCTION__, "Should not happen!");
    }
 
    SimplePeriodicSkill::StepResult
    SimplePeriodicSkill::step(const MainInput& in)
    {
        ARMARX_IMPORTANT << "Dummy executing once skill '" << description.skillId
                         << "'. Please overwrite this method!";
        return {ActiveOrTerminatedSkillStatus::Succeeded, nullptr};
    }
} // namespace armarx::skills