Orocos Real-Time Toolkit  2.6.0
StateMachine.cpp
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             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                     get<6>(*eit).disconnect();
00503                 }
00504 
00505     }
00506 
00507     bool StateMachine::createEventTransition( ServicePtr sp,
00508                                               const std::string& ename, vector<DataSourceBase::shared_ptr> args,
00509                                               StateInterface* from, StateInterface* to,
00510                                               ConditionInterface* guard, boost::shared_ptr<ProgramInterface> transprog,
00511                                               StateInterface* elseto, boost::shared_ptr<ProgramInterface> elseprog )
00512     {
00513         Logger::In in("StateMachine::createEventTransition");
00514         if ( false ) { // todo: check for OwnThread
00515             log(Error) << "Can not receive event '"<< ename <<"' in StateMachine for Operation not executed in OwnThread."<< endlog();
00516             return false;
00517         }
00518 
00519         if ( !( sp && guard ) ) {
00520             log(Error) << "Invalid arguments for event '"<< ename <<"'. ";
00521             if (!sp)
00522                 log() <<"EventService was null. ";
00523             if (!guard)
00524                 log() <<"Guard Condition was null. ";
00525             log() << endlog();
00526             return false;
00527         }
00528 
00529         if ( to == 0 )
00530             to = from;
00531 
00532         // get ename from event service, provide args as arguments
00533         // event should be activated upon entry of 'from'
00534         // guard is evaluated to get final 'ok'.
00535         // event does a requestStateChange( to );
00536         // if 'ok', execute transprog during transition.
00537 
00538         // Store guard and transprog for copy/clone semantics.
00539         // upon SM copy, recreate the handles for the copy SM with a copy of guard/transprog.
00540         // Same for args. I guess we need to store all arguments of this function to allow
00541         // proper copy semantics, such that the event handle can be created for each new SM
00542         // instance. Ownership of guard and transprog is to be determined, but seems to ly
00543         // with the SM. handle.destroy() can be called upon SM destruction.
00544         Handle handle;
00545 
00546         log(Debug) << "Creating Signal handler for Operation '"<< ename <<"'."<<Logger::endl;
00547 #ifdef ORO_SIGNALLING_OPERATIONS
00548         handle = sp->produceSignal( ename, new CommandFunction( boost::bind( &StateMachine::eventTransition, this, from, guard, transprog.get(), to, elseprog.get(), elseto) ), args );
00549 #endif
00550         if ( !handle.ready() ) {
00551             Logger::log() << Logger::Error << "Could not setup handle for event '"<<ename<<"'."<<Logger::endl;
00552             return false; // event does not exist...
00553         }
00554         // BIG NOTE : we MUST store handle otherwise, the connection is destroyed (cfr setup vs connect).
00555         // Off course, we also need it to connect/disconnect the event.
00556         eventMap[from].push_back( boost::make_tuple( sp, ename, args, to, guard, transprog, handle, elseto, elseprog) );
00557         // add the states to the statemap.
00558         stateMap[from];
00559         stateMap[to];
00560         return true;
00561     }
00562 
00563     bool StateMachine::eventTransition(StateInterface* from, ConditionInterface* c, ProgramInterface* p, StateInterface* to, ProgramInterface* elsep, StateInterface* elseto )
00564     {
00565         TRACE_INIT();
00566         // called by event to begin Transition to 'to'.
00567         // This interrupts the current run program at an interruption point ?
00568         // the transition and/or exit program can cleanup...
00569 
00570         // this will never be called if the event connection is destroyed, unless called from the
00571         // CompletionProcessor (asyn event arrival). Therefore we must add extra checks :
00572         // only transition if this event was meant for this state and we are not
00573         // in transition already.
00574         // If condition fails, check precondition 'else' state (if present) and
00575         // execute else program (may be null).
00576         if ( !current)
00577             return true;
00578 
00579         TRACE("Received Signal in state '"+ current->getName()+"'.");
00580         if (from == 0)
00581             from  = current;
00582         if (to == 0)
00583             to = current;
00584         if ( from == current && !this->inTransition() ) {
00585             if ( c->evaluate() && checkConditions(to, false) == 1 ) {
00586                 TRACE( "Valid transition from " + from->getName() +
00587                             +" to "+to->getName()+".");
00588                 changeState( to, p );              //  valid transition to 'to'.
00589             }
00590             else {
00591                 TRACE( "Rejected transition from " + from->getName() +
00592                         " to " + to->getName() +
00593                         " within state " + current->getName() + ": guards failed.");
00594             }
00595         }
00596 #if 1
00597         else {
00598             if (this->inTransition() ) {
00599                 TRACE( "Rejected transition from " + from->getName() +
00600                               " within " + current->getName() + ": in transition.");
00601             } else {
00602                 TRACE( "Rejected transition from " + from->getName() +
00603                               + " within " + current->getName() + ": wrong state.");
00604             }
00605         }
00606 #endif
00607         return true;
00608     }
00609 
00610     StateInterface* StateMachine::requestNextState(bool stepping)
00611     {
00612         TRACE_INIT();
00613         // bad idea, user, don't run this if we're not active...
00614         if( current == 0 )
00615             return 0;
00616         // only a run program may be interrupted...
00617         if ( !interruptible() || currentTrans ) {
00618             return current; // can not accept request, still in transition.
00619         }
00620 
00621         // Reset global conditions.
00622         TransList::const_iterator it, it1, it2;
00623         it1 = stateMap.find( 0 )->second.begin();
00624         it2 = stateMap.find( 0 )->second.end();
00625 
00626         if ( reqstep == stateMap.find( current )->second.begin() ) // avoid reseting too much in stepping mode.
00627             for ( it= it1; it != it2; ++it)
00628                 get<0>(*it)->reset();
00629 
00630         if ( reqstep == reqend ) { // if nothing to evaluate, eval globals, then just handle()
00631 
00632             for ( ; it1 != it2; ++it1 )
00633                 if ( get<0>(*it1)->evaluate()
00634                      && checkConditions( get<1>(*it1) ) == 1 ) {
00635                     StateInterface* next = get<1>(*it1);
00636                     if ( next == 0 ) // handle current if no next
00637                         changeState( current, get<4>(*it1).get(), stepping );
00638                     else
00639                         changeState( next, get<4>(*it1).get(), stepping );
00640                     // the request was accepted
00641                     return current;
00642                 }
00643             // no transition found: handle()
00644             changeState( current, 0, stepping );
00645             return current;
00646         }
00647 
00648         // if we got here, at least one evaluation to check
00649         do {
00650             if ( get<0>(*reqstep)->evaluate() ) {
00651                 // evaluate() might call stop() or other sm functions:
00652                 if (reqstep == reqend )
00653                     return current;
00654                 // check preconds of target state :
00655                 int cres = checkConditions( get<1>(*reqstep), stepping );
00656                 if (cres == 0) {
00657                     break; // only returned in stepping
00658                 }
00659                 if( cres == 1) {
00660                     changeState( get<1>(*reqstep), get<4>(*reqstep).get(), stepping );
00661                     break; // valid transition
00662                 }
00663                 // if cres == -1 : precondition failed, increment reqstep...
00664             }
00665             if ( reqstep + 1 == reqend ) {
00666                 // to a state specified by the user (global)
00667                 for ( ; it1 != it2; ++it1 ) {
00668                     if ( get<0>(*it1)->evaluate() && checkConditions( get<1>(*it1) ) == 1 ) {
00669                              StateInterface* next = get<1>(*it1);
00670                              if ( next == 0) // handle current if no next
00671                                  changeState( current, get<4>(*it1).get(), stepping );
00672                              else
00673                                  changeState( next, get<4>(*it1).get(), stepping );
00674                              // the request was accepted
00675                              return current;
00676                          }
00677                     }
00678                 // no transition was found, reset and 'schedule' a handle :
00679                 reqstep = stateMap.find( current )->second.begin();
00680                 evaluating = get<3>(*reqstep);
00681                 changeState( current, 0, stepping );
00682                 break;
00683             }
00684             else {
00685                 ++reqstep;
00686                 evaluating = get<3>(*reqstep);
00687             }
00688         } while ( !stepping );
00689 
00690         return current;
00691     }
00692 
00693     int StateMachine::checkConditions( StateInterface* state, bool stepping ) {
00694 
00695         // if the preconditions of \a state are checked the first time in stepping mode, reset the iterators.
00696         if ( !checking_precond || !stepping ) {
00697             prec_it = precondMap.equal_range(state); // state is the _target_ state
00698         }
00699 
00700         // will be set to true if stepping below.
00701         //checking_precond = false;
00702 
00703         while ( prec_it.first != prec_it.second ) {
00704             if (checking_precond == false && stepping ) {
00705                 evaluating = prec_it.first->second.second; // indicate we will evaluate this line (if any).
00706                 checking_precond = true;
00707                 return 0;
00708             }
00709             if ( prec_it.first->second.first->evaluate() == false ) {
00710                 checking_precond = false;
00711                 return -1; // precondition failed
00712             }
00713             ++( prec_it.first );
00714             if (stepping) {
00715                 if ( prec_it.first != prec_it.second )
00716                     evaluating = prec_it.first->second.second; // indicate we will evaluate the next line (if any).
00717                 checking_precond = true;
00718                 return 0; // not done yet.
00719             }
00720         }
00721         checking_precond = false;
00722         return 1; // success !
00723     }
00724 
00725 
00726     StateInterface* StateMachine::nextState()
00727     {
00728         // bad idea, user, don't run this if we're not active...
00729         if ( current == 0 )
00730             return 0;
00731         TransList::const_iterator it1, it2;
00732         it1 = stateMap.find( current )->second.begin();
00733         it2 = stateMap.find( current )->second.end();
00734 
00735         for ( ; it1 != it2; ++it1 )
00736             if ( get<0>(*it1)->evaluate() && checkConditions( get<1>(*it1)) == 1 ) {
00737                 return get<1>(*it1);
00738             }
00739 
00740         // also check the global transitions.
00741         it1 = stateMap.find( 0 )->second.begin();
00742         it2 = stateMap.find( 0 )->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         return current;
00750     }
00751 
00752     std::vector<std::string> StateMachine::getStateList() const {
00753         vector<string> result;
00754         vector<StateInterface*> sl;
00755         StateInterface* dummy = 0;
00756         transform( stateMap.begin(), stateMap.end(), back_inserter(sl), select1st<TransitionMap::value_type>() );
00757         sl.erase( find(sl.begin(), sl.end(), dummy) );
00758         transform( sl.begin(), sl.end(), back_inserter(result), boost::bind( &StateInterface::getName, _1 ) );
00759         return result;
00760     }
00761 
00762     void StateMachine::addState( StateInterface* s )
00763     {
00764         stateMap[s];
00765     }
00766 
00767 
00768     StateInterface* StateMachine::getState(const string& name) const
00769     {
00770         TransitionMap::const_iterator it = stateMap.begin();
00771         while ( it != stateMap.end() ) {
00772             if ( it->first && it->first->getName() == name )
00773                 return it->first;
00774             ++it;
00775         }
00776         return 0;
00777     }
00778 
00779     bool StateMachine::requestStateChange( StateInterface * s_n )
00780     {
00781         // bad idea, user, don't run this if we're not active...
00782         if( current == 0 )
00783             return false;
00784 
00785         if ( !interruptible() ) {
00786             return false; // can not accept request, still in transition
00787         }
00788 
00789         // we may make transition to next state :
00790 
00791         // to current state
00792         if ( current == s_n )
00793         {
00794             changeState( s_n, 0 );
00795             return true;
00796         }
00797 
00798         // between 2 states specified by the user.
00799         TransList::iterator it, it1, it2;
00800         it1 = stateMap.find( current )->second.begin();
00801         it2 = stateMap.find( current )->second.end();
00802 
00803         for ( ; it1 != it2; ++it1 )
00804             if ( get<1>(*it1) == s_n
00805                  && get<0>(*it1)->evaluate()
00806                  && checkConditions( s_n ) == 1 ) {
00807                 changeState( s_n, get<4>(*it1).get() );
00808                 // the request was accepted
00809                 return true;
00810             }
00811 
00812         // to a state specified by the user (global)
00813         it1 = stateMap.find( 0 )->second.begin();
00814         it2 = stateMap.find( 0 )->second.end();
00815 
00816         // reset all conditions
00817         for ( it= it1; it != it2; ++it)
00818             get<0>(*it)->reset();
00819 
00820         // evaluate them
00821         for ( ; it1 != it2; ++it1 )
00822             if ( get<1>(*it1) == s_n
00823                  && get<0>(*it1)->evaluate()
00824                  && checkConditions( s_n ) == 1 ) {
00825                 changeState( s_n, get<4>(*it1).get() );
00826                 // the request was accepted
00827                 return true;
00828             }
00829 
00830         // to final state
00831         if ( finistate == s_n )
00832         {
00833             changeState( s_n, 0 );
00834             return true;
00835         }
00836 
00837         // to inital state from final state
00838         if ( initstate == s_n && current == finistate)
00839         {
00840             changeState( s_n, 0 );
00841             return true;
00842         }
00843 
00844         // the request has failed.
00845         return false;
00846     }
00847 
00848     int StateMachine::getLineNumber() const {
00849         // if not activated, return first line.
00850         // work around race condition that current[Prog] may become zero,
00851         // thus first store in local variable :
00852         StateInterface* statecopy = current;
00853         if ( statecopy == 0 )
00854             return 1;
00855         ProgramInterface* copy = currentProg;
00856         if ( copy )
00857             return copy->getLineNumber();
00858         if ( evaluating )
00859             return evaluating;
00860 
00861         // if none of the above, return entry point :
00862         return statecopy->getEntryPoint();
00863     }
00864 
00865     string StateMachine::getText() const {
00866         return string();
00867     }
00868 
00869     void StateMachine::preconditionSet(StateInterface* state, ConditionInterface* cnd, int line )
00870     {
00871         // we must be inactive.
00872         if ( current != 0)
00873             return;
00874         precondMap.insert( make_pair(state, make_pair( cnd, line)) );
00875         stateMap[state]; // add to state map.
00876     }
00877 
00878     void StateMachine::transitionSet( StateInterface* from, StateInterface* to, ConditionInterface* cnd, int priority, int line )
00879     {
00880         this->transitionSet( from, to, cnd, boost::shared_ptr<ProgramInterface>(), priority, line);
00881     }
00882 
00883     void StateMachine::transitionSet( StateInterface* from, StateInterface* to,
00884                                       ConditionInterface* cnd, boost::shared_ptr<ProgramInterface> transprog,
00885                                       int priority, int line )
00886     {
00887         TRACE_INIT();
00888         // we must be inactive.
00889         if ( current != 0)
00890             return;
00891 
00892         if (from) {
00893             TRACE("Created transition from "+from->getName() +"' to '"+ to->getName()+"'");
00894         } else {
00895             TRACE("Created global transition to '"+ to->getName()+"'");
00896         }
00897         // insert both from and to in the statemap
00898         TransList::iterator it;
00899         for ( it= stateMap[from].begin(); it != stateMap[from].end() && get<2>(*it) >= priority; ++it)
00900             ; // this ';' is intentional
00901         stateMap[from].insert(it, boost::make_tuple( cnd, to, priority, line, transprog ) );
00902         stateMap[to]; // insert empty vector for 'to' state.
00903     }
00904 
00905     StateInterface* StateMachine::currentState() const
00906     {
00907         return current;
00908     }
00909 
00910     ProgramInterface* StateMachine::currentProgram() const
00911     {
00912         return currentProg;
00913     }
00914 
00915     void StateMachine::leaveState( StateInterface* s )
00916     {
00917         assert(s);
00918 //        TRACE_INIT();
00919 //        TRACE( "Planning to leave state " + s->getName() );
00920         disableEvents(s);
00921         currentExit = s->getExitProgram();
00922         if ( currentExit ) {
00923             currentExit->reset();
00924             if (currentExit->start() == false)
00925                 smStatus = Status::error;
00926 
00927             if (currentProg == 0 )
00928                 currentProg = currentExit;
00929         }
00930     }
00931 
00932     void StateMachine::runState( StateInterface* s )
00933     {
00934         TRACE_INIT();
00935         assert(s);
00936         currentRun = s->getRunProgram();
00937         if ( currentRun ) {
00938             currentRun->reset();
00939             if (currentRun->start() == false)
00940                 smStatus = Status::error;
00941             if (currentProg == 0 )
00942                 currentProg = currentRun;
00943         }
00944     }
00945 
00946     void StateMachine::handleState( StateInterface* s )
00947     {
00948         assert(s);
00949         currentHandle = s->getHandleProgram();
00950         if ( currentHandle ) {
00951             currentHandle->reset();
00952             if (currentHandle->start() == false)
00953                 smStatus = Status::error;
00954             if (currentProg == 0 )
00955                 currentProg = currentHandle;
00956         }
00957     }
00958 
00959     void StateMachine::enterState( StateInterface* s )
00960     {
00961         assert(s);
00962 //        TRACE_INIT();
00963 //        TRACE( "Planning to enter state " + s->getName() );
00964 
00965         // Before a state is entered, all transitions are reset !
00966         TransList::iterator it;
00967         for ( it= stateMap.find(s)->second.begin(); it != stateMap.find(s)->second.end(); ++it)
00968             get<0>(*it)->reset();
00969 
00970         enableEvents(s);
00971 
00972         next = s;
00973         currentEntry = s->getEntryProgram();
00974         if ( currentEntry ) {
00975             currentEntry->reset();
00976             if (currentEntry->start() == false)
00977                 smStatus = Status::error;
00978             if (currentProg == 0 )
00979                 currentProg = currentEntry;
00980         }
00981     }
00982 
00983     bool StateMachine::executePending( bool stepping )
00984     {
00985         TRACE_INIT();
00986         // This function has great resposibility, since it acts like
00987         // a scheduler for pending requests. It tries to devise what to
00988         // do on basis of the contents of variables (like current*, next,...).
00989         // This is a somewhat
00990         // fragile implementation but requires very little bookkeeping.
00991         // if returns true : a transition (exit/entry) is done
00992         // and a new state may be requested.
00993 
00994         if ( inError() )
00995             return false;
00996 
00997         // first try to execute transition program on behalf of current state.
00998         if ( currentTrans ) {
00999             TRACE("Executing transition program from '"+ (current ? current->getName() : "(null)") + "' to '"+ ( next ? next->getName() : "(null)")+"'" );
01000             // exception : transition during handle, first finish handle !
01001             if ( currentHandle ) {
01002                 if ( this->executeProgram(currentHandle, stepping) == false )
01003                 return false;
01004             } else
01005                 if ( this->executeProgram(currentTrans, stepping) == false )
01006                     return false;
01007             // done.
01008             TRACE("Finished  transition program from '"+ current->getName() + "' to '"+next->getName()+"'" );
01009             // in stepping mode, delay 'true' one executePending().
01010             if ( stepping ) {
01011                 currentProg = currentExit ? currentExit : (currentEntry ? currentEntry : currentRun);
01012                 return false;
01013             }
01014         }
01015 
01016         // last is exit
01017         if ( currentExit ) {
01018             TRACE("Executing exit program from '"+ current->getName() + "' (going to '"+ (next ? next->getName() : "(null)") +"')" );
01019             if ( this->executeProgram(currentExit, stepping) == false )
01020                 return false;
01021             // done.
01022             TRACE("Finished  exit program from '"+ current->getName() + "' (going to '"+ (next ? next->getName() : "(null)") +"')" );
01023             // in stepping mode, delay 'true' one executePending().
01024             if ( stepping ) {
01025                 currentProg = (currentEntry ? currentEntry : currentRun);
01026                 return false;
01027             }
01028         }
01029 
01030 
01031         // only reset the reqstep if we changed state.
01032         // if we did not change state, it will be reset in requestNextState().
01033         if ( current != next ) {
01034             if ( next ) {
01035                 reqstep = stateMap.find( next )->second.begin();
01036                 reqend  = stateMap.find( next )->second.end();
01037                 // init for getLineNumber() :
01038                 if ( reqstep == reqend )
01039                     evaluating = 0;
01040                 else
01041                     evaluating = get<3>(*reqstep);
01042             } else {
01043                 current = 0;
01044                 return true;  // done if current == 0 !
01045             }
01046             // make change transition after exit of previous state:
01047             TRACE("Formally  transitioning from      '"+ (current ? current->getName() : "(null)") + "' to '"+ (next ? next->getName() : "(null)") +"'" );
01048             current = next;
01049         }
01050 
01051         if ( currentEntry ) {
01052             TRACE("Executing entry program of '"+ current->getName() +"'" );
01053             if ( this->executeProgram(currentEntry, stepping) == false )
01054                 return false;
01055             // done.
01056             TRACE("Finished  entry program of '"+ current->getName() +"'" );
01057             // in stepping mode, delay 'true' one executePending().
01058             if ( stepping ) {
01059                 currentProg = currentRun;
01060                 return false;
01061             }
01062         }
01063         // from this point on, events must be enabled.
01064 
01065         // Handle is executed after the transitions failed.
01066         if ( currentHandle ) {
01067             TRACE("Executing handle program of '"+ current->getName() +"'" );
01068             if ( this->executeProgram(currentHandle, stepping) == false )
01069                 return false;
01070             // done.
01071             TRACE("Finished  handle program of '"+ current->getName() +"'" );
01072             // in stepping mode, delay 'true' one executePending().
01073             if ( stepping ) {
01074                 currentProg = currentRun;
01075                 return false;
01076             }
01077         }
01078 
01079         // Run is executed before the transitions.
01080         if ( currentRun ) {
01081             TRACE("Executing run program of '"+ current->getName() +"'" );
01082             if ( this->executeProgram(currentRun, stepping) == false )
01083                 return false;
01084             // done.
01085             TRACE("Finished  run program of '"+ current->getName() +"'" );
01086             // in stepping mode, delay 'true' one executePending().
01087             if ( stepping )
01088                 return false;
01089 
01090         }
01091 
01092         return true; // all pending is done
01093     }
01094 
01095     bool StateMachine::executeProgram(ProgramInterface*& cp, bool stepping)
01096     {
01097         TRACE_INIT();
01098         if ( cp == 0)
01099             return false;
01100         // execute this stateprogram and cleanup if needed.
01101         currentProg = cp;
01102         if ( stepping )
01103             currentProg->step();
01104         else
01105             currentProg->execute();
01106         if ( currentProg->inError() ) {
01107             smStatus = Status::error;
01108             smpStatus = nill;
01109             TRACE("Encountered run-time error at line " << this->getLineNumber() );
01110             return false;
01111         }
01112 
01113         if ( currentProg && !currentProg->isStopped() )
01114             return false;
01115 
01116         cp = currentProg = 0;
01117         return true;
01118     }
01119 
01120 
01121     // in exit/entry or transition programs
01122     bool StateMachine::inTransition() const {
01123         return currentProg != 0  && currentProg != currentRun && currentProg != currentHandle;
01124     }
01125 
01126     // only run program may be interrupted.
01127     bool StateMachine::interruptible() const {
01128         return currentProg == 0  || currentProg == currentRun;
01129     }
01130 
01131     void StateMachine::setInitialState( StateInterface* s )
01132     {
01133         initstate = s;
01134         stateMap[initstate];
01135     }
01136 
01137     void StateMachine::setFinalState( StateInterface* s )
01138     {
01139         finistate = s;
01140         stateMap[finistate];
01141     }
01142 
01143     void StateMachine::trace(bool t) {
01144         mtrace =t;
01145     }
01146 
01147     bool StateMachine::activate()
01148     {
01149         TRACE_INIT();
01150         // inactive implies loaded, but check additionally if smp is at least active
01151         if ( smStatus != Status::inactive ) {
01152             TRACE("Won't activate: already active.");
01153             return false;
01154         }
01155 
01156         smpStatus = nill;
01157 
01158         if ( this->checkConditions( getInitialState() ) != 1 ) {
01159             TRACE("Won't activate: preconditions failed.");
01160             return false; //preconditions not met.
01161         }
01162 
01163         if ( initc ) {
01164             initc->reset();
01165             initc->readArguments();
01166             if ( initc->execute() == false ) {
01167                 TRACE("Won't activate: Init Commands failed.");
01168                 return false; // fail to activate.
01169             }
01170         }
01171 
01172         //current = getInitialState();
01173         enterState( getInitialState() );
01174         reqstep = stateMap.find( next )->second.begin();
01175         reqend = stateMap.find( next )->second.end();
01176 
01177         // Enable all event handlers
01178         enableGlobalEvents();
01179 
01180         // execute the entry program of the initial state.
01181         if ( !inError() ) {
01182             if ( this->executePending() ) {
01183                 smStatus = Status::active;
01184                 TRACE("Activated.");
01185             } else {
01186                 if ( !inError() ) {
01187                     TRACE("Still activating.");
01188                     smStatus = Status::activating;
01189                 }
01190             }
01191         }
01192 
01193         return true;
01194     }
01195 
01196 
01197     bool StateMachine::deactivate()
01198     {
01199         TRACE_INIT();
01200         // the only time to refuse executing this is when we did set ourselves to inactive before.
01201         if ( smStatus == Status::inactive) {
01202             TRACE("Won't deactivate: already inactive.");
01203             return false;
01204         }
01205         os::MutexLock lock(execlock);
01206 
01207         // disable all event handlers
01208         disableGlobalEvents();
01209 
01210         // whatever state we are in, leave it.
01211         // but if current exit is in error, skip it alltogether.
01212         if ( currentExit && currentExit->inError() )
01213             currentExit = 0;
01214         if ( currentTrans && currentTrans->inError() )
01215             currentTrans = 0;
01216         // if we stalled, in previous deactivate
01217         // even skip/stop exit program.
01218         if ( next != 0 && current )
01219             leaveState( current );
01220         else {
01221             currentExit = 0;
01222             currentTrans = 0;
01223         }
01224 
01225         // do not call enterState( 0 )
01226         currentProg = 0;
01227         currentEntry = 0;
01228         currentHandle  = 0;
01229         currentRun  = 0;
01230         next = 0;
01231 
01232         // this will execute the exitFunction (if any) and, if successfull,
01233         // set current to zero (using next).
01234         if ( this->executePending() ) {
01235             TRACE("Deactivated.");
01236             smStatus = Status::inactive;
01237         } else {
01238             TRACE("Still deactivating.");
01239             smStatus = Status::deactivating;
01240         }
01241 
01242         return true;
01243     }
01244 }