Skill.h

Go to the documentation of this file.

#pragma once
 
#include <functional>
#include <mutex>
#include <queue>
#include <thread>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Metronome.h>
 
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
 
#include "SkillDescription.h"
#include "SkillID.h"
#include "SkillPreparationInput.h"
#include "SkillProxy.h"
#include "SkillStatusUpdate.h"
#include "error/Exception.h"
 
namespace armarx
{
    namespace skills
    {
        /**
         * @brief Base class for skills.
         */
        class Skill : public armarx::Logging
        {
        public:
            using CallbackT =
                std::function<void(const SkillStatus s, const armarx::aron::data::DictPtr&)>;
 
            /// A result struct for skill initialization
            struct InitResult
            {
                TerminatedSkillStatus status;
            };
 
            /// A result struct for skill preparing
            struct PrepareResult
            {
                ActiveOrTerminatedSkillStatus status;
            };
 
            /// A result struct for th main method of a skill
            struct MainResult
            {
                TerminatedSkillStatus status;
                aron::data::DictPtr data = nullptr;
            };
 
            /// A result struct for skill exit function
            struct ExitResult
            {
                TerminatedSkillStatus status;
            };
 
            /// We completely remove the default constructor! A skill without a desciption cannot exist
            Skill() = delete;
 
            /// Constructor of a skill for inheritance. Every skill must have a skill description
            Skill(const SkillDescription&);
 
            /// Virtual destructor of a skill
            virtual ~Skill()
            {
                //ARMARX_IMPORTANT << "DESTROY SKILL " << getSkillId();
            }
 
            /// Get the id of the skill
            SkillID
            getSkillId() const
            {
                return description.skillId;
            }
 
            /// Get the description of a skill
            SkillDescription
            getSkillDescription() const
            {
                return description;
            }
 
            /// Set the provider id of the description of the skill.
            /// This method is called when creating a skill in a skill provider
            void
            setProviderId(const skills::ProviderID& pid)
            {
                description.skillId.providerId = pid;
            }
 
            void
            setCallback(const CallbackT& callback)
            {
                this->callback = callback;
            }
 
            void
            setManager(const manager::dti::SkillManagerInterfacePrx& manager)
            {
                this->manager = manager;
            }
 
            void
            setExecutorName(const std::string& executorName)
            {
                this->executorName = executorName;
            }
 
            /// Prepare a skill once. This method is called in a loop as long as it returns RUNNING
            /// If the loop does not terminate with SUCCEDED the skill execution is failed.
            PrepareResult prepareSkill();
 
            /// Initialization of a skill. Called directly after construction.
            /// If this method does not return SUCCEEDED the skill execution is failed.
            InitResult initSkill();
 
            /// Main method of a skill.
            MainResult mainOfSkill();
 
            /// Exit method of a skill. It is guaranteed that exit is always called
            /// (unless there is a segfault or similar)
            ExitResult exitSkill();
 
            /// Notify the skill from extern to stop
            void notifySkillToStop();
 
            /// Returns whether the skill should terminate as soon as possible
            bool shouldSkillTerminate() const;
 
            /// Merge parameters to the local parameters of the skill
            void updateParameters(const aron::data::DictPtr& d);
 
            /// Hard set the parameters, ignoring everything that has been set or merged before
            void setParameters(const aron::data::DictPtr& d);
 
            /// Get the parameters of a skill that have been set so far
            aron::data::DictPtr getParameters() const;
 
        protected:
            void throwIfSkillShouldTerminate(const std::string& abortedMessage = "");
            void throwIfSkillShouldTerminate(const std::function<void()>& do_before,
                                             const std::string& abortedMessage = "");
 
            static MainResult MakeSucceededResult(aron::data::DictPtr data = nullptr);
            static MainResult MakeFailedResult();
            static MainResult MakeAbortedResult();
 
            // fires if the skill reaches timeout
            void notifyTimeoutReached();
 
        private:
            // helper methods to do all the static initialization stuff
            InitResult _init();
            PrepareResult _prepare();
            MainResult _main();
            ExitResult _exit();
 
            void _onTimeoutReached();
            void _onStopRequested();
 
        protected:
            /// Override this method with the actual implementation.
            virtual InitResult init();
 
            /// Override this method with the actual implementation.
            virtual PrepareResult prepare();
 
            /// Override this method with the actual implementation. The callback is for status updates to the calling instance
            virtual MainResult main();
 
            /// Override this method with the actual implementation.
            virtual ExitResult exit();
 
        protected:
            /// Override these methods if you want to do something special when notification comes
            virtual void onTimeoutReached();
            virtual void onStopRequested();
 
        protected:
            /// install a condition which is frequently checked from the conditionCheckingThread
            void installConditionWithCallback(std::function<bool()>&& f,
                                              std::function<void()>&& cb);
 
            // Calling subskills
 
            /**
             * @brief Call a subskill with default parameters and block until the subskill terminates.
             *
             * If you call a subskill this way it will be stopped if the current skill stops.
             *
             * @param proxy Skill proxy.
             * @return Terminated skill status update.
             * @throw armarx::skills::error::SkillAbortedException
             *      If the calling skill has been aborted.
             * @throw armarx::skills::error::SkillFailedException
             *      If the calling skill's timeout was reached.
             */
            std::optional<TerminatedSkillStatusUpdate>
            callSubskill(const skills::SkillProxy& proxy);
 
            /**
             * @brief Call a subskill with given parameters and block until the subskill terminates.
             * @param proxy Skill proxy.
             * @param parameters Parameters passed to the skill.
             * @return Terminated skill status update.
             * @throw armarx::skills::error::SkillAbortedException
             *      If the calling skill has been aborted.
             * @throw armarx::skills::error::SkillFailedException
             *      If the calling skill's timeout was reached.
             */
            std::optional<TerminatedSkillStatusUpdate>
            callSubskill(const skills::SkillProxy& proxy, const aron::data::DictPtr& parameters);
 
