Skill.h

Go to the documentation of this file.

#pragma once
 
#include <atomic>
#include <functional>
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <thread>
#include <type_traits>
#include <utility>
#include <vector>
 
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/core/time/Frequency.h>
 
#include <RobotAPI/interface/skills/SkillManagerInterface.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/skills/client/SkillExecutorInterface.h>
#include <RobotAPI/libraries/skills/core/ProviderID.h>
#include <RobotAPI/libraries/armem_task_outcome/types.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>
 
namespace armarx
{
    namespace skills
    {
        using SkillExecutionHandlePtr = std::unique_ptr<class SkillExecutionHandle>;
 
        /**
         * @brief Base class for skills.
         */
        class Skill : public armarx::Logging, protected client::SkillExecutorInterface
        {
            using SkillProxyPtr = std::unique_ptr<class SkillProxy>;
 
        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
            ~Skill() override;
 
            /// Get the id of the skill
            SkillID getSkillId() const;
 
            /// Get the description of a skill
            SkillDescription getSkillDescription() const;
 
            /// 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);
 
            void setCallback(const CallbackT& callback);
 
            void setManager(const manager::dti::SkillManagerInterfacePrx& manager);
 
            void setExecutorName(const std::string& executorName);
 
            void updateSubSkillStatus(const skills::SkillStatusUpdate& statusUpdate);
 
            /// 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 override;
 
            /// 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 = "") const;
            void throwIfSkillShouldTerminate(const std::function<void()>& do_before,
                                             const std::string& abortedMessage = "") const;
 
            static MainResult MakeSucceededResult(aron::data::DictPtr data = nullptr);
            static MainResult MakeFailedResult(aron::data::DictPtr data = nullptr);
            static MainResult MakeAbortedResult(aron::data::DictPtr data = nullptr);
 
            // 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();
 
            std::optional<TerminatedSkillStatusUpdate>
            executeSkill(const ::armarx::skills::SkillID& skillID,
                         std::function<aron::data::DictPtr(aron::data::DictPtr&)> const&
                             parametersFunction) override;
 
        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();
 
        public:
            /// install a condition which is frequently checked from the conditionCheckingThread
            void installConditionWithCallback(std::function<bool()>&& f,
                                              std::function<void()>&& cb);
 
            void waitFor(const armarx::Duration& duration,
                         const armarx::Duration& interval =
                             armarx::Duration::MilliSeconds(200)) const override;
 
            // Calling subskills
 
            [[nodiscard]] SkillExecutionHandlePtr
            callSubskillAsync(const SkillID& skillId,
                              std::function<void(aron::data::DictPtr&)> parametersFunction);
 
            [[nodiscard]] SkillExecutionHandlePtr callSubskillAsync(const SkillID& skillId);
 
            /**
             * @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)
            {
                SkillProxyPtr proxy = std::make_unique<SkillProxy>(manager, skillId);
 
                ParameterT parameters;
                if (auto parametersAron = proxy->getRootProfileParameters())
                {
                    parameters = ParameterT::FromAron(parametersAron);
                }
 
                parametersFunction(parameters);
 
                return callSubskill(std::move(proxy), parameters.toAron());
            }
 
            /// Similar to callSubskill but non-blocking
            template <class ParameterT>
            [[nodiscard]] SkillExecutionHandlePtr
            callSubskillAsync(const SkillID& skillId,
                              std::function<void(ParameterT& parameters)> parametersFunction)
            {
                SkillProxyPtr proxy = std::make_unique<SkillProxy>(manager, skillId);
                return callSubskillAsync(std::move(proxy), parametersFunction);
            }
 
            [[nodiscard]] SkillExecutionHandlePtr
            callSubskillAsync(const SkillID& skillId, const aron::data::DictPtr& parameters);
 
            // FIXME: make these protected again
            // private:
        public:
            /**
             * @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(SkillProxyPtr 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(SkillProxyPtr proxy, const aron::data::DictPtr& parameters);
 
            /// Similar to callSubskill but non-blocking
            [[nodiscard]] SkillExecutionHandlePtr callSubskillAsync(SkillProxyPtr proxy);
 
            /// Similar to callSubskill but non-blocking
            [[nodiscard]] SkillExecutionHandlePtr
            callSubskillAsync(SkillProxyPtr proxy, const aron::data::DictPtr& parameters);
 
            template <class ParameterT>
            SkillExecutionHandlePtr
            callSubskillAsync(SkillProxyPtr proxy,
                              std::function<void(ParameterT& parameters)> parametersFunction)
            {
                ParameterT parameters;
                if (auto parametersAron = proxy->getRootProfileParameters())
                {
                    parameters = ParameterT::FromAron(parametersAron);
                }
 
                parametersFunction(parameters);
 
                return callSubskillAsync(std::move(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;
 
            // Task outcome tracking for the current skill execution
            armem::task_outcome::TaskOutcome taskOutcome_;
 
            // 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;
 
        public:
            /**
             * @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.
             */
            [[deprecated("callSubskill(proxy) should be replaced by callSubskill(skillId)")]] std::
                optional<TerminatedSkillStatusUpdate>
                callSubskill(const skills::SkillProxy& proxy)
            {
                return callSubskill(proxy.getSkillId());
            }
 
            /**
             * @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.
             */
            [[deprecated(
                "callSubskill(proxy, parameters) should be replaced by "
                "callSubskill(skillId, parameters)")]] std::optional<TerminatedSkillStatusUpdate>
            callSubskill(const skills::SkillProxy& proxy, const aron::data::DictPtr& parameters)
            {
                return callSubskill(proxy.getSkillId(), parameters);
            }
 
            /// Similar to callSubskill but non-blocking
            template <class ParameterT>
            [[deprecated("callSubskillAsync(proxy, parametersFunction) should be replaced by "
                         "callSubskillAsync(skillId, parametersFunction)")]] SkillExecutionHandlePtr
            callSubskillAsync(const SkillProxy& proxy,
                              std::function<void(ParameterT& parameters)> parametersFunction)
            {
                ParameterT parameters;
                if (auto parametersAron = proxy.getRootProfileParameters())
                {
                    parameters = ParameterT::FromAron(parametersAron);
                }
 
                parametersFunction(parameters);
 
                return callSubskillAsync(proxy.getSkillId(), parameters.toAron());
            }
 
            [[deprecated("callSubskillAsync(proxy, parameters) should be replaced by "
                         "callSubskillAsync(skillId, parameters)")]] SkillExecutionHandlePtr
            callSubskillAsync(const skills::SkillProxy& proxy,
                              const aron::data::DictPtr& parameters)
            {
                return callSubskillAsync(proxy.getSkillId(), parameters);
            }
        };
 
        template <class T>
        concept isSkill = std::is_base_of<Skill, T>::value;
 
    } // namespace skills
} // namespace armarx