00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "StateMachine.hpp"
00038 #include "../ExecutionEngine.hpp"
00039 #include "../internal/DataSource.hpp"
00040 #include "../Service.hpp"
00041 #include "CommandFunctors.hpp"
00042 #include <Logger.hpp>
00043 #include <functional>
00044
00045 #include <assert.h>
00046 #include <boost/bind.hpp>
00047 #include <boost/tuple/tuple.hpp>
00048 #include "internal/mystd.hpp"
00049
00050 #define TRACE_INIT() Logger::In in( _name )
00051 #define TRACE(msg) if (mtrace) log(Info) << '[' << this->getStatusStr() << ']' << std::string(" ") + msg <<endlog()
00052
00053 namespace RTT {
00054 using namespace detail;
00055 using boost::tuples::get;
00056 using namespace std;
00057 using namespace boost;
00058
00059
00060 std::string StateMachine::emptyString;
00061
00062 StateMachine::StateMachine(StateMachinePtr parent, const string& name )
00063 : smpStatus(nill), _parent (parent) , _name(name), smStatus(Status::unloaded),
00064 initstate(0), finistate(0), current( 0 ), next(0), initc(0),
00065 currentProg(0), currentExit(0), currentHandle(0), currentEntry(0), currentRun(0), currentTrans(0),
00066 checking_precond(false), mstep(false), mtrace(false), evaluating(0)
00067 {
00068 this->addState(0);
00069 }
00070
00071 StateMachine::~StateMachine()
00072 {
00073 TRACE_INIT();
00074 if ( this->isLoaded() ){
00075 getEngine()->removeFunction(this);
00076 }
00077 delete initc;
00078 TRACE( "StateMachine '" + _name + "' destroyed." );
00079 }
00080
00081 void StateMachine::loading() {
00082 TRACE_INIT();
00083 TRACE( "Being Loaded in ExecutionEngine." );
00084 smStatus = Status::inactive;
00085 for(TransitionMap::iterator it=stateMap.begin(); it != stateMap.end(); ++it) {
00086
00087 if (it->first)
00088 it->first->loaded( this->getEngine() );
00089
00090 for(TransList::iterator tlit= it->second.begin(); tlit != it->second.end(); ++tlit ) {
00091 if ( get<4>(*tlit) )
00092 get<4>(*tlit)->loaded( this->getEngine() );
00093 }
00094 }
00095 for( EventMap::iterator it = eventMap.begin(); it != eventMap.end(); ++it)
00096 {
00097
00098 for(EventList::iterator tlit= it->second.begin(); tlit != it->second.end(); ++tlit ) {
00099 if ( get<5>(*tlit) )
00100 get<5>(*tlit)->loaded( this->getEngine() );
00101 }
00102 }
00103 }
00104
00105 void StateMachine::unloading() {
00106 TRACE_INIT();
00107 TRACE( "Being unloaded from ExecutionEngine." );
00108 if ( this->isActive() == false)
00109 return;
00110 if ( this->isReactive() )
00111 this->requestFinalState();
00112 if ( this->isAutomatic() )
00113 this->stop();
00114 if (this->currentState() != this->getFinalState() )
00115 this->execute();
00116 if (this->currentState() != this->getFinalState() )
00117 log(Critical) << "Failed to bring StateMachine "<< this->getName()
00118 << " into the final state. Program stalled in state '"
00119 << this->currentState()->getName()<<"' line number "
00120 << this->getLineNumber()<<endlog();
00121 }
00122
00123 StateMachine::Status::StateMachineStatus StateMachine::getStatus() const {
00124 return smStatus;
00125 }
00126
00127 string StateMachine::getStatusStr() const {
00128
00129 switch ( smStatus )
00130 {
00131 case Status::inactive:
00132 return "inactive";
00133 break;
00134 case Status::stopping:
00135 return "stopping";
00136 break;
00137 case Status::stopped:
00138 return "stopped";
00139 break;
00140 case Status::requesting:
00141 return "requesting";
00142 break;
00143 case Status::running:
00144 return "running";
00145 break;
00146 case Status::paused:
00147 return "paused";
00148 break;
00149 case Status::active:
00150 return "active";
00151 break;
00152 case Status::activating:
00153 return "activating";
00154 break;
00155 case Status::deactivating:
00156 return "deactivating";
00157 break;
00158 case Status::resetting:
00159 return "resetting";
00160 break;
00161 case Status::error:
00162 return "error";
00163 break;
00164 case Status::unloaded:
00165 return "unloaded";
00166 break;
00167 }
00168 return "na";
00169 }
00170
00171 bool StateMachine::pause()
00172 {
00173 if ( smStatus != Status::inactive && smStatus != Status::unloaded ) {
00174 TRACE_INIT();
00175 TRACE( "Will pause." );
00176 if (currentProg) {
00177 currentProg->pause();
00178 currentProg->execute();
00179 }
00180 smpStatus = pausing;
00181 return true;
00182 }
00183 TRACE( "Won't pause." );
00184 return false;
00185 }
00186
00187 bool StateMachine::step()
00188 {
00189 TRACE_INIT();
00190 if ( smStatus == Status::paused && mstep == false ) {
00191 TRACE( "Will step." );
00192 mstep = true;
00193 return true;
00194 }
00195 if ( smStatus == Status::active ) {
00196 TRACE( "Will step." );
00197 smStatus = Status::requesting;
00198 return true;
00199 }
00200 TRACE( "Won't step." );
00201 return false;
00202 }
00203
00204 bool StateMachine::start()
00205 {
00206 return this->automatic();
00207 }
00208
00209 bool StateMachine::automatic()
00210 {
00211 TRACE_INIT();
00212
00213
00214
00215 if ( smStatus != Status::inactive && smStatus != Status::unloaded && smStatus != Status::error) {
00216 TRACE( "Will start." );
00217 smStatus = Status::running;
00218 runState( current );
00219 return true;
00220 }
00221 TRACE( "Won't start." );
00222 return false;
00223 }
00224
00225 bool StateMachine::reactive()
00226 {
00227 TRACE_INIT();
00228 if ( smStatus != Status::inactive && smStatus != Status::unloaded && smStatus != Status::error ) {
00229 TRACE( "Will enter reactive mode." );
00230 smStatus = Status::active;
00231 return true;
00232 }
00233 TRACE( "Won't enter reactive mode." );
00234 return false;
00235 }
00236
00237 bool StateMachine::stop()
00238 {
00239 TRACE_INIT();
00240 if ( smStatus != Status::inactive && smStatus != Status::unloaded ) {
00241 TRACE( "Will stop." );
00242 smpStatus = gostop;
00243 return true;
00244 }
00245 TRACE( "Won't stop." );
00246 return false;
00247 }
00248
00249 bool StateMachine::reset()
00250 {
00251 TRACE_INIT();
00252
00253 if ( smStatus == Status::stopped ) {
00254 TRACE( "Will reset.");
00255 smpStatus = goreset;
00256 return true;
00257 }
00258 TRACE("Won't reset.");
00259 return false;
00260 }
00261
00262 bool StateMachine::execute()
00263 {
00264 TRACE_INIT();
00265 os::MutexLock lock(execlock);
00266
00267
00268 if (smStatus == Status::inactive || smStatus == Status::unloaded) {
00269 smpStatus = nill;
00270 return true;
00271 }
00272
00273
00274 switch (smpStatus) {
00275 case pausing:
00276 TRACE("Is paused now.");
00277 smStatus = Status::paused;
00278 smpStatus = nill;
00279 return true;
00280 break;
00281 case gostop:
00282 this->executePending();
00283 if ( this->requestFinalState() ) {
00284
00285 if ( this->inTransition() ) {
00286 smStatus = Status::stopping;
00287 } else {
00288 TRACE("Is stopped now.");
00289 smStatus = Status::stopped;
00290 }
00291 smpStatus = nill;
00292 }
00293 return true;
00294 break;
00295 case goreset:
00296 if ( this->executePending() ) {
00297 this->requestInitialState();
00298 if ( this->inTransition() ) {
00299 smStatus = Status::resetting;
00300 } else {
00301 TRACE("Is reset now.");
00302 smStatus = Status::active;
00303 }
00304 }
00305 smpStatus = nill;
00306 return true;
00307 break;
00308 case nill:
00309 break;
00310 }
00311
00312
00313 switch (smStatus) {
00314 case Status::inactive:
00315 return true;
00316 break;
00317 case Status::requesting:
00318 if ( this->executePending() ) {
00319 this->requestNextState();
00320 TRACE("Is active now.");
00321 smStatus = Status::active;
00322 }
00323 break;
00324 case Status::active:
00325 this->executePending();
00326 break;
00327 case Status::running:
00328 if ( this->executePending() == false)
00329 break;
00330
00331 this->requestNextState();
00332 break;
00333 case Status::paused:
00334 if (mstep) {
00335 if ( this->executePending(true) )
00336 this->requestNextState(true);
00337 TRACE("Did a step.");
00338 mstep = false;
00339 }
00340 break;
00341 case Status::error:
00342 case Status::unloaded:
00343 break;
00344 case Status::activating:
00345 this->executePending();
00346 if ( !this->inTransition() ) {
00347 TRACE("Is active now.");
00348 smStatus = Status::active;
00349 }
00350 break;
00351 case Status::stopping:
00352 if ( this->executePending() ) {
00353 TRACE("Is stopped now.");
00354 smStatus = Status::stopped;
00355 }break;
00356 case Status::deactivating:
00357 if ( this->executePending() ) {
00358 TRACE("Is inactive now.");
00359 smStatus = Status::inactive;
00360 }break;
00361 case Status::resetting:
00362 if ( this->executePending() ) {
00363 TRACE("Is reset now.");
00364 smStatus = Status::active;
00365 }break;
00366 case Status::stopped:
00367 this->executePending();
00368 if ( current != finistate ) {
00369 smStatus = Status::active;
00370 } break;
00371 }
00372 return true;
00373 }
00374
00375 bool StateMachine::requestInitialState()
00376 {
00377 TRACE_INIT();
00378
00379 if ( interruptible() && ( current == initstate || current == finistate ) )
00380 {
00381 TRACE("Will enter initial state.");
00382
00383 this->requestStateChange( initstate );
00384 return true;
00385 }
00386 TRACE("Won't enter initial state.");
00387 return false;
00388 }
00389
00390 bool StateMachine::requestFinalState()
00391 {
00392 TRACE_INIT();
00393
00394 if ( current == 0 || ( !inError() && !interruptible() ) ) {
00395 TRACE("Won't enter final state.");
00396 return false;
00397 }
00398
00399
00400 if ( this->requestStateChange( finistate ) ) {
00401 TRACE("Will enter final state.");
00402 return true;
00403 }
00404 TRACE("Won't enter final state.");
00405 return false;
00406 }
00407
00408 void StateMachine::changeState(StateInterface* newState, ProgramInterface* transProg, bool stepping) {
00409 TRACE_INIT();
00410 if ( newState == current )
00411 {
00412
00413 if ( transProg ) {
00414 TRACE("Transition triggered to self: '"+current->getName()+"'");
00415 transProg->reset();
00416 if (transProg->start() == false )
00417 smStatus = Status::error;
00418 currentTrans = transProg;
00419 currentProg = transProg;
00420
00421 reqstep = stateMap.find( current )->second.begin();
00422
00423
00424
00425
00426
00427 } else {
00428
00429
00430
00431 if ( currentHandle == 0 )
00432 handleState( current );
00433 }
00434 }
00435 else
00436 {
00437 TRACE("Transition triggered from '"+ (current ? current->getName() : "null") +"' to '"+(newState ? newState->getName() : "null")+"'.");
00438
00439
00440 currentRun = 0;
00441 currentHandle = 0;
00442 if ( transProg ) {
00443 transProg->reset();
00444 if ( transProg->start() == false )
00445 smStatus = Status::error;
00446
00447 }
00448 currentTrans = transProg;
00449
00450 if ( currentExit && currentExit->inError() )
00451 currentExit = 0;
00452 else
00453 leaveState( current );
00454 enterState( newState );
00455 }
00456
00457
00458
00459 if ( !stepping )
00460 this->executePending(stepping);
00461
00462
00463
00464
00465
00466 runState( newState );
00467 }
00468
00469 void StateMachine::enableGlobalEvents( )
00470 {
00471 enableEvents(0);
00472 }
00473 void StateMachine::disableGlobalEvents( )
00474 {
00475 disableEvents(0);
00476 }
00477 void StateMachine::enableEvents( StateInterface* s )
00478 {
00479
00480
00481
00482
00483
00484 EventMap::mapped_type& hlist = eventMap[s];
00485 for (EventList::iterator eit = hlist.begin();
00486 eit != hlist.end();
00487 ++eit) {
00488 assert( get<6>(*eit).connected() == false );
00489 get<6>(*eit).connect();
00490 }
00491 }
00492 void StateMachine::disableEvents( StateInterface* s )
00493 {
00494
00495
00496
00497
00498
00499 EventMap::mapped_type& hlist = eventMap[s];
00500 for (EventList::iterator eit = hlist.begin();
00501 eit != hlist.end();
00502 ++eit) {
00503 assert( get<6>(*eit).connected() == true );
00504 get<6>(*eit).disconnect();
00505 }
00506
00507 }
00508
00509 bool StateMachine::createEventTransition( ServicePtr sp,
00510 const std::string& ename, vector<DataSourceBase::shared_ptr> args,
00511 StateInterface* from, StateInterface* to,
00512 ConditionInterface* guard, boost::shared_ptr<ProgramInterface> transprog,
00513 StateInterface* elseto, boost::shared_ptr<ProgramInterface> elseprog )
00514 {
00515 Logger::In in("StateMachine::createEventTransition");
00516 DisposableInterface::shared_ptr di = sp->getLocalOperation(ename);
00517 OperationCallerInterface::shared_ptr oci = dynamic_pointer_cast<OperationCallerInterface>(di);
00518 if ( !oci ) {
00519 log(Error) << "Can not receive event '"<< ename <<"' in StateMachine : not a local operation."<< endlog();
00520 return false;
00521 }
00522
00523 if ( !( sp && guard ) ) {
00524 log(Error) << "Invalid arguments for event '"<< ename <<"'. ";
00525 if (!sp)
00526 log() <<"Service was null. ";
00527 if (!guard)
00528 log() <<"Guard Condition was null. ";
00529 log() << endlog();
00530 return false;
00531 }
00532
00533 if ( to == 0 )
00534 to = from;
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548 Handle handle;
00549
00550 log(Debug) << "Creating Signal handler for Operation '"<< ename <<"' from state "<< (from ? from->getName() : string("(global)")) << " to state " << ( to ? to->getName() : string("(global)") ) <<Logger::endl;
00551 #ifdef ORO_SIGNALLING_OPERATIONS
00552 handle = sp->produceSignal( ename, new CommandFunction( boost::bind( &StateMachine::eventTransition, this, from, guard, transprog.get(), to, elseprog.get(), elseto) ), args, this->getEngine() );
00553 #endif
00554 if ( !handle.ready() ) {
00555 Logger::log() << Logger::Error << "Could not setup handle for event '"<<ename<<"'."<<Logger::endl;
00556 return false;
00557 }
00558
00559 handle.disconnect();
00560
00561
00562 eventMap[from].push_back( boost::make_tuple( sp, ename, args, to, guard, transprog, handle, elseto, elseprog) );
00563
00564 stateMap[from];
00565 stateMap[to];
00566 return true;
00567 }
00568
00569 bool StateMachine::eventTransition(StateInterface* from, ConditionInterface* c, ProgramInterface* p, StateInterface* to, ProgramInterface* elsep, StateInterface* elseto )
00570 {
00571 TRACE_INIT();
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582 if ( !current)
00583 return true;
00584
00585 if (from == 0)
00586 from = current;
00587 if (to == 0)
00588 to = current;
00589 TRACE("Received Signal in state '"+ current->getName()+"' for transition from state '" + from->getName() + "' to state '" + to->getName() + "'");
00590 if ( from == current && !this->inTransition() ) {
00591 if ( c->evaluate() && checkConditions(to, false) == 1 ) {
00592 TRACE( "Valid transition from " + from->getName() +
00593 +" to "+to->getName()+".");
00594
00595 changeState( to, p, true );
00596
00597 this->getEngine()->getActivity()->trigger();
00598 }
00599 else {
00600 TRACE( "Rejected transition from " + from->getName() +
00601 " to " + to->getName() +
00602 " within state " + current->getName() + ": guards failed.");
00603 }
00604 }
00605 #if 1
00606 else {
00607 if (this->inTransition() ) {
00608 TRACE( "Rejected transition from " + from->getName() +
00609 " within " + current->getName() + ": already in transition.");
00610 } else {
00611 TRACE( "Rejected transition from " + from->getName() +
00612 + " within " + current->getName() + ": wrong state.");
00613 }
00614 }
00615 #endif
00616 return true;
00617 }
00618
00619 StateInterface* StateMachine::requestNextState(bool stepping)
00620 {
00621 TRACE_INIT();
00622
00623 if( current == 0 )
00624 return 0;
00625
00626 if ( !interruptible() || currentTrans ) {
00627 return current;
00628 }
00629
00630
00631 TransList::const_iterator it, it1, it2;
00632 it1 = stateMap.find( 0 )->second.begin();
00633 it2 = stateMap.find( 0 )->second.end();
00634
00635 if ( reqstep == stateMap.find( current )->second.begin() )
00636 for ( it= it1; it != it2; ++it)
00637 get<0>(*it)->reset();
00638
00639 if ( reqstep == reqend ) {
00640
00641 for ( ; it1 != it2; ++it1 )
00642 if ( get<0>(*it1)->evaluate()
00643 && checkConditions( get<1>(*it1) ) == 1 ) {
00644 StateInterface* next = get<1>(*it1);
00645 if ( next == 0 )
00646 changeState( current, get<4>(*it1).get(), stepping );
00647 else
00648 changeState( next, get<4>(*it1).get(), stepping );
00649
00650 return current;
00651 }
00652
00653 changeState( current, 0, stepping );
00654 return current;
00655 }
00656
00657
00658 do {
00659 if ( get<0>(*reqstep)->evaluate() ) {
00660
00661 if (reqstep == reqend )
00662 return current;
00663
00664 int cres = checkConditions( get<1>(*reqstep), stepping );
00665 if (cres == 0) {
00666 break;
00667 }
00668 if( cres == 1) {
00669 changeState( get<1>(*reqstep), get<4>(*reqstep).get(), stepping );
00670 break;
00671 }
00672
00673 }
00674 if ( reqstep + 1 == reqend ) {
00675
00676 for ( ; it1 != it2; ++it1 ) {
00677 if ( get<0>(*it1)->evaluate() && checkConditions( get<1>(*it1) ) == 1 ) {
00678 StateInterface* next = get<1>(*it1);
00679 if ( next == 0)
00680 changeState( current, get<4>(*it1).get(), stepping );
00681 else
00682 changeState( next, get<4>(*it1).get(), stepping );
00683
00684 return current;
00685 }
00686 }
00687
00688 reqstep = stateMap.find( current )->second.begin();
00689 evaluating = get<3>(*reqstep);
00690 changeState( current, 0, stepping );
00691 break;
00692 }
00693 else {
00694 ++reqstep;
00695 evaluating = get<3>(*reqstep);
00696 }
00697 } while ( !stepping );
00698
00699 return current;
00700 }
00701
00702 int StateMachine::checkConditions( StateInterface* state, bool stepping ) {
00703
00704
00705 if ( !checking_precond || !stepping ) {
00706 prec_it = precondMap.equal_range(state);
00707 }
00708
00709
00710
00711
00712 while ( prec_it.first != prec_it.second ) {
00713 if (checking_precond == false && stepping ) {
00714 evaluating = prec_it.first->second.second;
00715 checking_precond = true;
00716 return 0;
00717 }
00718 if ( prec_it.first->second.first->evaluate() == false ) {
00719 checking_precond = false;
00720 return -1;
00721 }
00722 ++( prec_it.first );
00723 if (stepping) {
00724 if ( prec_it.first != prec_it.second )
00725 evaluating = prec_it.first->second.second;
00726 checking_precond = true;
00727 return 0;
00728 }
00729 }
00730 checking_precond = false;
00731 return 1;
00732 }
00733
00734
00735 StateInterface* StateMachine::nextState()
00736 {
00737
00738 if ( current == 0 )
00739 return 0;
00740 TransList::const_iterator it1, it2;
00741 it1 = stateMap.find( current )->second.begin();
00742 it2 = stateMap.find( current )->second.end();
00743
00744 for ( ; it1 != it2; ++it1 )
00745 if ( get<0>(*it1)->evaluate() && checkConditions( get<1>(*it1)) == 1 ) {
00746 return get<1>(*it1);
00747 }
00748
00749
00750 it1 = stateMap.find( 0 )->second.begin();
00751 it2 = stateMap.find( 0 )->second.end();
00752
00753 for ( ; it1 != it2; ++it1 )
00754 if ( get<0>(*it1)->evaluate() && checkConditions( get<1>(*it1)) == 1 ) {
00755 return get<1>(*it1);
00756 }
00757
00758 return current;
00759 }
00760
00761 std::vector<std::string> StateMachine::getStateList() const {
00762 vector<string> result;
00763 vector<StateInterface*> sl;
00764 StateInterface* dummy = 0;
00765 transform( stateMap.begin(), stateMap.end(), back_inserter(sl), select1st<TransitionMap::value_type>() );
00766 sl.erase( find(sl.begin(), sl.end(), dummy) );
00767 transform( sl.begin(), sl.end(), back_inserter(result), boost::bind( &StateInterface::getName, _1 ) );
00768 return result;
00769 }
00770
00771 void StateMachine::addState( StateInterface* s )
00772 {
00773 stateMap[s];
00774 }
00775
00776
00777 StateInterface* StateMachine::getState(const string& name) const
00778 {
00779 TransitionMap::const_iterator it = stateMap.begin();
00780 while ( it != stateMap.end() ) {
00781 if ( it->first && it->first->getName() == name )
00782 return it->first;
00783 ++it;
00784 }
00785 return 0;
00786 }
00787
00788 bool StateMachine::requestStateChange( StateInterface * s_n )
00789 {
00790
00791 if( current == 0 )
00792 return false;
00793
00794 if ( !interruptible() ) {
00795 return false;
00796 }
00797
00798
00799
00800
00801 if ( current == s_n )
00802 {
00803 changeState( s_n, 0 );
00804 return true;
00805 }
00806
00807
00808 TransList::iterator it, it1, it2;
00809 it1 = stateMap.find( current )->second.begin();
00810 it2 = stateMap.find( current )->second.end();
00811
00812 for ( ; it1 != it2; ++it1 )
00813 if ( get<1>(*it1) == s_n
00814 && get<0>(*it1)->evaluate()
00815 && checkConditions( s_n ) == 1 ) {
00816 changeState( s_n, get<4>(*it1).get() );
00817
00818 return true;
00819 }
00820
00821
00822 it1 = stateMap.find( 0 )->second.begin();
00823 it2 = stateMap.find( 0 )->second.end();
00824
00825
00826 for ( it= it1; it != it2; ++it)
00827 get<0>(*it)->reset();
00828
00829
00830 for ( ; it1 != it2; ++it1 )
00831 if ( get<1>(*it1) == s_n
00832 && get<0>(*it1)->evaluate()
00833 && checkConditions( s_n ) == 1 ) {
00834 changeState( s_n, get<4>(*it1).get() );
00835
00836 return true;
00837 }
00838
00839
00840 if ( finistate == s_n )
00841 {
00842 changeState( s_n, 0 );
00843 return true;
00844 }
00845
00846
00847 if ( initstate == s_n && current == finistate)
00848 {
00849 changeState( s_n, 0 );
00850 return true;
00851 }
00852
00853
00854 return false;
00855 }
00856
00857 int StateMachine::getLineNumber() const {
00858
00859
00860
00861 StateInterface* statecopy = current;
00862 if ( statecopy == 0 )
00863 return 1;
00864 ProgramInterface* copy = currentProg;
00865 if ( copy )
00866 return copy->getLineNumber();
00867 if ( evaluating )
00868 return evaluating;
00869
00870
00871 return statecopy->getEntryPoint();
00872 }
00873
00874 string StateMachine::getText() const {
00875 return string();
00876 }
00877
00878 void StateMachine::preconditionSet(StateInterface* state, ConditionInterface* cnd, int line )
00879 {
00880
00881 if ( current != 0)
00882 return;
00883 precondMap.insert( make_pair(state, make_pair( cnd, line)) );
00884 stateMap[state];
00885 }
00886
00887 void StateMachine::transitionSet( StateInterface* from, StateInterface* to, ConditionInterface* cnd, int priority, int line )
00888 {
00889 this->transitionSet( from, to, cnd, boost::shared_ptr<ProgramInterface>(), priority, line);
00890 }
00891
00892 void StateMachine::transitionSet( StateInterface* from, StateInterface* to,
00893 ConditionInterface* cnd, boost::shared_ptr<ProgramInterface> transprog,
00894 int priority, int line )
00895 {
00896 TRACE_INIT();
00897
00898 if ( current != 0)
00899 return;
00900
00901 if (from) {
00902 TRACE("Created transition from "+from->getName() +"' to '"+ to->getName()+"'");
00903 } else {
00904 TRACE("Created global transition to '"+ to->getName()+"'");
00905 }
00906
00907 TransList::iterator it;
00908 for ( it= stateMap[from].begin(); it != stateMap[from].end() && get<2>(*it) >= priority; ++it)
00909 ;
00910 stateMap[from].insert(it, boost::make_tuple( cnd, to, priority, line, transprog ) );
00911 stateMap[to];
00912 }
00913
00914 StateInterface* StateMachine::currentState() const
00915 {
00916 return current;
00917 }
00918
00919 ProgramInterface* StateMachine::currentProgram() const
00920 {
00921 return currentProg;
00922 }
00923
00924 void StateMachine::leaveState( StateInterface* s )
00925 {
00926 assert(s);
00927
00928
00929 currentExit = s->getExitProgram();
00930 if ( currentExit ) {
00931 currentExit->reset();
00932 if (currentExit->start() == false)
00933 smStatus = Status::error;
00934
00935 if (currentProg == 0 )
00936 currentProg = currentExit;
00937 }
00938 }
00939
00940 void StateMachine::runState( StateInterface* s )
00941 {
00942 TRACE_INIT();
00943 assert(s);
00944 currentRun = s->getRunProgram();
00945 if ( currentRun ) {
00946 currentRun->reset();
00947 if (currentRun->start() == false)
00948 smStatus = Status::error;
00949 if (currentProg == 0 )
00950 currentProg = currentRun;
00951 }
00952 }
00953
00954 void StateMachine::handleState( StateInterface* s )
00955 {
00956 assert(s);
00957 currentHandle = s->getHandleProgram();
00958 if ( currentHandle ) {
00959 currentHandle->reset();
00960 if (currentHandle->start() == false)
00961 smStatus = Status::error;
00962 if (currentProg == 0 )
00963 currentProg = currentHandle;
00964 }
00965 }
00966
00967 void StateMachine::enterState( StateInterface* s )
00968 {
00969 assert(s);
00970
00971
00972
00973
00974 TransList::iterator it;
00975 for ( it= stateMap.find(s)->second.begin(); it != stateMap.find(s)->second.end(); ++it)
00976 get<0>(*it)->reset();
00977
00978 next = s;
00979 currentEntry = s->getEntryProgram();
00980 if ( currentEntry ) {
00981 currentEntry->reset();
00982 if (currentEntry->start() == false)
00983 smStatus = Status::error;
00984 if (currentProg == 0 )
00985 currentProg = currentEntry;
00986 }
00987 }
00988
00989 bool StateMachine::executePending( bool stepping )
00990 {
00991 TRACE_INIT();
00992
00993
00994
00995
00996
00997
00998
00999
01000 if ( inError() )
01001 return false;
01002
01003
01004 if ( currentTrans ) {
01005 TRACE("Executing transition program from '"+ (current ? current->getName() : "(null)") + "' to '"+ ( next ? next->getName() : "(null)")+"'" );
01006
01007 if ( currentHandle ) {
01008 if ( this->executeProgram(currentHandle, stepping) == false )
01009 return false;
01010 } else
01011 if ( this->executeProgram(currentTrans, stepping) == false )
01012 return false;
01013
01014 TRACE("Finished transition program from '"+ current->getName() + "' to '"+next->getName()+"'" );
01015
01016 if ( stepping ) {
01017 currentProg = currentExit ? currentExit : (currentEntry ? currentEntry : currentRun);
01018 return false;
01019 }
01020 }
01021
01022
01023 if ( currentExit ) {
01024 TRACE("Executing exit program from '"+ current->getName() + "' (going to '"+ (next ? next->getName() : "(null)") +"')" );
01025 if ( this->executeProgram(currentExit, stepping) == false )
01026 return false;
01027
01028 TRACE("Finished exit program from '"+ current->getName() + "' (going to '"+ (next ? next->getName() : "(null)") +"')" );
01029
01030 if ( stepping ) {
01031 currentProg = (currentEntry ? currentEntry : currentRun);
01032 return false;
01033 }
01034 }
01035
01036
01037
01038
01039 if ( current != next ) {
01040 if ( next ) {
01041 reqstep = stateMap.find( next )->second.begin();
01042 reqend = stateMap.find( next )->second.end();
01043
01044 if ( reqstep == reqend )
01045 evaluating = 0;
01046 else
01047 evaluating = get<3>(*reqstep);
01048 } else {
01049 current = 0;
01050 return true;
01051 }
01052
01053 TRACE("Formally transitioning from '"+ (current ? current->getName() : "(null)") + "' to '"+ (next ? next->getName() : "(null)") +"'" );
01054
01055 disableEvents(current);
01056 current = next;
01057 enableEvents(current);
01058
01059 }
01060
01061 if ( currentEntry ) {
01062 TRACE("Executing entry program of '"+ current->getName() +"'" );
01063 if ( this->executeProgram(currentEntry, stepping) == false )
01064 return false;
01065
01066 TRACE("Finished entry program of '"+ current->getName() +"'" );
01067
01068 if ( stepping ) {
01069 currentProg = currentRun;
01070 return false;
01071 }
01072 }
01073
01074
01075
01076 if ( currentHandle ) {
01077 TRACE("Executing handle program of '"+ current->getName() +"'" );
01078 if ( this->executeProgram(currentHandle, stepping) == false )
01079 return false;
01080
01081 TRACE("Finished handle program of '"+ current->getName() +"'" );
01082
01083 if ( stepping ) {
01084 currentProg = currentRun;
01085 return false;
01086 }
01087 }
01088
01089
01090 if ( currentRun ) {
01091 TRACE("Executing run program of '"+ current->getName() +"'" );
01092 if ( this->executeProgram(currentRun, stepping) == false )
01093 return false;
01094
01095 TRACE("Finished run program of '"+ current->getName() +"'" );
01096
01097 if ( stepping )
01098 return false;
01099
01100 }
01101
01102 return true;
01103 }
01104
01105 bool StateMachine::executeProgram(ProgramInterface*& cp, bool stepping)
01106 {
01107 TRACE_INIT();
01108 if ( cp == 0)
01109 return false;
01110
01111 currentProg = cp;
01112 if ( stepping )
01113 currentProg->step();
01114 else
01115 currentProg->execute();
01116 if ( currentProg->inError() ) {
01117 smStatus = Status::error;
01118 smpStatus = nill;
01119 TRACE("Encountered run-time error at line " << this->getLineNumber() );
01120 return false;
01121 }
01122
01123 if ( currentProg && !currentProg->isStopped() )
01124 return false;
01125
01126 cp = currentProg = 0;
01127 return true;
01128 }
01129
01130
01131
01132 bool StateMachine::inTransition() const {
01133 return ( (currentProg != 0) && (currentProg != currentRun) && (currentProg != currentHandle) ) || (current != next);
01134 }
01135
01136
01137 bool StateMachine::interruptible() const {
01138 return currentProg == 0 || currentProg == currentRun;
01139 }
01140
01141 void StateMachine::setInitialState( StateInterface* s )
01142 {
01143 initstate = s;
01144 stateMap[initstate];
01145 }
01146
01147 void StateMachine::setFinalState( StateInterface* s )
01148 {
01149 finistate = s;
01150 stateMap[finistate];
01151 }
01152
01153 void StateMachine::trace(bool t) {
01154 mtrace =t;
01155 }
01156
01157 bool StateMachine::activate()
01158 {
01159 TRACE_INIT();
01160
01161 if ( smStatus != Status::inactive ) {
01162 TRACE("Won't activate: already active.");
01163 return false;
01164 }
01165
01166 smpStatus = nill;
01167
01168 if ( this->checkConditions( getInitialState() ) != 1 ) {
01169 TRACE("Won't activate: preconditions failed.");
01170 return false;
01171 }
01172
01173 if ( initc ) {
01174 initc->reset();
01175 initc->readArguments();
01176 if ( initc->execute() == false ) {
01177 TRACE("Won't activate: Init Commands failed.");
01178 return false;
01179 }
01180 }
01181
01182
01183 enterState( getInitialState() );
01184 reqstep = stateMap.find( next )->second.begin();
01185 reqend = stateMap.find( next )->second.end();
01186
01187
01188 enableGlobalEvents();
01189
01190
01191 if ( !inError() ) {
01192 if ( this->executePending() ) {
01193 smStatus = Status::active;
01194 TRACE("Activated.");
01195 } else {
01196 if ( !inError() ) {
01197 TRACE("Still activating.");
01198 smStatus = Status::activating;
01199 }
01200 }
01201 }
01202
01203 return true;
01204 }
01205
01206
01207 bool StateMachine::deactivate()
01208 {
01209 TRACE_INIT();
01210
01211 if ( smStatus == Status::inactive) {
01212 TRACE("Won't deactivate: already inactive.");
01213 return false;
01214 }
01215 os::MutexLock lock(execlock);
01216
01217
01218 disableGlobalEvents();
01219
01220
01221
01222 if ( currentExit && currentExit->inError() )
01223 currentExit = 0;
01224 if ( currentTrans && currentTrans->inError() )
01225 currentTrans = 0;
01226
01227
01228 if ( next != 0 && current )
01229 leaveState( current );
01230 else {
01231 currentExit = 0;
01232 currentTrans = 0;
01233 }
01234
01235
01236 currentProg = 0;
01237 currentEntry = 0;
01238 currentHandle = 0;
01239 currentRun = 0;
01240 next = 0;
01241
01242
01243
01244 if ( this->executePending() ) {
01245 TRACE("Deactivated.");
01246 smStatus = Status::inactive;
01247 } else {
01248 TRACE("Still deactivating.");
01249 smStatus = Status::deactivating;
01250 }
01251
01252 return true;
01253 }
01254 }