            /// Similar to callSubskill but non-blocking
            skills::SkillExecutionID callSubskillAsync(const skills::SkillProxy& proxy);
 
            /// Similar to callSubskill but non-blocking
            skills::SkillExecutionID callSubskillAsync(const skills::SkillProxy& proxy,
                                                       const aron::data::DictPtr& parameters);
 
            /**
             * @brief Call a subskill with the given ID and its default parameters.
             * @param skillId The subskill's ID.
             * @return The terminated skill status update.
             * @throw armarx::skills::error::SkillAbortedException
             *      If the calling skill has been aborted.
             * @throw armarx::skills::error::SkillFailedException
             *      If the calling skill's timeout was reached.
             */
            std::optional<TerminatedSkillStatusUpdate> callSubskill(const SkillID& skillId);
 
            /**
             * @brief Call a subskill with the given ID and parameters.
             * @param skillId The subskill's ID.
             * @param parameters The parameters.
             * @return The terminated skill status update.
             * @throw armarx::skills::error::SkillAbortedException
             *      If the calling skill has been aborted.
             * @throw armarx::skills::error::SkillFailedException
             *      If the calling skill's timeout was reached.
             */
            std::optional<TerminatedSkillStatusUpdate>
            callSubskill(const SkillID& skillId, const aron::data::DictPtr& parameters);
 
            /**
             * @brief Call a subskill with the given ID and parameters.
             * @param skillId The subskill's ID.
             * @param parameters The parameters.
             * @return The terminated skill status update.
             * @throw armarx::skills::error::SkillAbortedException
             *      If the calling skill has been aborted.
             * @throw armarx::skills::error::SkillFailedException
             *      If the calling skill's timeout was reached.
             */
            template <class ParameterT>
            std::optional<TerminatedSkillStatusUpdate>
            callSubskill(const SkillID& skillId, const ParameterT& parameters)
            {
                return callSubskill(skillId, parameters.toAron());
            }
 
            /**
             * @brief Call a subskill with parameters based on the default parameters.
             *
             * Creates the skill's default parameters, and calls `parametersFunction` on them.
             * This allows the caller to modify the parameters before executing the skill.
             *
             * @param skillId The subskill's ID.
             * @param parametersFunction Function which edits the parameters.
             * @return The terminated skill status update.
             * @throw armarx::skills::error::SkillAbortedException
             *      If the calling skill has been aborted.
             * @throw armarx::skills::error::SkillFailedException
             *      If the calling skill's timeout was reached.
             */
            std::optional<TerminatedSkillStatusUpdate>
            callSubskill(const SkillID& skillId,
                         std::function<void(aron::data::DictPtr& parameters)> parametersFunction);
 
            /**
             * @brief Call a subskill with parameters based on the default parameters.
             *
             * Creates the skill's default parameters, converts them to `ParameterT`,
             * and calls `parametersFunction` on them.
             * This allows the caller to modify the parameters as `ParameterT` before executing
             * the skill.
             *
             * @param skillId The subskill's ID.
             * @param parametersFunction Function which edits the parameters.
             * @return The terminated skill status update.
             * @throw armarx::skills::error::SkillAbortedException
             *      If the calling skill has been aborted.
             * @throw armarx::skills::error::SkillFailedException
             *      If the calling skill's timeout was reached.
             */
            template <class ParameterT>
            std::optional<TerminatedSkillStatusUpdate>
            callSubskill(const SkillID& skillId,
                         std::function<void(ParameterT& parameters)> parametersFunction)
            {
                SkillProxy proxy(manager, skillId);
 
                ParameterT parameters;
                if (auto parametersAron = proxy.getRootProfileParameters())
                {
                    parameters = ParameterT::FromAron(parametersAron);
                }
 
                parametersFunction(parameters);
 
                return callSubskill(proxy, parameters.toAron());
            }
 
 
        public:
            // running params
            armarx::core::time::DateTime started = armarx::core::time::DateTime::Now();
            armarx::core::time::DateTime exited = armarx::core::time::DateTime::Invalid();
 
        protected:
            // parameterization. Will be set from implementation wrapper. No getters available
            // const after construction!!
            CallbackT callback = CallbackT();
            manager::dti::SkillManagerInterfacePrx manager = nullptr;
            std::string executorName = "";
 
        protected:
            // non-const params. Const after preparation. Getters are available
            mutable std::mutex parametersMutex;
            armarx::aron::data::DictPtr parameters = nullptr;
 
            // The descripion of the skill
            SkillDescription description;
 
            // Status variables
            std::atomic_bool constructing = true;
            std::atomic_bool initializing = false;
            std::atomic_bool preparing = false;
            std::atomic_bool running = false;
            std::atomic_bool exiting = false;
            std::atomic_bool finished = false;
 
            // Conditionals to indicate that an event has occured. Use conditions this way
            std::atomic_bool stopped = false;
            std::atomic_bool timeoutReached = false;
 
 
        private:
            // active conditions. First is condition (bool return func)
            mutable std::mutex conditionCallbacksMutex;
            std::vector<std::pair<std::function<bool()>, std::function<void()>>>
                conditionCallbacks = {};
 
            std::thread conditionCheckingThread; // A thread that checks the conditions frequently
            armarx::Frequency conditionCheckingThreadFrequency = armarx::Frequency::Hertz(20);
 
            mutable std::mutex subskillsMutex;
            std::vector<skills::SkillExecutionID> subskills;
        };
 
        template <class T>
        concept isSkill = std::is_base_of<Skill, T>::value;
 
    } // namespace skills
} // namespace armarx