StateMachine.h

Go to the documentation of this file.

// -*- C++ -*-
#ifndef RTC_STATEMACHINE_H
#define RTC_STATEMACHINE_H

#include <rtm/RTC.h>
#include <coil/Mutex.h>
#include <coil/Guard.h>

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
  {
    typedef coil::Mutex Mutex;
    typedef coil::Guard<Mutex> Guard;
  public:
    StateMachine(int num_of_state)
      : m_num(num_of_state),
        m_entry (new Callback[m_num]),
        m_predo (new Callback[m_num]),
        m_do    (new Callback[m_num]),
        m_postdo(new Callback[m_num]),
        m_exit  (new Callback[m_num])
    {
      setNullFunc(m_entry,  NULL);
      setNullFunc(m_do,     NULL);
      setNullFunc(m_exit,   NULL);
      setNullFunc(m_predo,  NULL);
      setNullFunc(m_postdo, NULL);
      m_transit = NULL;
    };
    

    virtual ~StateMachine()
    {
      delete [] m_entry;
      delete [] m_predo;
      delete [] m_do;
      delete [] m_postdo;
      delete [] m_exit;
    };


    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)
    {
      m_listener = listener;
    }
    
    bool setEntryAction(State state, Callback call_back)
    {
      m_entry[state] = call_back;
      return true;
    }
    
    bool setPreDoAction(State state, Callback call_back)
    {
      m_predo[state] = call_back;
      return true;
    }
    
    bool setDoAction(State state, Callback call_back)
    {
      m_do[state] = call_back;
      return true;
    }
    
    bool setPostDoAction(State state, Callback call_back)
    {
      m_postdo[state] = call_back;
      return true;
    }
    
    bool setExitAction(State state, Callback call_back)
    {
      m_exit[state] = call_back;
      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()
    {
      Guard guard(m_mutex);
      return m_states;
    }
    
    State getState()
    {
      Guard guard(m_mutex);
      return m_states.curr;
    }
    
    bool isIn(State state)
    {
      Guard guard(m_mutex);
      return m_states.curr == state ? true : false;
    }
    
    void goTo(State state)
    {
      Guard 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);
            }
        }
    }
    
  protected:
    void setNullFunc(Callback* s, Callback nullfunc)
    {
      for (int i = 0; i < m_num; ++i) s[i] = nullfunc;
    }
    
    int m_num;
    
    Listener* m_listener;
    
    Callback* m_entry;
    
    Callback* m_predo;
    
    Callback* m_do;
    
    Callback* m_postdo;
    
    Callback* m_exit;
    
    Callback  m_transit;
    
    States m_states;
    bool m_selftrans;
    Mutex m_mutex;
    
  private:
    inline void sync(States& st)
    {
      Guard guard(m_mutex);
      st = m_states;
    }
    
    inline bool need_trans()
    {
      Guard guard(m_mutex);
      return (m_states.curr != m_states.next);
    }
    
    inline void update_curr(const State curr)
    {
      Guard guard(m_mutex);
      m_states.curr = curr;
    }
  };
}; // namespace RTC_Utils

#endif // RTC_STATEMACHINE_H