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 <functional>
#include <type_traits>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/util/CPPUtility/hash.h>
#include <ArmarXCore/util/CPPUtility/trace.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;
    };
} // namespace armarx