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 
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 
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;
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() );
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);
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 
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 ) {
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 ) {
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 ) {
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 ) {
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 ) {
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;
1148  }
1149 
1151  {
1152  finistate = s;
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 
1186  next = current;
1188  reqstep = stateMap.find( next )->second.begin();
1189  reqend = stateMap.find( next )->second.end();
1190 
1191  // Enable all event handlers
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 );
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 }
bool executePending(bool stepping=false)
void transitionSet(StateInterface *from, StateInterface *to, ConditionInterface *cnd, int priority, int line)
ActivityInterface * getActivity() const
Query for the task this interface is run in.
virtual bool removeFunction(base::ExecutableInterface *f)
TransList::iterator reqend
ProgramInterface * currentHandle
ProgramInterface * currentEntry
std::vector< std::string > getStateList() const
void enableEvents(StateInterface *s)
This interface represents the concept of a condition which can be evaluated and return true or false...
const std::string & getName() const
Definition: mystd.hpp:163
ProgramInterface * currentRun
virtual void readArguments()=0
virtual int getEntryPoint() const =0
std::string getStatusStr() const
virtual const std::string & getName() const =0
Get the name of this state.
void setFinalState(StateInterface *s)
os::MutexRecursive execlock
enum RTT::scripting::StateMachine::PrivateStatus smpStatus
void disableEvents(StateInterface *s)
void preconditionSet(StateInterface *state, ConditionInterface *cnd, int line)
virtual ProgramInterface * getEntryProgram() const =0
boost::shared_ptr< OperationCallerInterface > shared_ptr
static std::string emptyString
ProgramInterface * currentProgram() const
virtual ProgramInterface * getHandleProgram() const =0
#define TRACE(msg)
StateInterface * nextState()
void handleState(StateInterface *s)
Status::StateMachineStatus getStatus() const
boost::shared_ptr< Service > ServicePtr
Definition: rtt-fwd.hpp:86
void addState(StateInterface *s)
virtual bool execute()=0
virtual int getLineNumber() const =0
ProgramInterface * currentExit
StateInterface * currentState() const
Status::StateMachineStatus smStatus
static std::ostream & endl(std::ostream &__os)
Definition: Logger.cpp:383
bool executeProgram(ProgramInterface *&cp, bool stepping)
boost::shared_ptr< StateMachine > StateMachinePtr
void changeState(StateInterface *s, ProgramInterface *tprog, bool stepping=false)
boost::shared_ptr< DisposableInterface > shared_ptr
StateInterface * getInitialState() const
std::pair< PreConditionMap::const_iterator, PreConditionMap::const_iterator > prec_it
virtual ProgramInterface * getExitProgram() const =0
void runState(StateInterface *s)
virtual ProgramInterface * getRunProgram() const =0
StateInterface * getState(const std::string &name) const
virtual bool evaluate()=0
Evaluate the Condition and return the outcome.
static Logger & log()
Definition: Logger.cpp:117
base::ActionInterface * initc
StateInterface * requestNextState(bool stepping=false)
bool eventTransition(StateInterface *from, ConditionInterface *c, ProgramInterface *p, StateInterface *to, ProgramInterface *elsep, StateInterface *elseto)
void setInitialState(StateInterface *s)
int checkConditions(StateInterface *state, bool stepping=false)
TransList::iterator reqstep
Contains TaskContext, Activity, OperationCaller, Operation, Property, InputPort, OutputPort, Attribute.
Definition: Activity.cpp:53
A Program represents a collection of instructions that can be stepwise executed.
bool requestStateChange(StateInterface *s_n)
StateInterface * getFinalState() const
static Logger & log()
Definition: Logger.hpp:350
The Handle holds the information, and allows manipulation, of a connection between a internal::Signal...
Definition: Handle.hpp:66
StateMachine(StateMachinePtr parent, const std::string &name="Default")
static Logger::LogFunction endlog()
Definition: Logger.hpp:362
ProgramInterface * currentProg
void enterState(StateInterface *s)
ProgramInterface * currentTrans
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 >())
virtual std::string getText() const


rtt
Author(s): RTT Developers
autogenerated on Tue Jun 25 2019 19:33:36