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