SimpleStatemachine.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ARCHES::ArmarXObjects::ARCHESArmar6GraspCandidateExecutor
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <type_traits>
#include <functional>
 
#include <ArmarXCore/util/CPPUtility/trace.h>
#include <ArmarXCore/util/CPPUtility/hash.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
namespace armarx
{
    template<class EnumT>
    class simple_state_machine;
 
    template<class EnumT>
    class state_t
    {
        static_assert(std::is_enum_v<EnumT>);
    public:
        using enum_t = EnumT;
 
        // *INDENT-OFF*
        auto previous_state   () const { return _previous_state   ; }
        auto current_state    () const { return _current_state    ; }
        auto is_initial_state () const { return _is_initial_state ; }
 
        void quit()
        {
            _quit = true;
        }
        // *INDENT-ON*
    private:
        friend class simple_state_machine<enum_t>;
        enum_t _previous_state;
        enum_t _current_state;
        bool _quit{false};
        bool _is_initial_state{true};
    };
 
    template<class EnumT>
    class simple_state_machine
    {
        static_assert(std::is_enum_v<EnumT>);
    public:
        using enum_t = EnumT;
        using state_t = ::armarx::state_t<enum_t>;
        using state_machine_t = simple_state_machine<enum_t>;
 
        using state_function_t = std::function<enum_t(state_t&)>;
        using transition_function_t = std::function<void(const state_t&)>;
        using hook_function_t = std::function<void(const state_t&)>;
 
 
    public:
        static void noop_transition(const state_t&) {}
 
        template<enum_t state>
        void configure_state(state_function_t f)
        {
            ARMARX_CHECK_EXPRESSION(!_states.count(state));
            _states[state] = std::move(f);
        }
 
        template<enum_t state0, enum_t state1>
        void configure_transition(transition_function_t f = noop_transition)
        {
            ARMARX_CHECK_EXPRESSION(!_transitions.count({state0, state1}));
            _transitions[ {state0, state1}] = std::move(f);
        }
 
        void set_post_transition_callback(hook_function_t f)
        {
            _post_transition_callback = std::move(f);
        }
 
        void set_pre_transition_callback(hook_function_t f)
        {
            _pre_transition_callback = std::move(f);
        }
 
        void set_post_state_exit_callback(hook_function_t f)
        {
            _post_state_exit_callback = std::move(f);
        }
 
        void set_pre_state_enter_callback(hook_function_t f)
        {
            _pre_state_enter_callback = std::move(f);
        }
        template<enum_t state0>
        state_t run()
        {
            ARMARX_TRACE;
            for (const auto& [edge, _] : _transitions)
            {
                ARMARX_TRACE;
                const auto [f, t] = edge;
                ARMARX_CHECK_EXPRESSION(_states.count(f)) << "No state " << f;
                ARMARX_CHECK_EXPRESSION(_states.count(t)) << "No state " << t;
            }
            state_t state;
            state._previous_state   = state0;
            state._current_state    = state0;
            state._is_initial_state = true;
            state._quit             = false;
 
            while (true)
            {
                ARMARX_TRACE;
                _pre_state_enter_callback(state);
                ARMARX_IMPORTANT << "Executing state '" << state._current_state << "'";
                const auto next = _states.at(state._current_state)(state);
                state._previous_state   = state._current_state;
                state._current_state    = next;
                state._is_initial_state = false;
                _post_state_exit_callback(state);
                if (state._quit)
                {
                    break;
                }
                const std::pair<enum_t, enum_t> t{state._previous_state, state._current_state};
                ARMARX_CHECK_EXPRESSION(_transitions.count(t))
                        << "No transition from " << state._previous_state << " to " << state._current_state;
                _pre_transition_callback(state);
                ARMARX_IMPORTANT << "Taking transition '" << state._previous_state << "' -> '" << state._current_state << "'";
                _transitions.at(t)(state);
                _post_transition_callback(state);
            }
            ARMARX_IMPORTANT << "Exiting while in state '" << state._current_state << "'";
            return state;
        }
 
    private:
        std::unordered_map<enum_t, state_function_t> _states;
        std::unordered_map<std::pair<enum_t, enum_t>, transition_function_t> _transitions;
 
        hook_function_t _pre_transition_callback = noop_transition;
        hook_function_t _post_transition_callback = noop_transition;
 
        hook_function_t _pre_state_enter_callback = noop_transition;
        hook_function_t _post_state_exit_callback = noop_transition;
    };
}