StateMachine.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002   tag: Peter Soetens  Mon May 10 19:10:29 CEST 2004  StateMachine.cxx
00003 
00004                         StateMachine.cxx -  description
00005                            -------------------
00006     begin                : Mon May 10 2004
00007     copyright            : (C) 2004 Peter Soetens
00008     email                : peter.soetens@mech.kuleuven.ac.be
00009 
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU General Public                   *
00013  *   License as published by the Free Software Foundation;                 *
00014  *   version 2 of the License.                                             *
00015  *                                                                         *
00016  *   As a special exception, you may use this file as part of a free       *
00017  *   software library without restriction.  Specifically, if other files   *
00018  *   instantiate templates or use macros or inline functions from this     *
00019  *   file, or you compile this file and link it with other files to        *
00020  *   produce an executable, this file does not by itself cause the         *
00021  *   resulting executable to be covered by the GNU General Public          *
00022  *   License.  This exception does not however invalidate any other        *
00023  *   reasons why the executable file might be covered by the GNU General   *
00024  *   Public License.                                                       *
00025  *                                                                         *
00026  *   This library is distributed in the hope that it will be useful,       *
00027  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00028  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00029  *   Lesser General Public License for more details.                       *
00030  *                                                                         *
00031  *   You should have received a copy of the GNU General Public             *
00032  *   License along with this library; if not, write to the Free Software   *
00033  *   Foundation, Inc., 59 Temple Place,                                    *
00034  *   Suite 330, Boston, MA  02111-1307  USA                                *
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); // allows global state transitions
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            // inform all entry/exit state scripts:
00087            if (it->first)
00088                it->first->loaded( this->getEngine() );
00089            // inform all transition scripts
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            // inform all transition scripts
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(); // try one last time
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(); // critical failure !
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         // if you go from reactive to automatic,
00213         // first execute the run program, before
00214         // evaluating transitions.
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         // if waiting in final state, go ahead.
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         // before dealing with transitional states,
00267         // check if we're actually running.
00268         if (smStatus == Status::inactive || smStatus == Status::unloaded) {
00269             smpStatus = nill;
00270             return true;
00271         }
00272 
00273         // internal transitional issues.
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                 // test for atomicity :
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         // public visible states.
00313         switch (smStatus) {
00314         case Status::inactive:
00315             return true;
00316             break;
00317         case Status::requesting:
00318             if ( this->executePending() ) {   // if all steps done,
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             // if all pending done:
00331             this->requestNextState(); // one state at a time
00332             break;
00333         case Status::paused:
00334             if (mstep) {
00335                 if ( this->executePending(true) )    // if all steps done,
00336                     this->requestNextState(true); // one state at a time
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 ) {// detect leaving final state by event/request
00369                 smStatus = Status::active;
00370             } break;
00371         }
00372         return true;
00373     }
00374 
00375     bool StateMachine::requestInitialState()
00376     {
00377         TRACE_INIT();
00378         // all conditions that must be satisfied to enter the initial state :
00379         if ( interruptible() && ( current == initstate || current == finistate ) )
00380         {
00381             TRACE("Will enter initial state.");
00382             // this will try to execute the state change atomically
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         // if we are inactive or in transition, don't do anything.
00394         if ( current == 0 || ( !inError() && !interruptible() ) ) {
00395             TRACE("Won't enter final state.");
00396             return false;
00397         }
00398 
00399         // this will try to execute the state change atomically
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                 // this is only true if current state was selected in a transition of current.
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                     // manually reset reqstep, or the next iteration would skip transition checks.
00421                     reqstep = stateMap.find( current )->second.begin();
00422                     // from now on, we are in transition to self !
00423                     // currentRun is _not_ set to zero or reset.
00424                     // it is/may be interrupted by trans, then continued.
00425                     // if already in handle, it is completed before trans, then trans is executed.
00426                     // since a transProg was given, no handle is executed.
00427                 } else {
00428                     // execute the default action (schedule handle )
00429                     // if no transition to another state took place and no transprog specified.
00430                     // only schedule a handle if not yet in progress.
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                 // reset handle and run, in case it is still set ( during error
00439                 // or when an event arrived ).
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                 // if error in current Exit, skip it.
00450                 if ( currentExit && currentExit->inError() )
00451                     currentExit = 0;
00452                 else
00453                     leaveState( current );
00454                 enterState( newState );
00455             }
00456 
00457         // if not stepping, try to execute exit/entry directly.
00458         // if stepping, postpone this
00459         if ( !stepping )
00460             this->executePending(stepping);
00461 
00462         // schedule a run for the next 'step'.
00463         // if handle above finished, run will be called directly
00464         // in executePending. if handle was not finished
00465         // or stepping, it will be called after handle.
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 //        TRACE_INIT();
00480 //        if (s) {
00481 //            TRACE("Enabling events for state '"+s->getName()+"'.");
00482 //        } else
00483 //            TRACE("Enabling global events.");
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 //        TRACE_INIT();
00495 //        if (s) {
00496 //            TRACE("Disabling events for state '"+s->getName()+"'.");
00497 //        } else
00498 //            TRACE("Disabling global events.");
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         // get ename from event service, provide args as arguments
00537         // event should be activated upon entry of 'from'
00538         // guard is evaluated to get final 'ok'.
00539         // event does a requestStateChange( to );
00540         // if 'ok', execute transprog during transition.
00541 
00542         // Store guard and transprog for copy/clone semantics.
00543         // upon SM copy, recreate the handles for the copy SM with a copy of guard/transprog.
00544         // Same for args. I guess we need to store all arguments of this function to allow
00545         // proper copy semantics, such that the event handle can be created for each new SM
00546         // instance. Ownership of guard and transprog is to be determined, but seems to ly
00547         // with the SM. handle.destroy() can be called upon SM destruction.
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; // event does not exist...
00557         }
00558         // all our handles must start in disconnected state:
00559         handle.disconnect();
00560         // BIG NOTE : we MUST store handle otherwise, the connection is destroyed (cfr setup vs connect).
00561         // Off course, we also need it to connect/disconnect the event.
00562         eventMap[from].push_back( boost::make_tuple( sp, ename, args, to, guard, transprog, handle, elseto, elseprog) );
00563         // add the states to the statemap.
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         // called by event to begin Transition to 'to'.
00573         // This interrupts the current run program at an interruption point ?
00574         // the transition and/or exit program can cleanup...
00575 
00576         // this will never be called if the event connection is destroyed, unless called from the
00577         // CompletionProcessor (asyn event arrival). Therefore we must add extra checks :
00578         // only transition if this event was meant for this state and we are not
00579         // in transition already.
00580         // If condition fails, check precondition 'else' state (if present) and
00581         // execute else program (may be null).
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                 // stepping == true ! We don't want to execute the whole state transition
00595                 changeState( to, p, true );              //  valid transition to 'to'.
00596                 // trigger EE in order to force execution of the remainder of the state transition:
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         // bad idea, user, don't run this if we're not active...
00623         if( current == 0 )
00624             return 0;
00625         // only a run program may be interrupted...
00626         if ( !interruptible() || currentTrans ) {
00627             return current; // can not accept request, still in transition.
00628         }
00629 
00630         // Reset global conditions.
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() ) // avoid reseting too much in stepping mode.
00636             for ( it= it1; it != it2; ++it)
00637                 get<0>(*it)->reset();
00638 
00639         if ( reqstep == reqend ) { // if nothing to evaluate, eval globals, then just handle()
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 ) // handle current if no next
00646                         changeState( current, get<4>(*it1).get(), stepping );
00647                     else
00648                         changeState( next, get<4>(*it1).get(), stepping );
00649                     // the request was accepted
00650                     return current;
00651                 }
00652             // no transition found: handle()
00653             changeState( current, 0, stepping );
00654             return current;
00655         }
00656 
00657         // if we got here, at least one evaluation to check
00658         do {
00659             if ( get<0>(*reqstep)->evaluate() ) {
00660                 // evaluate() might call stop() or other sm functions:
00661                 if (reqstep == reqend )
00662                     return current;
00663                 // check preconds of target state :
00664                 int cres = checkConditions( get<1>(*reqstep), stepping );
00665                 if (cres == 0) {
00666                     break; // only returned in stepping
00667                 }
00668                 if( cres == 1) {
00669                     changeState( get<1>(*reqstep), get<4>(*reqstep).get(), stepping );
00670                     break; // valid transition
00671                 }
00672                 // if cres == -1 : precondition failed, increment reqstep...
00673             }
00674              if ( reqstep + 1 == reqend ) {
00675                 // to a state specified by the user (global)
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) // handle current if no next
00680                                  changeState( current, get<4>(*it1).get(), stepping );
00681                              else
00682                                  changeState( next, get<4>(*it1).get(), stepping );
00683                              // the request was accepted
00684                              return current;
00685                          }
00686                     }
00687                 // no transition was found, reset and 'schedule' a handle :
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         // if the preconditions of \a state are checked the first time in stepping mode, reset the iterators.
00705         if ( !checking_precond || !stepping ) {
00706             prec_it = precondMap.equal_range(state); // state is the _target_ state
00707         }
00708 
00709         // will be set to true if stepping below.
00710         //checking_precond = false;
00711 
00712         while ( prec_it.first != prec_it.second ) {
00713             if (checking_precond == false && stepping ) {
00714                 evaluating = prec_it.first->second.second; // indicate we will evaluate this line (if any).
00715                 checking_precond = true;
00716                 return 0;
00717             }
00718             if ( prec_it.first->second.first->evaluate() == false ) {
00719                 checking_precond = false;
00720                 return -1; // precondition failed
00721             }
00722             ++( prec_it.first );
00723             if (stepping) {
00724                 if ( prec_it.first != prec_it.second )
00725                     evaluating = prec_it.first->second.second; // indicate we will evaluate the next line (if any).
00726                 checking_precond = true;
00727                 return 0; // not done yet.
00728             }
00729         }
00730         checking_precond = false;
00731         return 1; // success !
00732     }
00733 
00734 
00735     StateInterface* StateMachine::nextState()
00736     {
00737         // bad idea, user, don't run this if we're not active...
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         // also check the global transitions.
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         // bad idea, user, don't run this if we're not active...
00791         if( current == 0 )
00792             return false;
00793 
00794         if ( !interruptible() ) {
00795             return false; // can not accept request, still in transition
00796         }
00797 
00798         // we may make transition to next state :
00799 
00800         // to current state
00801         if ( current == s_n )
00802         {
00803             changeState( s_n, 0 );
00804             return true;
00805         }
00806 
00807         // between 2 states specified by the user.
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                 // the request was accepted
00818                 return true;
00819             }
00820 
00821         // to a state specified by the user (global)
00822         it1 = stateMap.find( 0 )->second.begin();
00823         it2 = stateMap.find( 0 )->second.end();
00824 
00825         // reset all conditions
00826         for ( it= it1; it != it2; ++it)
00827             get<0>(*it)->reset();
00828 
00829         // evaluate them
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                 // the request was accepted
00836                 return true;
00837             }
00838 
00839         // to final state
00840         if ( finistate == s_n )
00841         {
00842             changeState( s_n, 0 );
00843             return true;
00844         }
00845 
00846         // to inital state from final state
00847         if ( initstate == s_n && current == finistate)
00848         {
00849             changeState( s_n, 0 );
00850             return true;
00851         }
00852 
00853         // the request has failed.
00854         return false;
00855     }
00856 
00857     int StateMachine::getLineNumber() const {
00858         // if not activated, return first line.
00859         // work around race condition that current[Prog] may become zero,
00860         // thus first store in local variable :
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         // if none of the above, return entry point :
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         // we must be inactive.
00881         if ( current != 0)
00882             return;
00883         precondMap.insert( make_pair(state, make_pair( cnd, line)) );
00884         stateMap[state]; // add to state map.
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         // we must be inactive.
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         // insert both from and to in the statemap
00907         TransList::iterator it;
00908         for ( it= stateMap[from].begin(); it != stateMap[from].end() && get<2>(*it) >= priority; ++it)
00909             ; // this ';' is intentional
00910         stateMap[from].insert(it, boost::make_tuple( cnd, to, priority, line, transprog ) );
00911         stateMap[to]; // insert empty vector for 'to' state.
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 //        TRACE_INIT();
00928 //        TRACE( "Planning to leave state " + s->getName() );
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 //        TRACE_INIT();
00971 //        TRACE( "Planning to enter state " + s->getName() );
00972 
00973         // Before a state is entered, all transitions are reset !
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         // This function has great resposibility, since it acts like
00993         // a scheduler for pending requests. It tries to devise what to
00994         // do on basis of the contents of variables (like current*, next,...).
00995         // This is a somewhat
00996         // fragile implementation but requires very little bookkeeping.
00997         // if returns true : a transition (exit/entry) is done
00998         // and a new state may be requested.
00999 
01000         if ( inError() )
01001             return false;
01002 
01003         // first try to execute transition program on behalf of current state.
01004         if ( currentTrans ) {
01005             TRACE("Executing transition program from '"+ (current ? current->getName() : "(null)") + "' to '"+ ( next ? next->getName() : "(null)")+"'" );
01006             // exception : transition during handle, first finish handle !
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             // done.
01014             TRACE("Finished  transition program from '"+ current->getName() + "' to '"+next->getName()+"'" );
01015             // in stepping mode, delay 'true' one executePending().
01016             if ( stepping ) {
01017                 currentProg = currentExit ? currentExit : (currentEntry ? currentEntry : currentRun);
01018                 return false;
01019             }
01020         }
01021 
01022         // last is exit
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             // done.
01028             TRACE("Finished  exit program from '"+ current->getName() + "' (going to '"+ (next ? next->getName() : "(null)") +"')" );
01029             // in stepping mode, delay 'true' one executePending().
01030             if ( stepping ) {
01031                 currentProg = (currentEntry ? currentEntry : currentRun);
01032                 return false;
01033             }
01034         }
01035 
01036 
01037         // only reset the reqstep if we changed state.
01038         // if we did not change state, it will be reset in requestNextState().
01039         if ( current != next ) {
01040             if ( next ) {
01041                 reqstep = stateMap.find( next )->second.begin();
01042                 reqend  = stateMap.find( next )->second.end();
01043                 // init for getLineNumber() :
01044                 if ( reqstep == reqend )
01045                     evaluating = 0;
01046                 else
01047                     evaluating = get<3>(*reqstep);
01048             } else {
01049                 current = 0;
01050                 return true;  // done if current == 0 !
01051             }
01052             // make change transition after exit of previous state:
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             // done.
01066             TRACE("Finished  entry program of '"+ current->getName() +"'" );
01067             // in stepping mode, delay 'true' one executePending().
01068             if ( stepping ) {
01069                 currentProg = currentRun;
01070                 return false;
01071             }
01072         }
01073         // from this point on, events must be enabled.
01074 
01075         // Handle is executed after the transitions failed.
01076         if ( currentHandle ) {
01077             TRACE("Executing handle program of '"+ current->getName() +"'" );
01078             if ( this->executeProgram(currentHandle, stepping) == false )
01079                 return false;
01080             // done.
01081             TRACE("Finished  handle program of '"+ current->getName() +"'" );
01082             // in stepping mode, delay 'true' one executePending().
01083             if ( stepping ) {
01084                 currentProg = currentRun;
01085                 return false;
01086             }
01087         }
01088 
01089         // Run is executed before the transitions.
01090         if ( currentRun ) {
01091             TRACE("Executing run program of '"+ current->getName() +"'" );
01092             if ( this->executeProgram(currentRun, stepping) == false )
01093                 return false;
01094             // done.
01095             TRACE("Finished  run program of '"+ current->getName() +"'" );
01096             // in stepping mode, delay 'true' one executePending().
01097             if ( stepping )
01098                 return false;
01099 
01100         }
01101 
01102         return true; // all pending is done
01103     }
01104 
01105     bool StateMachine::executeProgram(ProgramInterface*& cp, bool stepping)
01106     {
01107         TRACE_INIT();
01108         if ( cp == 0)
01109             return false;
01110         // execute this stateprogram and cleanup if needed.
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     // in exit/entry or transition programs
01132     bool StateMachine::inTransition() const {
01133         return ( (currentProg != 0)  && (currentProg != currentRun) && (currentProg != currentHandle) ) || (current != next);
01134     }
01135 
01136     // only run program may be interrupted.
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         // inactive implies loaded, but check additionally if smp is at least active
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; //preconditions not met.
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; // fail to activate.
01179             }
01180         }
01181 
01182         //current = getInitialState();
01183         enterState( getInitialState() );
01184         reqstep = stateMap.find( next )->second.begin();
01185         reqend = stateMap.find( next )->second.end();
01186 
01187         // Enable all event handlers
01188         enableGlobalEvents();
01189 
01190         // execute the entry program of the initial state.
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         // the only time to refuse executing this is when we did set ourselves to inactive before.
01211         if ( smStatus == Status::inactive) {
01212             TRACE("Won't deactivate: already inactive.");
01213             return false;
01214         }
01215         os::MutexLock lock(execlock);
01216 
01217         // disable all event handlers
01218         disableGlobalEvents();
01219 
01220         // whatever state we are in, leave it.
01221         // but if current exit is in error, skip it alltogether.
01222         if ( currentExit && currentExit->inError() )
01223             currentExit = 0;
01224         if ( currentTrans && currentTrans->inError() )
01225             currentTrans = 0;
01226         // if we stalled, in previous deactivate
01227         // even skip/stop exit program.
01228         if ( next != 0 && current )
01229             leaveState( current );
01230         else {
01231             currentExit = 0;
01232             currentTrans = 0;
01233         }
01234 
01235         // do not call enterState( 0 )
01236         currentProg = 0;
01237         currentEntry = 0;
01238         currentHandle  = 0;
01239         currentRun  = 0;
01240         next = 0;
01241 
01242         // this will execute the exitFunction (if any) and, if successfull,
01243         // set current to zero (using next).
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 }


rtt
Author(s): RTT Developers
autogenerated on Wed Aug 26 2015 16:16:19