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             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         // if waiting in final state, go ahead.
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         // before dealing with transitional states,
00268         // check if we're actually running.
00269         if (smStatus == Status::inactive || smStatus == Status::unloaded) {
00270             smpStatus = nill;
00271             return true;
00272         }
00273 
00274         // internal transitional issues.
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                 // test for atomicity :
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         // public visible states.
00314         switch (smStatus) {
00315         case Status::inactive:
00316             return true;
00317             break;
00318         case Status::requesting:
00319             if ( this->executePending() ) {   // if all steps done,
00320                 this->requestNextState();
00321                 this->executePending();       // execute steps of next state
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             // if all pending done:
00333             this->requestNextState();     // one state at a time
00334             this->executePending();       // execute steps of next state
00335             break;
00336         case Status::paused:
00337             if (mstep) {
00338                 if ( this->executePending(true) ) {    // if all steps done,
00339                     this->requestNextState(true); // one state at a time
00340                     this->executePending(true);       // execute steps of next state
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 ) {// detect leaving final state by event/request
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         // all conditions that must be satisfied to enter the initial state :
00385         if ( interruptible() && ( current == initstate || current == finistate ) )
00386         {
00387             TRACE("Will enter initial state.");
00388             // this will try to execute the state change atomically
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         // if we are inactive or in transition, don't do anything.
00401         if ( current == 0 || ( !inError() && !interruptible() ) ) {
00402             TRACE("Won't enter final state.");
00403             return false;
00404         }
00405 
00406         // this will try to execute the state change atomically
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                 // this is only true if current state was selected in a transition of current.
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                     // manually reset reqstep, or the next iteration would skip transition checks.
00427                     reqstep = stateMap.find( current )->second.begin();
00428                     // from now on, we are in transition to self !
00429                     // currentRun is _not_ set to zero or reset.
00430                     // it is/may be interrupted by trans, then continued.
00431                     // if already in handle, it is completed before trans, then trans is executed.
00432                     // since a transProg was given, no handle is executed.
00433                 } else {
00434                     // execute the default action (schedule handle )
00435                     // if no transition to another state took place and no transprog specified.
00436                     // only schedule a handle if not yet in progress.
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                 // reset handle and run, in case it is still set ( during error
00445                 // or when an event arrived ).
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                 // if error in current Exit, skip it.
00457                 if ( currentExit && currentExit->inError() )
00458                     currentExit = 0;
00459                 leaveState(current);
00460             }
00461         // schedule a run for the next 'step'.
00462         // if handle above finished, run will be called directly
00463         // in executePending. if handle was not finished
00464         // or stepping, it will be called after handle.
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 //        TRACE_INIT();
00479 //        if (s) {
00480 //            TRACE("Enabling events for state '"+s->getName()+"'.");
00481 //        } else
00482 //            TRACE("Enabling global events.");
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 //        TRACE_INIT();
00494 //        if (s) {
00495 //            TRACE("Disabling events for state '"+s->getName()+"'.");
00496 //        } else
00497 //            TRACE("Disabling global events.");
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         // get ename from event service, provide args as arguments
00536         // event should be activated upon entry of 'from'
00537         // guard is evaluated to get final 'ok'.
00538         // event does a requestStateChange( to );
00539         // if 'ok', execute transprog during transition.
00540 
00541         // Store guard and transprog for copy/clone semantics.
00542         // upon SM copy, recreate the handles for the copy SM with a copy of guard/transprog.
00543         // Same for args. I guess we need to store all arguments of this function to allow
00544         // proper copy semantics, such that the event handle can be created for each new SM
00545         // instance. Ownership of guard and transprog is to be determined, but seems to ly
00546         // with the SM. handle.destroy() can be called upon SM destruction.
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         // don't deliver this signal asynchronously, we want it right away.
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; // 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 yield point
00574         // the transition and/or exit program can cleanup...
00575 
00576         MutexLock lock(execlock); // recursive
00577 
00578         if ( !current)
00579             return true;
00580 
00581         // Only transition if this event was meant for this state and we are not
00582         // in transition already.
00583         // If condition fails, check precondition 'else' state (if present) and
00584         // execute else program (may be null).
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                 // stepping == true ! We don't want to execute the whole state transition
00596                 changeState( to, p, true );              //  valid transition to 'to'.
00597                 // trigger EE in order to force execution of the remainder of the state transition:
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         // bad idea, user, don't run this if we're not active...
00625         if( current == 0 )
00626             return 0;
00627         // only a run program may be interrupted...
00628         if ( !interruptible() || currentTrans ) {
00629             return current; // can not accept request, still in transition.
00630         }
00631 
00632         // Reset global conditions.
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() ) // avoid reseting too much in stepping mode.
00638             for ( it= it1; it != it2; ++it)
00639                 get<0>(*it)->reset();
00640 
00641         if ( reqstep == reqend ) { // if nothing to evaluate, eval globals, then just handle()
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 ) // handle current if no next
00648                         changeState( current, get<4>(*it1).get(), stepping );
00649                     else
00650                         changeState( next, get<4>(*it1).get(), stepping );
00651                     // the request was accepted
00652                     return current;
00653                 }
00654             // no transition found: handle()
00655             changeState( current, 0, stepping );
00656             return current;
00657         }
00658 
00659         // if we got here, at least one evaluation to check
00660         do {
00661             if ( get<0>(*reqstep)->evaluate() ) {
00662                 // evaluate() might call stop() or other sm functions:
00663                 if (reqstep == reqend )
00664                     return current;
00665                 // check preconds of target state :
00666                 int cres = checkConditions( get<1>(*reqstep), stepping );
00667                 if (cres == 0) {
00668                     break; // only returned in stepping
00669                 }
00670                 if( cres == 1) {
00671                     changeState( get<1>(*reqstep), get<4>(*reqstep).get(), stepping );
00672                     break; // valid transition
00673                 }
00674                 // if cres == -1 : precondition failed, increment reqstep...
00675             }
00676              if ( reqstep + 1 == reqend ) {
00677                 // to a state specified by the user (global)
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) // handle current if no next
00682                                  changeState( current, get<4>(*it1).get(), stepping );
00683                              else
00684                                  changeState( next, get<4>(*it1).get(), stepping );
00685                              // the request was accepted
00686                              return current;
00687                          }
00688                     }
00689                 // no transition was found, reset and 'schedule' a handle :
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         // if the preconditions of \a state are checked the first time in stepping mode, reset the iterators.
00707         if ( !checking_precond || !stepping ) {
00708             prec_it = precondMap.equal_range(state); // state is the _target_ state
00709         }
00710 
00711         // will be set to true if stepping below.
00712         //checking_precond = false;
00713 
00714         while ( prec_it.first != prec_it.second ) {
00715             if (checking_precond == false && stepping ) {
00716                 evaluating = prec_it.first->second.second; // indicate we will evaluate this line (if any).
00717                 checking_precond = true;
00718                 return 0;
00719             }
00720             if ( prec_it.first->second.first->evaluate() == false ) {
00721                 checking_precond = false;
00722                 return -1; // precondition failed
00723             }
00724             ++( prec_it.first );
00725             if (stepping) {
00726                 if ( prec_it.first != prec_it.second )
00727                     evaluating = prec_it.first->second.second; // indicate we will evaluate the next line (if any).
00728                 checking_precond = true;
00729                 return 0; // not done yet.
00730             }
00731         }
00732         checking_precond = false;
00733         return 1; // success !
00734     }
00735 
00736 
00737     StateInterface* StateMachine::nextState()
00738     {
00739         os::MutexLock lock(execlock);
00740         // bad idea, user, don't run this if we're not active...
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         // also check the global transitions.
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         // bad idea, user, don't run this if we're not active...
00795         if( current == 0 )
00796             return false;
00797 
00798         if ( !interruptible() ) {
00799             return false; // can not accept request, still in transition
00800         }
00801 
00802         // we may make transition to next state :
00803 
00804         // to current state
00805         if ( current == s_n )
00806         {
00807             changeState( s_n, 0 );
00808             executePending();
00809             return true;
00810         }
00811 
00812         // between 2 states specified by the user.
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                 // the request was accepted
00823                 executePending();
00824                 return true;
00825             }
00826 
00827         // to a state specified by the user (global)
00828         it1 = stateMap.find( 0 )->second.begin();
00829         it2 = stateMap.find( 0 )->second.end();
00830 
00831         // reset all conditions
00832         for ( it= it1; it != it2; ++it)
00833             get<0>(*it)->reset();
00834 
00835         // evaluate them
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                 // the request was accepted
00842                 executePending();
00843                 return true;
00844             }
00845 
00846         // to final state
00847         if ( finistate == s_n )
00848         {
00849             changeState( s_n, 0 );
00850             executePending();
00851             return true;
00852         }
00853 
00854         // to inital state from final state
00855         if ( initstate == s_n && current == finistate)
00856         {
00857             changeState( s_n, 0 );
00858             executePending();
00859             return true;
00860         }
00861 
00862         // the request has failed.
00863         return false;
00864     }
00865 
00866     int StateMachine::getLineNumber() const {
00867         // if not activated, return first line.
00868         // work around race condition that current[Prog] may become zero,
00869         // thus first store in local variable :
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         // if none of the above, return entry point :
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         // we must be inactive.
00890         if ( current != 0)
00891             return;
00892         precondMap.insert( make_pair(state, make_pair( cnd, line)) );
00893         stateMap[state]; // add to state map.
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         // we must be inactive.
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         // insert both from and to in the statemap
00916         TransList::iterator it;
00917         for ( it= stateMap[from].begin(); it != stateMap[from].end() && get<2>(*it) >= priority; ++it)
00918             ; // this ';' is intentional
00919         stateMap[from].insert(it, boost::make_tuple( cnd, to, priority, line, transprog ) );
00920         stateMap[to]; // insert empty vector for 'to' state.
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 //        TRACE_INIT();
00937 //        TRACE( "Planning to leave state " + s->getName() );
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 //        TRACE_INIT();
00973 //        TRACE( "Planning to enter state " + s->getName() );
00974 
00975         // Before a state is entered, all transitions are reset !
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         // This function has great resposibility, since it acts like
00992         // a scheduler for pending requests. It tries to devise what to
00993         // do on basis of the contents of variables (like current*, next,...).
00994         // This is a somewhat
00995         // fragile implementation but requires very little bookkeeping.
00996         // if returns true : a transition (exit/entry) is done
00997         // and a new state may be requested.
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             // done.
01009             TRACE("Finished  entry program of '"+ (current ? current->getName() : "(null)") +"'" );
01010             // in stepping mode, delay 'true' one executePending().
01011             if ( stepping ) {
01012                 currentProg = currentRun;
01013                 return false;
01014             }
01015         }
01016         // from this point on, events must be enabled.
01017 
01018         // first try to execute transition program on behalf of current state.
01019         if ( currentTrans ) {
01020             TRACE("Executing transition program from '"+ (current ? current->getName() : "(null)") + "' to '"+ ( next ? next->getName() : "(null)")+"'" );
01021             // exception : transition during handle, first finish handle !
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             // done.
01029             TRACE("Finished  transition program from '"+ (current ? current->getName() : "(null)") + "' to '"+ ( next ? next->getName() : "(null)")+"'" );
01030             // in stepping mode, delay 'true' one executePending().
01031             if ( stepping ) {
01032                 currentProg = currentExit ? currentExit : (currentEntry ? currentEntry : currentRun);
01033                 return false;
01034             }
01035         }
01036 
01037         // last is exit
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             // done.
01043             TRACE("Finished  exit program from '"+ (current ? current->getName() : "(null)") + "' (going to '"+ (next ? next->getName() : "(null)") +"')" );
01044             // in stepping mode, delay 'true' one executePending().
01045             if ( stepping ) {
01046                 currentProg = (currentEntry ? currentEntry : currentRun);
01047                 return false;
01048             }
01049         }
01050 
01051         // we only get here if all entry/transition/exit programs have been executed.
01052         // so now we schedule a new entry program for the next state.
01053 
01054         // only reset the reqstep if we changed state.
01055         // if we did not change state, it will be reset in requestNextState().
01056         if ( current != next ) {
01057             if ( next ) {
01058                 reqstep = stateMap.find( next )->second.begin();
01059                 reqend  = stateMap.find( next )->second.end();
01060                 // init for getLineNumber() :
01061                 if ( reqstep == reqend )
01062                     evaluating = 0;
01063                 else
01064                     evaluating = get<3>(*reqstep);
01065             } else {
01066                 current = 0;
01067                 return true;  // done if current == 0 !
01068             }
01069             // make change transition after exit of previous state:
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         // give new current a chance to execute the entry program and run program :
01080         if ( currentEntry ) {
01081             TRACE("Executing entry program of '"+ (current ? current->getName() : "(null)") +"'" );
01082             if ( this->executeProgram(currentEntry, stepping) == false )
01083                 return false;
01084             // done.
01085             TRACE("Finished  entry program of '"+ (current ? current->getName() : "(null)") +"'" );
01086             // in stepping mode, delay 'true' one executePending().
01087             if ( stepping ) {
01088                 currentProg = currentRun;
01089                 return false;
01090             }
01091         }
01092 
01093         // Handle is executed after the transitions failed.
01094         if ( currentHandle ) {
01095             TRACE("Executing handle program of '"+ (current ? current->getName() : "(null)") +"'" );
01096             if ( this->executeProgram(currentHandle, stepping) == false )
01097                 return false;
01098             // done.
01099             TRACE("Finished  handle program of '"+ (current ? current->getName() : "(null)") +"'" );
01100             // in stepping mode, delay 'true' one executePending().
01101             if ( stepping ) {
01102                 currentProg = currentRun;
01103                 return false;
01104             }
01105         }
01106 
01107         // Run is executed before the transitions.
01108         if ( currentRun ) {
01109             TRACE("Executing run program of '"+ (current ? current->getName() : "(null)") +"'" );
01110             if ( this->executeProgram(currentRun, stepping) == false )
01111                 return false;
01112             // done.
01113             TRACE("Finished  run program of '"+ (current ? current->getName() : "(null)") +"'" );
01114             // in stepping mode, delay 'true' one executePending().
01115             if ( stepping )
01116                 return false;
01117         }
01118 
01119         return true; // all pending is done
01120     }
01121 
01122     bool StateMachine::executeProgram(ProgramInterface*& cp, bool stepping)
01123     {
01124         TRACE_INIT();
01125         if ( cp == 0)
01126             return false;
01127         // execute this stateprogram and cleanup if needed.
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     // in exit/entry or transition programs
01149     bool StateMachine::inTransition() const {
01150         return ( (currentProg != 0)  && (currentProg != currentRun) && (currentProg != currentHandle) ) || (current != next);
01151     }
01152 
01153     // only run program may be interrupted.
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         // inactive implies loaded, but check additionally if smp is at least active
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; //preconditions not met.
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; // fail to activate.
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         // Enable all event handlers
01207         enableGlobalEvents();
01208         enableEvents(current);
01209 
01210         // execute the entry program of the initial state.
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         // the only time to refuse executing this is when we did set ourselves to inactive before.
01231         if ( smStatus == Status::inactive) {
01232             TRACE("Won't deactivate: already inactive.");
01233             return false;
01234         }
01235         os::MutexLock lock(execlock);
01236 
01237         // disable all event handlers
01238         disableGlobalEvents();
01239 
01240         // whatever state we are in, leave it.
01241         // but if current exit is in error, skip it alltogether.
01242         if ( currentExit && currentExit->inError() )
01243             currentExit = 0;
01244         if ( currentTrans && currentTrans->inError() )
01245             currentTrans = 0;
01246         // if we stalled, in previous deactivate
01247         // even skip/stop exit program.
01248         if ( next != 0 && current ) {
01249             leaveState( current );
01250             disableEvents(current);
01251         } else {
01252             currentExit = 0;
01253             currentTrans = 0;
01254         }
01255 
01256         // do not call enterState( 0 )
01257         currentProg = 0;
01258         currentEntry = 0;
01259         currentHandle  = 0;
01260         currentRun  = 0;
01261         next = 0;
01262 
01263         // this will execute the exitFunction (if any) and, if successfull,
01264         // set current to zero (using next).
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 }


rtt
Author(s): RTT Developers
autogenerated on Sat Jun 8 2019 18:46:32