Orocos Real-Time Toolkit  2.9.0
StateMachine.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  tag: Peter Soetens Mon May 10 19:10:29 CEST 2004 StateMachine.cxx
3 
4  StateMachine.cxx - description
5  -------------------
6  begin : Mon May 10 2004
7  copyright : (C) 2004 Peter Soetens
8  email : peter.soetens@mech.kuleuven.ac.be
9 
10  ***************************************************************************
11  * This library is free software; you can redistribute it and/or *
12  * modify it under the terms of the GNU General Public *
13  * License as published by the Free Software Foundation; *
14  * version 2 of the License. *
15  * *
16  * As a special exception, you may use this file as part of a free *
17  * software library without restriction. Specifically, if other files *
18  * instantiate templates or use macros or inline functions from this *
19  * file, or you compile this file and link it with other files to *
20  * produce an executable, this file does not by itself cause the *
21  * resulting executable to be covered by the GNU General Public *
22  * License. This exception does not however invalidate any other *
23  * reasons why the executable file might be covered by the GNU General *
24  * Public License. *
25  * *
26  * This library is distributed in the hope that it will be useful, *
27  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
28  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
29  * Lesser General Public License for more details. *
30  * *
31  * You should have received a copy of the GNU General Public *
32  * License along with this library; if not, write to the Free Software *
33  * Foundation, Inc., 59 Temple Place, *
34  * Suite 330, Boston, MA 02111-1307 USA *
35  * *
36  ***************************************************************************/
37 #include "StateMachine.hpp"
38 #include "../ExecutionEngine.hpp"
39 #include "../internal/DataSource.hpp"
40 #include "../Service.hpp"
41 #include "CommandFunctors.hpp"
42 #include <Logger.hpp>
43 #include <functional>
44 
45 #include <assert.h>
46 #include <boost/bind.hpp>
47 #include <boost/tuple/tuple.hpp>
48 #include "internal/mystd.hpp"
49 
50 #define TRACE(msg) do { \
51  if (!mtrace) \
52  break; \
53  Logger::In in( _name ); \
54  log(Info) << '[' << this->getStatusStr() << ']' << std::string(" ") + msg <<endlog(); \
55 } while(0)
56 
57 namespace RTT {
58  using namespace detail;
59  using boost::tuples::get;
60  using namespace std;
61  using namespace boost;
62 
63 
64  std::string StateMachine::emptyString;
65 
66  StateMachine::StateMachine(StateMachinePtr parent, const string& name )
67  : smpStatus(nill), _parent (parent) , _name(name), smStatus(Status::unloaded),
68  initstate(0), finistate(0), current( 0 ), next(0), initc(0),
69  currentProg(0), currentExit(0), currentHandle(0), currentEntry(0), currentRun(0), currentTrans(0),
70  checking_precond(false), mstep(false), mtrace(false), evaluating(0)
71  {
72  this->addState(0); // allows global state transitions
73  }
74 
76  {
77  if ( this->isLoaded() ){
78  getEngine()->removeFunction(this);
79  }
80  delete initc;
81  TRACE( "StateMachine '" + _name + "' destroyed." );
82  }
83 
85  TRACE( "Being Loaded in ExecutionEngine." );
87  for(TransitionMap::iterator it=stateMap.begin(); it != stateMap.end(); ++it) {
88  // inform all entry/exit state scripts:
89  if (it->first)
90  it->first->loaded( this->getEngine() );
91  // inform all transition scripts
92  for(TransList::iterator tlit= it->second.begin(); tlit != it->second.end(); ++tlit ) {
93  if ( get<4>(*tlit) )
94  get<4>(*tlit)->loaded( this->getEngine() );
95  }
96  }
97  for( EventMap::iterator it = eventMap.begin(); it != eventMap.end(); ++it)
98  {
99  // inform all transition scripts
100  for(EventList::iterator tlit= it->second.begin(); tlit != it->second.end(); ++tlit ) {
101  if ( get<5>(*tlit) )
102  get<5>(*tlit)->loaded( this->getEngine() );
103  }
104  }
105  }
106 
108  TRACE( "Being unloaded from ExecutionEngine." );
109  if ( this->isActive() == false)
110  return;
111  if ( this->isReactive() )
112  this->requestFinalState();
113  if ( this->isAutomatic() )
114  this->stop();
115  if (this->currentState() != this->getFinalState() )
116  this->execute(); // try one last time
117  if (this->currentState() != this->getFinalState() )
118  log(Critical) << "Failed to bring StateMachine "<< this->getName()
119  << " into the final state. Program stalled in state '"
120  << this->currentState()->getName()<<"' line number "
121  << this->getLineNumber()<<endlog(); // critical failure !
122  }
123 
125  return smStatus;
126  }
127 
128  string StateMachine::getStatusStr() const {
129 
130  switch ( smStatus )
131  {
132  case Status::inactive:
133  return "inactive";
134  break;
135  case Status::stopping:
136  return "stopping";
137  break;
138  case Status::stopped:
139  return "stopped";
140  break;
141  case Status::requesting:
142  return "requesting";
143  break;
144  case Status::running:
145  return "running";
146  break;
147  case Status::paused:
148  return "paused";
149  break;
150  case Status::active:
151  return "active";
152  break;
153  case Status::activating:
154  return "activating";
155  break;
157  return "deactivating";
158  break;
159  case Status::resetting:
160  return "resetting";
161  break;
162  case Status::error:
163  return "error";
164  break;
165  case Status::unloaded:
166  return "unloaded";
167  break;
168  }
169  return "na";
170  }
171 
173  {
175  TRACE( "Will pause." );
176  if (currentProg) {
177  currentProg->pause();
178  currentProg->execute();
179  }
180  smpStatus = pausing;
181  return true;
182  }
183  TRACE( "Won't pause." );
184  return false;
185  }
186 
188  {
189  if ( smStatus == Status::paused && mstep == false ) {
190  TRACE( "Will step." );
191  mstep = true;
192  return true;
193  }
194  if ( smStatus == Status::active ) {
195  TRACE( "Will step." );
197  return true;
198  }
199  TRACE( "Won't step." );
200  return false;
201  }
202 
204  {
205  return this->automatic();
206  }
207 
209  {
210  // if you go from reactive to automatic,
211  // first execute the run program, before
212  // evaluating transitions.
214  TRACE( "Will start." );
216  os::MutexLock lock(execlock);
217  runState( current );
218  return true;
219  }
220  TRACE( "Won't start." );
221  return false;
222  }
223 
225  {
227  TRACE( "Will enter reactive mode." );
229  return true;
230  }
231  TRACE( "Won't enter reactive mode." );
232  return false;
233  }
234 
236  {
238  TRACE( "Will stop." );
239  smpStatus = gostop;
240  return true;
241  }
242  TRACE( "Won't stop." );
243  return false;
244  }
245 
247  {
248  // if waiting in final state, go ahead.
249  if ( smStatus == Status::stopped ) {
250  TRACE( "Will reset.");
251  smpStatus = goreset;
252  return true;
253  }
254  TRACE("Won't reset.");
255  return false;
256  }
257 
259  {
260  // before dealing with transitional states,
261  // check if we're actually running.
263  smpStatus = nill;
264  return true;
265  }
266  os::MutexLock lock(execlock);
267 
268  // internal transitional issues.
269  switch (smpStatus) {
270  case pausing:
271  TRACE("Is paused now.");
273  smpStatus = nill;
274  return true;
275  break;
276  case gostop:
277  this->executePending();
278  if ( this->requestFinalState() ) {
279  // test for atomicity :
280  if ( this->inTransition() ) {
282  } else {
283  TRACE("Is stopped now.");
285  }
286  smpStatus = nill;
287  }
288  return true;
289  break;
290  case goreset:
291  if ( this->executePending() ) {
292  this->requestInitialState();
293  if ( this->inTransition() ) {
295  } else {
296  TRACE("Is reset now.");
298  }
299  }
300  smpStatus = nill;
301  return true;
302  break;
303  case nill:
304  break;
305  }
306 
307  // public visible states.
308  switch (smStatus) {
309  case Status::inactive:
310  return true;
311  break;
312  case Status::requesting:
313  if ( this->executePending() ) { // if all steps done,
314  this->requestNextState();
315  this->executePending(); // execute steps of next state
316  TRACE("Is active now.");
318  }
319  break;
320  case Status::active:
321  this->executePending();
322  break;
323  case Status::running:
324  if ( this->executePending() == false) {
325  TRACE("Yielding...");
326  break;
327  }
328  // if all pending done:
329  this->requestNextState(); // one state at a time
330  this->executePending(); // execute steps of next state
331  break;
332  case Status::paused:
333  if (mstep) {
334  if ( this->executePending(true) ) { // if all steps done,
335  this->requestNextState(true); // one state at a time
336  this->executePending(true); // execute steps of next state
337  }
338  TRACE("Did a step.");
339  mstep = false;
340  }
341  break;
342  case Status::error:
343  case Status::unloaded:
344  break;
345  case Status::activating:
346  this->executePending();
347  if ( !this->inTransition() ) {
348  TRACE("Is active now.");
350  }
351  break;
352  case Status::stopping:
353  if ( this->executePending() ) {
354  TRACE("Is stopped now.");
356  }break;
358  if ( this->executePending() ) {
359  TRACE("Is inactive now.");
361  }break;
362  case Status::resetting:
363  if ( this->executePending() ) {
364  TRACE("Is reset now.");
366  }break;
367  case Status::stopped:
368  this->executePending();
369  if ( current != finistate ) {// detect leaving final state by event/request
371  } break;
372  }
373  return true;
374  }
375 
377  {
378  os::MutexLock lock(execlock);
379  // all conditions that must be satisfied to enter the initial state :
380  if ( interruptible() && ( current == initstate || current == finistate ) )
381  {
382  TRACE("Will enter initial state.");
383  // this will try to execute the state change atomically
384  this->requestStateChange( initstate );
385  return true;
386  }
387  TRACE("Won't enter initial state.");
388  return false;
389  }
390 
392  {
393  os::MutexLock lock(execlock);
394  // if we are inactive or in transition, don't do anything.
395  if ( current == 0 || ( !inError() && !interruptible() ) ) {
396  TRACE("Won't enter final state.");
397  return false;
398  }
399 
400  // this will try to execute the state change atomically
401  if ( this->requestStateChange( finistate ) ) {
402  TRACE("Will enter final state.");
403  return true;
404  }
405  TRACE("Won't enter final state.");
406  return false;
407  }
408 
409  void StateMachine::changeState(StateInterface* newState, ProgramInterface* transProg, bool stepping) {
410  if ( newState == current )
411  {
412  // this is only true if current state was selected in a transition of current.
413  if ( transProg ) {
414  TRACE("Transition triggered to self: '"+current->getName()+"'");
415  transProg->reset();
416  if (transProg->start() == false )
418  currentTrans = transProg;
419  // manually reset reqstep, or the next iteration would skip transition checks.
420  reqstep = stateMap.find( current )->second.begin();
421  // from now on, we are in transition to self !
422  // currentRun is _not_ set to zero or reset.
423  // it is/may be interrupted by trans, then continued.
424  // if already in handle, it is completed before trans, then trans is executed.
425  // since a transProg was given, no handle is executed.
426  } else {
427  // execute the default action (schedule handle )
428  // if no transition to another state took place and no transprog specified.
429  // only schedule a handle if not yet in progress.
430  if ( currentHandle == 0 )
431  handleState( current );
432  }
433  }
434  else
435  {
436  TRACE("Transition triggered from '"+ (current ? current->getName() : "null") +"' to '"+(newState ? newState->getName() : "null")+"'.");
437  // reset handle and run, in case it is still set ( during error
438  // or when an event arrived ).
439  currentRun = 0;
440  currentHandle = 0;
441  if ( transProg ) {
442  transProg->reset();
443  if ( transProg->start() == false )
445 
446  }
447  currentTrans = transProg;
448 
449  next = newState;
450 
451  disableEvents(current);
452  leaveState(current);
453  assert( currentEntry == 0); // we assume that in executePending, trans and exit get executed first and entry is stil null.
454  }
455  // schedule a run for the next 'step'.
456  // if handle above finished, run will be called directly
457  // in executePending. if handle was not finished
458  // or stepping, it will be called after handle.
459  runState( newState );
460  }
461 
463  {
464  enableEvents(0);
465  }
467  {
468  disableEvents(0);
469  }
471  {
472 // if (s) {
473 // TRACE("Enabling events for state '"+s->getName()+"'.");
474 // } else
475 // TRACE("Enabling global events.");
476  EventMap::mapped_type& hlist = eventMap[s];
477  for (EventList::iterator eit = hlist.begin();
478  eit != hlist.end();
479  ++eit) {
480  assert( get<6>(*eit).connected() == false );
481  get<6>(*eit).connect();
482  }
483  }
485  {
486 // if (s) {
487 // TRACE("Disabling events for state '"+s->getName()+"'.");
488 // } else
489 // TRACE("Disabling global events.");
490  EventMap::mapped_type& hlist = eventMap[s];
491  for (EventList::iterator eit = hlist.begin();
492  eit != hlist.end();
493  ++eit) {
494  assert( get<6>(*eit).connected() == true );
495  get<6>(*eit).disconnect();
496  }
497 
498  }
499 
501  const std::string& ename, vector<DataSourceBase::shared_ptr> args,
502  StateInterface* from, StateInterface* to,
503  ConditionInterface* guard, boost::shared_ptr<ProgramInterface> transprog,
504  StateInterface* elseto, boost::shared_ptr<ProgramInterface> elseprog )
505  {
506  Logger::In in("StateMachine::createEventTransition");
507  DisposableInterface::shared_ptr di = sp->getLocalOperation(ename);
508  OperationCallerInterface::shared_ptr oci = dynamic_pointer_cast<OperationCallerInterface>(di);
509  if ( !oci ) {
510  log(Error) << "Can not receive event '"<< ename <<"' in StateMachine : not a local operation."<< endlog();
511  return false;
512  }
513 
514  if ( !( sp && guard ) ) {
515  log(Error) << "Invalid arguments for event '"<< ename <<"'. ";
516  if (!sp)
517  log() <<"Service was null. ";
518  if (!guard)
519  log() <<"Guard Condition was null. ";
520  log() << endlog();
521  return false;
522  }
523 
524  if ( to == 0 )
525  to = from;
526 
527  // get ename from event service, provide args as arguments
528  // event should be activated upon entry of 'from'
529  // guard is evaluated to get final 'ok'.
530  // event does a requestStateChange( to );
531  // if 'ok', execute transprog during transition.
532 
533  // Store guard and transprog for copy/clone semantics.
534  // upon SM copy, recreate the handles for the copy SM with a copy of guard/transprog.
535  // Same for args. I guess we need to store all arguments of this function to allow
536  // proper copy semantics, such that the event handle can be created for each new SM
537  // instance. Ownership of guard and transprog is to be determined, but seems to ly
538  // with the SM. handle.destroy() can be called upon SM destruction.
539  Handle handle;
540 
541  log(Debug) << "Creating Signal handler for Operation '"<< ename <<"' from state "<< (from ? from->getName() : string("(global)")) << " to state " << ( to ? to->getName() : string("(global)") ) <<Logger::endl;
542 #ifdef ORO_SIGNALLING_OPERATIONS
543  // don't deliver this signal asynchronously, we want it right away.
544  handle = sp->produceSignal( ename, new CommandFunction( boost::bind( &StateMachine::eventTransition, this, from, guard, transprog.get(), to, elseprog.get(), elseto) ), args, 0 );
545 #endif
546  if ( !handle.ready() ) {
547  Logger::log() << Logger::Error << "Could not setup handle for event '"<<ename<<"'."<<Logger::endl;
548  return false; // event does not exist...
549  }
550  // all our handles must start in disconnected state:
551  handle.disconnect();
552  // BIG NOTE : we MUST store handle otherwise, the connection is destroyed (cfr setup vs connect).
553  // Off course, we also need it to connect/disconnect the event.
554  eventMap[from].push_back( boost::make_tuple( sp, ename, args, to, guard, transprog, handle, elseto, elseprog) );
555  // add the states to the statemap.
556  stateMap[from];
557  stateMap[to];
558  return true;
559  }
560 
561  bool StateMachine::eventTransition(StateInterface* from, ConditionInterface* c, ProgramInterface* p, StateInterface* to, ProgramInterface* elsep, StateInterface* elseto )
562  {
563  // called by event to begin Transition to 'to'.
564  // This interrupts the current run program at yield point
565  // the transition and/or exit program can cleanup...
566 
567  MutexLock lock(execlock); // recursive
568 
569  if ( !current)
570  return true;
571 
572  // Only transition if this event was meant for this state and we are not
573  // in transition already.
574  // If condition fails, check precondition 'else' state (if present) and
575  // execute else program (may be null).
576 
577  if (from == 0)
578  from = current;
579  if (to == 0)
580  to = current;
581  TRACE("Received Signal in state '"+ current->getName()+"' for transition from state '" + from->getName() + "' to state '" + to->getName() + "'");
582  if ( from == current && !this->inTransition() ) {
583  if ( c->evaluate() && checkConditions(to, false) == 1 ) {
584  TRACE( "Valid transition from " + from->getName() +
585  +" to "+to->getName()+".");
586  // stepping == false ! This executes the whole state transition
587  changeState( to, p, false ); // valid transition to 'to'. Q: should stepping be on or off ?
588  // trigger EE in order to force execution of the remainder of the state transition:
589  this->getEngine()->getActivity()->timeout();
590  }
591  else {
592  if ( c->evaluate() == false )
593  TRACE( "Rejected transition from " + from->getName() +
594  " to " + to->getName() +
595  " within state " + current->getName() + ": guards failed.");
596  if ( checkConditions(to,false) != 1 )
597  TRACE( "Rejected transition from " + from->getName() +
598  " to " + to->getName() +
599  " within state " + current->getName() + ": preconditions failed.");
600  }
601  }
602 #if 1
603  else {
604  if (this->inTransition() ) {
605  TRACE( "Rejected transition from " + from->getName() +
606  " within " + current->getName() + ": already in transition.");
607  } else {
608  TRACE( "Rejected transition from " + from->getName() +
609  + " within " + current->getName() + ": wrong state.");
610  }
611  }
612 #endif
613  return true;
614  }
615 
617  {
618  os::MutexLock lock(execlock);
619  // bad idea, user, don't run this if we're not active...
620  if( current == 0 )
621  return 0;
622  // only a run program may be interrupted...
623  if ( !interruptible() || currentTrans ) {
624  return current; // can not accept request, still in transition.
625  }
626 
627  // Reset global conditions.
628  TransList::const_iterator it, it1, it2;
629  it1 = stateMap.find( 0 )->second.begin();
630  it2 = stateMap.find( 0 )->second.end();
631 
632  if ( reqstep == stateMap.find( current )->second.begin() ) // avoid reseting too much in stepping mode.
633  for ( it= it1; it != it2; ++it)
634  get<0>(*it)->reset();
635 
636  if ( reqstep == reqend ) { // if nothing to evaluate, eval globals, then just handle()
637 
638  for ( ; it1 != it2; ++it1 )
639  if ( get<0>(*it1)->evaluate()
640  && checkConditions( get<1>(*it1) ) == 1 ) {
641  StateInterface* next = get<1>(*it1);
642  if ( next == 0 ) // handle current if no next
643  changeState( current, get<4>(*it1).get(), stepping );
644  else
645  changeState( next, get<4>(*it1).get(), stepping );
646  // the request was accepted
647  return current;
648  }
649  // no transition found: handle()
650  changeState( current, 0, stepping );
651  return current;
652  }
653 
654  // if we got here, at least one evaluation to check
655  do {
656  if ( get<0>(*reqstep)->evaluate() ) {
657  // evaluate() might call stop() or other sm functions:
658  if (reqstep == reqend )
659  return current;
660  // check preconds of target state :
661  int cres = checkConditions( get<1>(*reqstep), stepping );
662  if (cres == 0) {
663  break; // only returned in stepping
664  }
665  if( cres == 1) {
666  changeState( get<1>(*reqstep), get<4>(*reqstep).get(), stepping );
667  break; // valid transition
668  }
669  // if cres == -1 : precondition failed, increment reqstep...
670  }
671  if ( reqstep + 1 == reqend ) {
672  // to a state specified by the user (global)
673  for ( ; it1 != it2; ++it1 ) {
674  if ( get<0>(*it1)->evaluate() && checkConditions( get<1>(*it1) ) == 1 ) {
675  StateInterface* next = get<1>(*it1);
676  if ( next == 0) // handle current if no next
677  changeState( current, get<4>(*it1).get(), stepping );
678  else
679  changeState( next, get<4>(*it1).get(), stepping );
680  // the request was accepted
681  return current;
682  }
683  }
684  // no transition was found, reset and 'schedule' a handle :
685  reqstep = stateMap.find( current )->second.begin();
686  evaluating = get<3>(*reqstep);
687  changeState( current, 0, stepping );
688  break;
689  }
690  else {
691  ++reqstep;
692  evaluating = get<3>(*reqstep);
693  }
694  } while ( !stepping );
695 
696  return current;
697  }
698 
699  int StateMachine::checkConditions( StateInterface* state, bool stepping ) {
700 
701  // if the preconditions of \a state are checked the first time in stepping mode, reset the iterators.
702  if ( !checking_precond || !stepping ) {
703  prec_it = precondMap.equal_range(state); // state is the _target_ state
704  }
705 
706  // will be set to true if stepping below.
707  //checking_precond = false;
708 
709  while ( prec_it.first != prec_it.second ) {
710  if (checking_precond == false && stepping ) {
711  evaluating = prec_it.first->second.second; // indicate we will evaluate this line (if any).
712  checking_precond = true;
713  return 0;
714  }
715  if ( prec_it.first->second.first->evaluate() == false ) {
716  checking_precond = false;
717  return -1; // precondition failed
718  }
719  ++( prec_it.first );
720  if (stepping) {
721  if ( prec_it.first != prec_it.second )
722  evaluating = prec_it.first->second.second; // indicate we will evaluate the next line (if any).
723  checking_precond = true;
724  return 0; // not done yet.
725  }
726  }
727  checking_precond = false;
728  return 1; // success !
729  }
730 
731 
733  {
734  os::MutexLock lock(execlock);
735  // bad idea, user, don't run this if we're not active...
736  if ( current == 0 )
737  return 0;
738  TransList::const_iterator it1, it2;
739  it1 = stateMap.find( current )->second.begin();
740  it2 = stateMap.find( current )->second.end();
741 
742  for ( ; it1 != it2; ++it1 )
743  if ( get<0>(*it1)->evaluate() && checkConditions( get<1>(*it1)) == 1 ) {
744  return get<1>(*it1);
745  }
746 
747  // also check the global transitions.
748  it1 = stateMap.find( 0 )->second.begin();
749  it2 = stateMap.find( 0 )->second.end();
750 
751  for ( ; it1 != it2; ++it1 )
752  if ( get<0>(*it1)->evaluate() && checkConditions( get<1>(*it1)) == 1 ) {
753  return get<1>(*it1);
754  }
755 
756  return current;
757  }
758 
759  std::vector<std::string> StateMachine::getStateList() const {
760  vector<string> result;
761  vector<StateInterface*> sl;
762  StateInterface* dummy = 0;
763  transform( stateMap.begin(), stateMap.end(), back_inserter(sl), select1st<TransitionMap::value_type>() );
764  sl.erase( find(sl.begin(), sl.end(), dummy) );
765  transform( sl.begin(), sl.end(), back_inserter(result), boost::bind( &StateInterface::getName, _1 ) );
766  return result;
767  }
768 
770  {
771  stateMap[s];
772  }
773 
774 
775  StateInterface* StateMachine::getState(const string& name) const
776  {
777  TransitionMap::const_iterator it = stateMap.begin();
778  while ( it != stateMap.end() ) {
779  if ( it->first && it->first->getName() == name )
780  return it->first;
781  ++it;
782  }
783  return 0;
784  }
785 
787  {
788  os::MutexLock lock(execlock);
789  // bad idea, user, don't run this if we're not active...
790  if( current == 0 )
791  return false;
792 
793  if ( !interruptible() ) {
794  return false; // can not accept request, still in transition
795  }
796 
797  // we may make transition to next state :
798 
799  // to current state
800  if ( current == s_n )
801  {
802  changeState( s_n, 0 );
803  executePending();
804  return true;
805  }
806 
807  // between 2 states specified by the user.
808  TransList::iterator it, it1, it2;
809  it1 = stateMap.find( current )->second.begin();
810  it2 = stateMap.find( current )->second.end();
811 
812  for ( ; it1 != it2; ++it1 )
813  if ( get<1>(*it1) == s_n
814  && get<0>(*it1)->evaluate()
815  && checkConditions( s_n ) == 1 ) {
816  changeState( s_n, get<4>(*it1).get() );
817  // the request was accepted
818  executePending();
819  return true;
820  }
821 
822  // to a state specified by the user (global)
823  it1 = stateMap.find( 0 )->second.begin();
824  it2 = stateMap.find( 0 )->second.end();
825 
826  // reset all conditions
827  for ( it= it1; it != it2; ++it)
828  get<0>(*it)->reset();
829 
830  // evaluate them
831  for ( ; it1 != it2; ++it1 )
832  if ( get<1>(*it1) == s_n
833  && get<0>(*it1)->evaluate()
834  && checkConditions( s_n ) == 1 ) {
835  changeState( s_n, get<4>(*it1).get() );
836  // the request was accepted
837  executePending();
838  return true;
839  }
840 
841  // to final state
842  if ( finistate == s_n )
843  {
844  changeState( s_n, 0 );
845  executePending();
846  return true;
847  }
848 
849  // to inital state from final state
850  if ( initstate == s_n && current == finistate)
851  {
852  changeState( s_n, 0 );
853  executePending();
854  return true;
855  }
856 
857  // the request has failed.
858  return false;
859  }
860 
862  // if not activated, return first line.
863  // work around race condition that current[Prog] may become zero,
864  // thus first store in local variable :
865  StateInterface* statecopy = current;
866  if ( statecopy == 0 )
867  return 1;
868  ProgramInterface* copy = currentProg;
869  if ( copy )
870  return copy->getLineNumber();
871  if ( evaluating )
872  return evaluating;
873 
874  // if none of the above, return entry point :
875  return statecopy->getEntryPoint();
876  }
877 
878  string StateMachine::getText() const {
879  return string();
880  }
881 
883  {
884  // we must be inactive.
885  if ( current != 0)
886  return;
887  precondMap.insert( make_pair(state, make_pair( cnd, line)) );
888  stateMap[state]; // add to state map.
889  }
890 
891  void StateMachine::transitionSet( StateInterface* from, StateInterface* to, ConditionInterface* cnd, int priority, int line )
892  {
893  this->transitionSet( from, to, cnd, boost::shared_ptr<ProgramInterface>(), priority, line);
894  }
895 
897  ConditionInterface* cnd, boost::shared_ptr<ProgramInterface> transprog,
898  int priority, int line )
899  {
900  // we must be inactive.
901  if ( current != 0)
902  return;
903 
904  if (from) {
905  TRACE("Created transition from "+from->getName() +"' to '"+ to->getName()+"'");
906  } else {
907  TRACE("Created global transition to '"+ to->getName()+"'");
908  }
909  // insert both from and to in the statemap
910  TransList::iterator it;
911  for ( it= stateMap[from].begin(); it != stateMap[from].end() && get<2>(*it) >= priority; ++it)
912  ; // this ';' is intentional
913  stateMap[from].insert(it, boost::make_tuple( cnd, to, priority, line, transprog ) );
914  stateMap[to]; // insert empty vector for 'to' state.
915  }
916 
918  {
919  return current;
920  }
921 
923  {
924  return currentProg;
925  }
926 
928  {
929  assert(s);
930 // TRACE( "Planning to leave state " + s->getName() );
931  currentExit = s->getExitProgram();
932  if ( currentExit ) {
933  currentExit->reset();
934  if (currentExit->start() == false)
936  }
937  }
938 
940  {
941  assert(s);
942  currentRun = s->getRunProgram();
943  if ( currentRun ) {
944  currentRun->reset();
945  if (currentRun->start() == false)
947  }
948  }
949 
951  {
952  assert(s);
953  currentHandle = s->getHandleProgram();
954  if ( currentHandle ) {
955  currentHandle->reset();
956  if (currentHandle->start() == false)
958  }
959  }
960 
962  {
963  assert(s);
964 // TRACE( "Planning to enter state " + s->getName() );
965 
966  // Before a state is entered, all transitions are reset !
967  TransList::iterator it;
968  for ( it= stateMap.find(s)->second.begin(); it != stateMap.find(s)->second.end(); ++it)
969  get<0>(*it)->reset();
970 
971  currentEntry = s->getEntryProgram();
972  if ( currentEntry ) {
973  currentEntry->reset();
974  if (currentEntry->start() == false)
976  }
977  }
978 
979  bool StateMachine::executePending( bool stepping )
980  {
981  // This function has great resposibility, since it acts like
982  // a scheduler for pending requests. It tries to devise what to
983  // do on basis of the contents of variables (like current*, next,...).
984  // This is a somewhat
985  // fragile implementation but requires very little bookkeeping.
986  // if returns true : a transition (exit/entry) is done
987  // and a new state may be requested.
988 
989  if ( inError() )
990  return false;
991 
992  // first try to finish the current entry program, if any:
993  if ( currentEntry ) {
994  TRACE("Executing entry program of '"+ (current ? current->getName() : "(null)") +"'" );
995  if ( this->executeProgram(currentEntry, stepping) == false )
996  return false;
997  // done.
998  TRACE("Finished entry program of '"+ (current ? current->getName() : "(null)") +"'" );
999  // in stepping mode, delay 'true' one executePending().
1000  if ( stepping ) {
1001  currentProg = currentRun;
1002  return false;
1003  }
1004  }
1005 
1006  // if a transition has been scheduled, proceed directly instead of doing a run:
1007  if ( currentTrans ) {
1008  TRACE("Executing transition program from '"+ (current ? current->getName() : "(null)") + "' to '"+ ( next ? next->getName() : "(null)")+"'" );
1009  // exception : transition during handle, first finish handle !
1010  if ( currentHandle ) {
1011  if ( this->executeProgram(currentHandle, stepping) == false )
1012  return false;
1013  } else
1014  if ( this->executeProgram(currentTrans, stepping) == false )
1015  return false;
1016  // done.
1017  TRACE("Finished transition program from '"+ (current ? current->getName() : "(null)") + "' to '"+ ( next ? next->getName() : "(null)")+"'" );
1018  // in stepping mode, delay 'true' one executePending().
1019  if ( stepping ) {
1020  currentProg = currentExit ? currentExit : (currentEntry ? currentEntry : currentRun);
1021  return false;
1022  }
1023  }
1024 
1025  // last is exit
1026  if ( currentExit ) {
1027  TRACE("Executing exit program from '"+ (current ? current->getName() : "(null)") + "' (going to '"+ (next ? next->getName() : "(null)") +"')" );
1028  if ( this->executeProgram(currentExit, stepping) == false )
1029  return false;
1030  // done.
1031  TRACE("Finished exit program from '"+ (current ? current->getName() : "(null)") + "' (going to '"+ (next ? next->getName() : "(null)") +"')" );
1032  // in stepping mode, delay 'true' one executePending().
1033  if ( stepping ) {
1034  currentProg = (currentEntry ? currentEntry : currentRun);
1035  return false;
1036  }
1037  }
1038 
1039  // only reset the reqstep if we changed state.
1040  // if we did not change state, it will be reset in requestNextState().
1041  if ( current != next ) {
1042  if ( next ) {
1043  reqstep = stateMap.find( next )->second.begin();
1044  reqend = stateMap.find( next )->second.end();
1045  // init for getLineNumber() :
1046  if ( reqstep == reqend )
1047  evaluating = 0;
1048  else
1049  evaluating = get<3>(*reqstep);
1050  } else {
1051  current = 0;
1052  return true; // done if current == 0 !
1053  }
1054  // make change transition after exit of previous state:
1055  TRACE("Formally transitioning from '"+ (current ? current->getName() : "(null)") + "' to '"+ (next ? next->getName() : "(null)") +"'" );
1056 
1057  current = next;
1058  enterState(next);
1059  enableEvents(next);
1060  }
1061 
1062  // finally, execute the current Entry of the new state:
1063  if ( currentEntry ) {
1064  TRACE("Executing entry program of '"+ (current ? current->getName() : "(null)") +"'" );
1065  if ( this->executeProgram(currentEntry, stepping) == false )
1066  return false;
1067  // done.
1068  TRACE("Finished entry program of '"+ (current ? current->getName() : "(null)") +"'" );
1069  // in stepping mode, delay 'true' one executePending().
1070  if ( stepping ) {
1071  currentProg = currentRun;
1072  return false;
1073  }
1074  // Either way, if currentEntry was set here, we get out by returning true or false.
1075  // currentHandle is unlikely to be set, but currentRun is set by 'changeState' and
1076  // should only be executed the next execution cycle (if no transition is scheduled by then).
1077  return true;
1078  }
1079 
1080  // Handle is executed after the transitions failed.
1081  if ( currentHandle ) {
1082  TRACE("Executing handle program of '"+ (current ? current->getName() : "(null)") +"'" );
1083  if ( this->executeProgram(currentHandle, stepping) == false )
1084  return false;
1085  // done.
1086  TRACE("Finished handle program of '"+ (current ? current->getName() : "(null)") +"'" );
1087  // in stepping mode, delay 'true' one executePending().
1088  if ( stepping ) {
1089  currentProg = currentRun;
1090  return false;
1091  }
1092  }
1093 
1094  // Run is executed before the transitions.
1095  if ( currentRun ) {
1096  TRACE("Executing run program of '"+ (current ? current->getName() : "(null)") +"'" );
1097  if ( this->executeProgram(currentRun, stepping) == false )
1098  return false;
1099  // done.
1100  TRACE("Finished run program of '"+ (current ? current->getName() : "(null)") +"'" );
1101  // in stepping mode, delay 'true' one executePending().
1102  if ( stepping )
1103  return false;
1104  }
1105 
1106  return true; // all pending is done
1107  }
1108 
1110  {
1111  if ( cp == 0)
1112  return false;
1113  // execute this stateprogram and cleanup if needed.
1114  currentProg = cp;
1115  if ( stepping )
1116  currentProg->step();
1117  else
1118  currentProg->execute();
1119  if ( currentProg && currentProg->inError() ) {
1121  smpStatus = nill;
1122  TRACE("Encountered run-time error at line " << this->getLineNumber() );
1123  return false;
1124  }
1125 
1126  if ( currentProg && !currentProg->isStopped() )
1127  return false;
1128 
1129  cp = currentProg = 0;
1130  return true;
1131  }
1132 
1133 
1134  // in exit/entry or transition programs
1136  return ( (currentProg != 0) && (currentProg != currentRun) && (currentProg != currentHandle) ) || (current != next);
1137  }
1138 
1139  // only run program may be interrupted.
1141  return currentProg == 0 || currentProg == currentRun;
1142  }
1143 
1145  {
1146  initstate = s;
1147  stateMap[initstate];
1148  }
1149 
1151  {
1152  finistate = s;
1153  stateMap[finistate];
1154  }
1155 
1156  void StateMachine::trace(bool t) {
1157  mtrace =t;
1158  }
1159 
1161  {
1162  // inactive implies loaded, but check additionally if smp is at least active
1163  if ( smStatus != Status::inactive ) {
1164  TRACE("Won't activate: already active.");
1165  return false;
1166  }
1167  os::MutexLock lock(execlock);
1168 
1169  smpStatus = nill;
1170 
1171  if ( this->checkConditions( getInitialState() ) != 1 ) {
1172  TRACE("Won't activate: preconditions failed.");
1173  return false; //preconditions not met.
1174  }
1175 
1176  if ( initc ) {
1177  initc->reset();
1178  initc->readArguments();
1179  if ( initc->execute() == false ) {
1180  TRACE("Won't activate: Init Commands failed.");
1181  return false; // fail to activate.
1182  }
1183  }
1184 
1185  current = getInitialState();
1186  next = current;
1188  reqstep = stateMap.find( next )->second.begin();
1189  reqend = stateMap.find( next )->second.end();
1190 
1191  // Enable all event handlers
1193  enableEvents(current);
1194 
1195  // execute the entry program of the initial state.
1196  if ( !inError() ) {
1197  if ( this->executePending() ) {
1199  TRACE("Activated.");
1200  } else {
1201  if ( !inError() ) {
1202  TRACE("Still activating.");
1204  }
1205  }
1206  }
1207 
1208  return true;
1209  }
1210 
1211 
1213  {
1214  // the only time to refuse executing this is when we did set ourselves to inactive before.
1215  if ( smStatus == Status::inactive) {
1216  TRACE("Won't deactivate: already inactive.");
1217  return false;
1218  }
1219  os::MutexLock lock(execlock);
1220 
1221  // disable all event handlers
1223 
1224  // whatever state we are in, leave it.
1225  // but if current exit is in error, skip it alltogether.
1226  if ( currentExit && currentExit->inError() )
1227  currentExit = 0;
1228  if ( currentTrans && currentTrans->inError() )
1229  currentTrans = 0;
1230  // if we stalled, in previous deactivate
1231  // even skip/stop exit program.
1232  if ( next != 0 && current ) {
1233  leaveState( current );
1234  disableEvents(current);
1235  } else {
1236  currentExit = 0;
1237  currentTrans = 0;
1238  }
1239 
1240  // do not call enterState( 0 )
1241  currentProg = 0;
1242  currentEntry = 0;
1243  currentHandle = 0;
1244  currentRun = 0;
1245  next = 0;
1246 
1247  // this will execute the exitFunction (if any) and, if successfull,
1248  // set current to zero (using next).
1249  if ( this->executePending() ) {
1250  TRACE("Deactivated.");
1252  } else {
1253  TRACE("Still deactivating.");
1255  }
1256 
1257  return true;
1258  }
1259 }
ActivityInterface * getActivity() const
Query for the task this interface is run in.
bool executePending(bool stepping=false)
Execute any pending State (exit, entry, handle) programs.
void loading()
Informs this object that it got loaded in an ExecutionEngine.
void transitionSet(StateInterface *from, StateInterface *to, ConditionInterface *cnd, int priority, int line)
Express a possible transition from one state to another under a certain condition.
The interface class for operation callers.
virtual bool removeFunction(base::ExecutableInterface *f)
Remove a running function added with runFunction.
bool isAutomatic() const
Query if the state machine is reacting to events and evaluating transition conditions.
virtual void reset()
Reset this action.
bool isStopped() const
Returns true if the program is not executing (stopped) or not loaded.
std::vector< std::string > getStateList() const
Get a list of the names of all the present states.
void enableEvents(StateInterface *s)
bool isLoaded()
Returns true if this object is loaded in an ExecutionEngine.
virtual void reset()=0
Reset the execution point to the beginning of this program interface.
This interface represents the concept of a condition which can be evaluated and return true or false...
bool deactivate()
Stop this StateMachine.
bool interruptible() const
Inspect if the StateMachine is interruptible by events.
virtual ~StateMachine()
The destructor is virtual since ParsedStateMachine still inherits this class.
const std::string & getName() const
This method must be overloaded to get a useful hierarchy.
STL namespace.
virtual bool timeout()=0
Requests this Activity to wakeup and call step() + work(Timeout).
bool reset()
Reset the state machine from the final state to the initial state and wait for events or requests...
virtual void readArguments()=0
This is invoked some time before execute() at a time when the action may read its function arguments...
virtual int getEntryPoint() const =0
Get the beginning definition of this State.
virtual bool pause()=0
Pause or start-and-pause the execution of this program.
std::string getStatusStr() const
Get the status in a readable string format.
virtual const std::string & getName() const =0
Get the name of this state.
void setFinalState(StateInterface *s)
Set the final state of this StateMachine.
void disableEvents(StateInterface *s)
void preconditionSet(StateInterface *state, ConditionInterface *cnd, int line)
Express a precondition for entering a state.
virtual ProgramInterface * getEntryProgram() const =0
Get the entry program of this State.
boost::shared_ptr< OperationCallerInterface > shared_ptr
Use this type for shared pointer storage of an OperationCallerInterface object.
bool pause()
Pause the state machine.
An execution engine serialises (executes one after the other) the execution of all commands...
ProgramInterface * currentProgram() const
Retrieve the current program in execution.
virtual ProgramInterface * getHandleProgram() const =0
Get the handle program of this State.
#define TRACE(msg)
StateInterface * nextState()
Search from the current state a candidate next state.
bool execute()
Used by the StateMachineProcessor to execute the next action(s) or state transitions.
int getLineNumber() const
Returns the current program line in execution,.
virtual bool start()=0
Start the execution of this program.
bool step()
Execute a single action if the state machine is paused or evaluate the transition conditions if the s...
virtual bool step()=0
Execute a single action when paused.
void handleState(StateInterface *s)
virtual bool execute()=0
Execute as much actions until the program needs to wait on a condition to become true.
bool inError() const
Get the error status of this StateMachine.
Status::StateMachineStatus getStatus() const
Get the status of this state machine.
boost::shared_ptr< Service > ServicePtr
Definition: rtt-fwd.hpp:86
void addState(StateInterface *s)
Add a State.
virtual bool execute()=0
Execute the functionality of this action.
virtual int getLineNumber() const =0
Return the current &#39;line number&#39; of the program.
StateInterface * currentState() const
Retrieve the current state of the state machine.
Status::StateMachineStatus smStatus
static std::ostream & endl(std::ostream &__os)
Definition: Logger.cpp:383
void unloading()
Informs this object that it got unloaded from an ExecutionEngine.
bool executeProgram(ProgramInterface *&cp, bool stepping)
void trace(bool on_off)
Turn log(Debug) messages on or off to track state transitions.
boost::shared_ptr< StateMachine > StateMachinePtr
void changeState(StateInterface *s, ProgramInterface *tprog, bool stepping=false)
bool start()
Enter automatic mode: evaluating the transition conditions continuously.
boost::shared_ptr< DisposableInterface > shared_ptr
Use this type for shared pointer storage of an DisposableInterface object.
StateInterface * getInitialState() const
Retrieve the initial state of the state machine.
ExecutionEngine * getEngine()
Returns the ExecutionEngine this object is loaded into or null otherwise.
Enumerates all possible state machine statuses.
PreConditionMap precondMap
A map keeping track of all preconditions of a state.
virtual ProgramInterface * getExitProgram() const =0
Get the exit program of this State.
void runState(StateInterface *s)
bool requestInitialState()
Request going to the Initial State.
virtual ProgramInterface * getRunProgram() const =0
Get the run program of this State.
bool activate()
Start this StateMachine.
StateInterface * getState(const std::string &name) const
Lookup a State by name.
bool requestFinalState()
Request going to the Final State.
Notify the Logger in which &#39;module&#39; the message occured.
Definition: Logger.hpp:159
virtual bool evaluate()=0
Evaluate the Condition and return the outcome.
static Logger & log()
As Instance(), but more userfriendly.
Definition: Logger.cpp:117
bool isReactive() const
Query if the state machine is currently reacting only to events.
StateInterface * requestNextState(bool stepping=false)
Search from the current state a candidate next state.
void setInitialState(StateInterface *s)
Set the initial state of this StateMachine.
int checkConditions(StateInterface *state, bool stepping=false)
Contains TaskContext, Activity, OperationCaller, Operation, Property, InputPort, OutputPort, Attribute.
Definition: Activity.cpp:52
TransitionMap stateMap
A map keeping track of all States and conditional transitions between two states. ...
A functor with the base::ActionInterface, for the case where the functor is a bool(void).
A State contains an entry, run, handle and exit program.
A Program represents a collection of instructions that can be stepwise executed.
bool requestStateChange(StateInterface *s_n)
Request a state transition to a new state.
StateInterface * getFinalState() const
Retrieve the final state of the state machine.
bool isActive() const
Returns true if the state machine is activated.
The Handle holds the information, and allows manipulation, of a connection between a internal::Signal...
Definition: Handle.hpp:66
bool reactive()
Switch to reactive mode from automatic mode.
bool inTransition() const
Inspect if the StateMachine is performing a state transition.
bool inError() const
Returns true if the program is in error.
StateMachine(StateMachinePtr parent, const std::string &name="Default")
Create a new StateMachine with an optional parent.
bool automatic()
Enter automatic mode: evaluating the transition conditions continuously.
EventMap eventMap
A map keeping track of all events of a state.
bool stop()
Bring the state machine to the safe final state and wait for events or requests.
void enterState(StateInterface *s)
void leaveState(StateInterface *s)
MutexLock is a scope based Monitor, protecting critical sections with a Mutex object through locking ...
Definition: MutexLock.hpp:51
bool createEventTransition(ServicePtr sp, ExecutionEngine *target_engine, const std::string &ename, std::vector< base::DataSourceBase::shared_ptr > args, StateInterface *from, StateInterface *to, ConditionInterface *guard, boost::shared_ptr< ProgramInterface > transprog, StateInterface *elseto=0, boost::shared_ptr< ProgramInterface > elseprog=boost::shared_ptr< ProgramInterface >())
Express a possible transition from one state to another when an Event is fired under a certain condit...
virtual std::string getText() const
Return the text to which getLineNumber() refers.