OpenRTM
1.0.0
|
00001 // -*- C++ -*- 00019 #ifndef RTC_STATEMACHINE_H 00020 #define RTC_STATEMACHINE_H 00021 00022 #include <rtm/RTC.h> 00023 #include <coil/Mutex.h> 00024 #include <coil/Guard.h> 00025 00026 namespace RTC_Utils 00027 { 00054 template <class State> 00055 struct StateHolder 00056 { 00057 State curr; 00058 State prev; 00059 State next; 00060 }; 00061 00258 template <class State, 00259 class Listener, 00260 class States = StateHolder<State>, 00261 class Callback = void (Listener::*)(const States& states) 00262 > 00263 class StateMachine 00264 { 00265 typedef coil::Mutex Mutex; 00266 typedef coil::Guard<Mutex> Guard; 00267 public: 00285 StateMachine(int num_of_state) 00286 : m_num(num_of_state), 00287 m_entry (new Callback[m_num]), 00288 m_predo (new Callback[m_num]), 00289 m_do (new Callback[m_num]), 00290 m_postdo(new Callback[m_num]), 00291 m_exit (new Callback[m_num]) 00292 { 00293 setNullFunc(m_entry, NULL); 00294 setNullFunc(m_do, NULL); 00295 setNullFunc(m_exit, NULL); 00296 setNullFunc(m_predo, NULL); 00297 setNullFunc(m_postdo, NULL); 00298 m_transit = NULL; 00299 }; 00300 00301 00302 virtual ~StateMachine() 00303 { 00304 delete [] m_entry; 00305 delete [] m_predo; 00306 delete [] m_do; 00307 delete [] m_postdo; 00308 delete [] m_exit; 00309 }; 00310 00311 00329 void setNOP(Callback call_back) 00330 { 00331 setNullFunc(m_entry, call_back); 00332 setNullFunc(m_do, call_back); 00333 setNullFunc(m_exit, call_back); 00334 setNullFunc(m_predo, call_back); 00335 setNullFunc(m_postdo, call_back); 00336 m_transit = call_back; 00337 } 00338 00356 void setListener(Listener* listener) 00357 { 00358 m_listener = listener; 00359 } 00360 00385 bool setEntryAction(State state, Callback call_back) 00386 { 00387 m_entry[state] = call_back; 00388 return true; 00389 } 00390 00414 bool setPreDoAction(State state, Callback call_back) 00415 { 00416 m_predo[state] = call_back; 00417 return true; 00418 } 00419 00443 bool setDoAction(State state, Callback call_back) 00444 { 00445 m_do[state] = call_back; 00446 return true; 00447 } 00448 00472 bool setPostDoAction(State state, Callback call_back) 00473 { 00474 m_postdo[state] = call_back; 00475 return true; 00476 } 00477 00501 bool setExitAction(State state, Callback call_back) 00502 { 00503 m_exit[state] = call_back; 00504 return true; 00505 } 00506 00530 bool setTransitionAction(Callback call_back) 00531 { 00532 m_transit = call_back; 00533 return true; 00534 } 00535 00553 void setStartState(States states) 00554 { 00555 m_states.curr = states.curr; 00556 m_states.prev = states.prev; 00557 m_states.next = states.next; 00558 } 00559 00580 States getStates() 00581 { 00582 Guard guard(m_mutex); 00583 return m_states; 00584 } 00585 00603 State getState() 00604 { 00605 Guard guard(m_mutex); 00606 return m_states.curr; 00607 } 00608 00630 bool isIn(State state) 00631 { 00632 Guard guard(m_mutex); 00633 return m_states.curr == state ? true : false; 00634 } 00635 00662 void goTo(State state) 00663 { 00664 Guard guard(m_mutex); 00665 m_states.next = state; 00666 if (m_states.curr == state) 00667 { 00668 m_selftrans = true; 00669 } 00670 } 00671 00672 00689 void worker() 00690 { 00691 States state; 00692 00693 sync(state); 00694 00695 if (state.curr == state.next) 00696 { 00697 // pre-do 00698 if (m_predo[state.curr] != NULL) 00699 (m_listener->*m_predo [state.curr])(state); 00700 00701 if (need_trans()) return; 00702 00703 // do 00704 if (m_do[state.curr] != NULL) 00705 (m_listener->*m_do [state.curr])(state); 00706 00707 if (need_trans()) return; 00708 00709 // post-do 00710 if (m_postdo[state.curr] != NULL) 00711 (m_listener->*m_postdo[state.curr])(state); 00712 } 00713 else 00714 { 00715 if (m_exit[state.curr] != NULL) 00716 (m_listener->*m_exit[state.curr])(state); 00717 00718 sync(state); 00719 00720 if (state.curr != state.next) 00721 { 00722 state.curr = state.next; 00723 if(m_entry[state.curr] != NULL) 00724 (m_listener->*m_entry[state.curr])(state); 00725 update_curr(state.curr); 00726 } 00727 } 00728 } 00729 00730 protected: 00750 void setNullFunc(Callback* s, Callback nullfunc) 00751 { 00752 for (int i = 0; i < m_num; ++i) s[i] = nullfunc; 00753 } 00754 00762 int m_num; 00763 00771 Listener* m_listener; 00772 00780 Callback* m_entry; 00781 00789 Callback* m_predo; 00790 00798 Callback* m_do; 00799 00807 Callback* m_postdo; 00808 00816 Callback* m_exit; 00817 00825 Callback m_transit; 00826 00834 States m_states; 00835 bool m_selftrans; 00836 Mutex m_mutex; 00837 00838 private: 00839 inline void sync(States& st) 00840 { 00841 Guard guard(m_mutex); 00842 st = m_states; 00843 } 00844 00845 inline bool need_trans() 00846 { 00847 Guard guard(m_mutex); 00848 return (m_states.curr != m_states.next); 00849 } 00850 00851 inline void update_curr(const State curr) 00852 { 00853 Guard guard(m_mutex); 00854 m_states.curr = curr; 00855 } 00856 }; 00857 }; // namespace RTC_Utils 00858 00859 #endif // RTC_STATEMACHINE_H