00001
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 #ifndef StateMachine_h
00047 #define StateMachine_h
00048
00049 #include <rtm/RTC.h>
00050
00051 #include <ace/Guard_T.h>
00052 #include <ace/Thread_Mutex.h>
00053
00054 template <class State>
00055 struct StateHolder
00056 {
00057 State curr;
00058 State prev;
00059 State next;
00060 };
00061
00154 template <class State,
00155 class Listener,
00156 class States = StateHolder<State>,
00157 class Callback = void (Listener::*)(const States& states)
00158 >
00159 class StateMachine
00160 {
00161 public:
00169 StateMachine(int num_of_state)
00170 : m_num(num_of_state),
00171 m_entry (new Callback[m_num]),
00172 m_predo (new Callback[m_num]),
00173 m_do (new Callback[m_num]),
00174 m_postdo(new Callback[m_num]),
00175 m_exit (new Callback[m_num])
00176 {
00177 setNullFunc(m_entry, NULL);
00178 setNullFunc(m_do, NULL);
00179 setNullFunc(m_exit, NULL);
00180 setNullFunc(m_predo, NULL);
00181 setNullFunc(m_postdo, NULL);
00182 m_transit = NULL;
00183 };
00184
00185
00193 void setNOP(Callback call_back)
00194 {
00195 setNullFunc(m_entry, call_back);
00196 setNullFunc(m_do, call_back);
00197 setNullFunc(m_exit, call_back);
00198 setNullFunc(m_predo, call_back);
00199 setNullFunc(m_postdo, call_back);
00200 m_transit = call_back;
00201 }
00202
00203
00211 void setListener(Listener* listener)
00212 {
00213 m_listener = listener;
00214 }
00215
00216
00224 bool setEntryAction(State state, Callback call_back)
00225 {
00226 m_entry[state] = call_back;
00227 return true;
00228 }
00229
00230
00238 bool setPreDoAction(State state, Callback call_back)
00239 {
00240 m_predo[state] = call_back;
00241 return true;
00242 }
00243
00244
00252 bool setDoAction(State state, Callback call_back)
00253 {
00254 m_do[state] = call_back;
00255 return true;
00256
00257 }
00258
00259
00267 bool setPostDoAction(State state, Callback call_back)
00268 {
00269 m_postdo[state] = call_back;
00270 return true;
00271 }
00272
00273
00281 bool setExitAction(State state, Callback call_back)
00282 {
00283 m_exit[state] = call_back;
00284 return true;
00285 }
00286
00287
00295 bool setTransitionAction(Callback call_back)
00296 {
00297 m_transit = call_back;
00298 return true;
00299 }
00300
00301
00309 void setStartState(States states)
00310 {
00311 m_states.curr = states.curr;
00312 m_states.prev = states.prev;
00313 m_states.next = states.next;
00314 }
00315
00316
00324 States getStates()
00325 {
00326 ACE_Guard<ACE_Thread_Mutex> guard(m_mutex);
00327 return m_states;
00328 }
00329
00330 State getState()
00331 {
00332 ACE_Guard<ACE_Thread_Mutex> guard(m_mutex);
00333 return m_states.curr;
00334 }
00335
00336
00344 bool isIn(State state)
00345 {
00346 ACE_Guard<ACE_Thread_Mutex> guard(m_mutex);
00347 return m_states.curr == state ? true : false;
00348 }
00349
00350
00358 void goTo(State state)
00359 {
00360 ACE_Guard<ACE_Thread_Mutex> guard(m_mutex);
00361 m_states.next = state;
00362 if (m_states.curr == state)
00363 {
00364 m_selftrans = true;
00365 }
00366 }
00367
00368
00376 void worker()
00377 {
00378 States state;
00379
00380 sync(state);
00381
00382 if (state.curr == state.next)
00383 {
00384
00385 if (m_predo[state.curr] != NULL)
00386 (m_listener->*m_predo [state.curr])(state);
00387
00388 if (need_trans()) return;
00389
00390
00391 if (m_do[state.curr] != NULL)
00392 (m_listener->*m_do [state.curr])(state);
00393
00394 if (need_trans()) return;
00395
00396
00397 if (m_postdo[state.curr] != NULL)
00398 (m_listener->*m_postdo[state.curr])(state);
00399 }
00400 else
00401 {
00402 if (m_exit[state.curr] != NULL)
00403 (m_listener->*m_exit[state.curr])(state);
00404
00405 sync(state);
00406
00407 if (state.curr != state.next)
00408 {
00409 state.curr = state.next;
00410 if(m_entry[state.curr] != NULL)
00411 (m_listener->*m_entry[state.curr])(state);
00412 update_curr(state.curr);
00413 }
00414 }
00415 }
00416
00417 protected:
00418 void setNullFunc(Callback* s, Callback nullfunc)
00419 {
00420 for (int i = 0; i < m_num; ++i) s[i] = nullfunc;
00421 }
00422
00423
00424 int m_num;
00425 Listener* m_listener;
00426 Callback* m_entry;
00427 Callback* m_predo;
00428 Callback* m_do;
00429 Callback* m_postdo;
00430 Callback* m_exit;
00431 Callback m_transit;
00432
00433 States m_states;
00434 bool m_selftrans;
00435 ACE_Thread_Mutex m_mutex;
00436
00437 private:
00438 inline void sync(States& st)
00439 {
00440 ACE_Guard<ACE_Thread_Mutex> guard(m_mutex);
00441 st = m_states;
00442 }
00443
00444 inline bool need_trans()
00445 {
00446 ACE_Guard<ACE_Thread_Mutex> guard(m_mutex);
00447 return (m_states.curr != m_states.next);
00448 }
00449
00450 inline void update_curr(const State curr)
00451 {
00452 ACE_Guard<ACE_Thread_Mutex> guard(m_mutex);
00453 m_states.curr = curr;
00454 }
00455 };
00456
00457 #endif // StateMachine_h