StateMachine.h

Go to the documentation of this file.

// -*- C++ -*-
#ifndef RTC_STATEMACHINE_H
#define RTC_STATEMACHINE_H
 
#include <mutex>
 
#include <cassert>
#include <utility>
#include <vector>
 
namespace RTC_Utils
{
  template <class State>
  struct StateHolder
  {
    State curr;
    State prev;
    State next;
  };
 
  template <class State,
            class Listener,
            class States = StateHolder<State>,
            class Callback = void (Listener::*)(const States& states)
            >
  class StateMachine
  {
  public:
    explicit StateMachine(int num_of_state)
      : m_num(num_of_state),
        m_listener(nullptr),
        m_entry(m_num, (Callback)nullptr),
        m_predo(m_num, (Callback)nullptr),
        m_do(m_num, (Callback)nullptr),
        m_postdo(m_num, (Callback)nullptr),
        m_exit(m_num, (Callback)nullptr),
        m_transit(nullptr),
        m_selftrans(false)
    {
    }
 
    virtual ~StateMachine();
 
    StateMachine(const StateMachine& other)
      : m_num(other.m_num),
        m_listener(other.m_listener),
        m_entry (other.m_entry),
        m_predo (other.m_predo),
        m_do    (other.m_do),
        m_postdo(other.m_postdo),
        m_exit  (other.m_exit),
        m_transit(other.m_transit),
        m_states(other.m_states),
        m_selftrans(other.m_selftrans)
    {
    }
 
    StateMachine& operator=(const StateMachine& other)
    {
      StateMachine temp(other);
      swap(temp);
      return *this;
    }
 
    void swap(StateMachine& other)
    {
      std::swap(m_num,       other.m_num);
      std::swap(m_listener,  other.m_listener);
      std::swap(m_entry,     other.m_entry);
      std::swap(m_predo,     other.m_predo);
      std::swap(m_do,        other.m_do);
      std::swap(m_postdo,    other.m_postdo);
      std::swap(m_exit,      other.m_exit);
      std::swap(m_transit,   other.m_transit);
      std::swap(m_states,    other.m_states);
      std::swap(m_selftrans, other.m_selftrans);
    }
    void setNOP(Callback call_back)
    {
      setNullFunc(m_entry,  call_back);
      setNullFunc(m_do,     call_back);
      setNullFunc(m_exit,   call_back);
      setNullFunc(m_predo,  call_back);
      setNullFunc(m_postdo, call_back);
      m_transit = call_back;
    }
 
    void setListener(Listener* listener)
    {
      assert(listener != nullptr);
      m_listener = listener;
    }
 
    bool setEntryAction(State state, Callback call_back)
    {
     try
        {
          m_entry.at(state) = call_back;
        }
      catch (...)
        {
          assert(false);
          return false;
        }
      return true;
    }
 
    bool setPreDoAction(State state, Callback call_back)
    {
      try
        {
          m_predo.at(state) = call_back;
        }
      catch (...)
        {
          assert(false);
          return false;
        }
      return true;
    }
 
    bool setDoAction(State state, Callback call_back)
    {
      try
        {
          m_do.at(state) = call_back;
        }
      catch (...)
        {
          assert(false);
          return false;
        }
      return true;
    }
 
    bool setPostDoAction(State state, Callback call_back)
    {
      try
        {
          m_postdo.at(state) = call_back;
        }
      catch (...)
        {
          assert(false);
          return false;
        }
      return true;
    }
 
    bool setExitAction(State state, Callback call_back)
    {
      try
        {
          m_exit.at(state) = call_back;
        }
      catch (...)
        {
          assert(false);
          return false;
        }
      return true;
    }
 
    bool setTransitionAction(Callback call_back)
    {
      m_transit = call_back;
      return true;
    }
 
    void setStartState(States states)
    {
      m_states.curr = states.curr;
      m_states.prev = states.prev;
      m_states.next = states.next;
    }
 
    States getStates()
    {
      std::lock_guard<std::mutex> guard(m_mutex);
      return m_states;
    }
 
    State getState()
    {
      std::lock_guard<std::mutex> guard(m_mutex);
      return m_states.curr;
    }
 
    bool isIn(State state)
    {
      std::lock_guard<std::mutex> guard(m_mutex);
      return m_states.curr == state ? true : false;
    }
 
    void goTo(State state)
    {
      std::lock_guard<std::mutex> guard(m_mutex);
      m_states.next = state;
      if (m_states.curr == state)
        {
          m_selftrans  = true;
        }
    }
 
 
    void worker()
    {
      States state;
 
      sync(state);
 
      if (state.curr == state.next)
        {
          // pre-do
          if (m_predo[state.curr] != NULL)
            (m_listener->*m_predo[state.curr])(state);
 
          if (need_trans()) return;
 
          // do
          if (m_do[state.curr] != NULL)
            (m_listener->*m_do[state.curr])(state);
 
          if (need_trans()) return;
 
          // post-do
          if (m_postdo[state.curr] != NULL)
            (m_listener->*m_postdo[state.curr])(state);
        }
      else
        {
          if (m_exit[state.curr] != NULL)
            (m_listener->*m_exit[state.curr])(state);
 
          sync(state);
 
          if (state.curr != state.next)
            {
              state.curr = state.next;
              if (m_entry[state.curr] != NULL)
                (m_listener->*m_entry[state.curr])(state);
              update_curr(state.curr);
            }
        }
    }
 
    //============================================================
    // divided worker functions
    // The following divided worker functions have to be used together.
    // - worker_pre()
    // - worker_do()
    // - worker_post()
    //
    void worker_pre()
    {
      States state;
      sync(state);
      if (state.curr == state.next)
        {
          if (m_predo[state.curr] != nullptr)
            {
              (m_listener->*m_predo[state.curr])(state);
            }
          return;
        }
 
      // State changed
      if (m_exit[state.curr] != nullptr)
        {
          (m_listener->*m_exit[state.curr])(state);
        }
      sync(state);
      if (state.curr != state.next)
        {
          state.curr = state.next;
          if (m_entry[state.curr] != nullptr)
            {
              (m_listener->*m_entry[state.curr])(state);
            }
          update_curr(state.curr);
        }
    }
 
    void worker_do()
    {
      States state;
      sync(state);
      if (m_do[state.curr] != nullptr)
        {
          (m_listener->*m_do[state.curr])(state);
        }
    }
 
    void worker_post()
    {
      States state;
      sync(state);
      if (m_postdo[state.curr] != nullptr)
        {
          (m_listener->*m_postdo[state.curr])(state);
        }
    }
 
  protected:
    void setNullFunc(std::vector<Callback>& s, Callback nullfunc)
    {
      s.clear();
      for (size_t i(0); i < m_num; ++i)
        {
          s.emplace_back(nullfunc);
        }
    }
 
    int m_num;
 
    Listener* m_listener;
 
    std::vector<Callback> m_entry;
 
    std::vector<Callback> m_predo;
 
    std::vector<Callback> m_do;
 
    std::vector<Callback> m_postdo;
 
    std::vector<Callback> m_exit;
 
    Callback  m_transit;
 
    States m_states;
    bool m_selftrans;
    std::mutex m_mutex;
 
  private:
    inline void sync(States& st)
    {
      std::lock_guard<std::mutex> guard(m_mutex);
      st = m_states;
    }
 
    inline bool need_trans()
    {
      std::lock_guard<std::mutex> guard(m_mutex);
      return (m_states.curr != m_states.next);
    }
 
    inline void update_curr(const State curr)
    {
      std::lock_guard<std::mutex> guard(m_mutex);
      m_states.curr = curr;
    }
  };
 
  // No inline for gcc warning, too big
  template <class T, class U, class V, class W>
  StateMachine<T, U, V, W>::~StateMachine() = default;
 
} // namespace RTC_Utils
 
#endif  // RTC_STATEMACHINE_H