$search
00001 /*************************************************************************** 00002 tag: Peter Soetens Tue Dec 21 22:43:07 CET 2004 StateMachineTree.hpp 00003 00004 StateMachineTree.hpp - description 00005 ------------------- 00006 begin : Tue December 21 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 00038 00039 #ifndef HIERARCHICAL_STATE_MACHINE_HPP 00040 #define HIERARCHICAL_STATE_MACHINE_HPP 00041 00042 #include "rtt-scripting-config.h" 00043 #include "StateInterface.hpp" 00044 #include "ConditionInterface.hpp" 00045 #include "../base/ActionInterface.hpp" 00046 #include "../base/ExecutableInterface.hpp" 00047 #include "../base/DataSourceBase.hpp" 00048 #include "../Handle.hpp" 00049 #include "../os/Mutex.hpp" 00050 00051 #include <map> 00052 #include <vector> 00053 #include <string> 00054 #include <utility> 00055 #include <boost/tuple/tuple.hpp> 00056 #include <boost/weak_ptr.hpp> 00057 #include <boost/shared_ptr.hpp> 00058 00059 namespace RTT 00060 { namespace scripting { 00061 typedef boost::shared_ptr<StateMachine> StateMachinePtr; 00062 typedef boost::weak_ptr<StateMachine> StateMachineWPtr; 00063 00072 class RTT_SCRIPTING_API StateMachine 00073 : public base::ExecutableInterface 00074 { 00075 enum PrivateStatus { nill, gostop, goreset, pausing } smpStatus; 00076 00077 static std::string emptyString; 00078 public: 00082 struct RTT_SCRIPTING_API Status { 00083 enum StateMachineStatus {inactive, activating, active, requesting, running, stopping, stopped, resetting, deactivating, paused, error, unloaded }; 00084 }; 00085 protected: 00090 typedef std::vector< boost::tuple<ConditionInterface*, StateInterface*, int, int, boost::shared_ptr<ProgramInterface> > > TransList; 00091 typedef std::map< StateInterface*, TransList > TransitionMap; 00092 typedef std::multimap< StateInterface*, std::pair<ConditionInterface*, int> > PreConditionMap; 00093 typedef std::vector< boost::tuple<ServicePtr, 00094 std::string, std::vector<base::DataSourceBase::shared_ptr>, 00095 StateInterface*, 00096 ConditionInterface*, boost::shared_ptr<ProgramInterface>, 00097 Handle, 00098 StateInterface*, boost::shared_ptr<ProgramInterface> > > EventList; 00099 typedef std::map< StateInterface*, EventList > EventMap; 00100 std::vector<StateMachinePtr> _children; 00101 typedef boost::weak_ptr<StateMachine> StateMachineParentPtr; 00102 StateMachineParentPtr _parent; 00103 00104 std::string _name; 00105 Status::StateMachineStatus smStatus; 00106 00107 public: 00108 00109 typedef std::vector<StateMachinePtr> ChildList; 00110 00115 virtual ~StateMachine(); 00116 00122 StateMachine(StateMachinePtr parent, const std::string& name="Default"); 00123 00127 void trace(bool on_off); 00128 00132 bool requestState(const std::string& statename) { 00133 StateInterface* tmp = this->getState(statename); 00134 if (tmp) { 00135 return this->requestStateChange( tmp ); 00136 } 00137 return false; 00138 } 00139 00143 bool inState(const std::string& state) const { 00144 StateInterface* copy = this->currentState(); 00145 if (copy == 0) 00146 return false; 00147 return copy->getName() == state; 00148 } 00149 00154 bool inStrictState(const std::string& state) const { 00155 StateInterface* copy = this->currentState(); 00156 if (copy == 0) 00157 return false; 00158 return copy->getName() == state && !this->inTransition(); 00159 } 00160 00164 const std::string& getCurrentStateName() const { 00165 StateInterface* copy = this->currentState(); 00166 if (copy == 0) 00167 return emptyString; 00168 return copy->getName(); 00169 } 00170 00174 inline bool isStrictlyActive() const { 00175 return this->isActive() && !this->inTransition(); 00176 } 00177 00181 inline bool inInitialState() const { 00182 return initstate == current;// && !_sc->inTransition(); 00183 } 00184 00188 inline bool inFinalState() const { 00189 return finistate == current;// && !this->inTransition(); 00190 } 00191 00196 bool stepDone() const { 00197 if ( isPaused() ) 00198 return !mstep; 00199 return isStrictlyActive(); 00200 } 00201 00205 inline bool isActive() const { return current != 0; } 00206 00211 inline bool isStopped() const { return smStatus == Status::stopped; } 00212 00216 inline bool inError() const { return smStatus == Status::error; } 00217 00221 inline bool isReactive() const { return current != 0 && smStatus != Status::running; } 00222 00227 inline bool isAutomatic() const { return smStatus == Status::running; } 00228 00232 inline bool isPaused() const { return smStatus == Status::paused; } 00233 00237 bool activate(); 00238 00242 bool pause(); 00243 00248 bool step(); 00249 00253 bool automatic(); 00254 00258 bool start(); 00259 00264 bool stop(); 00265 00270 bool reset(); 00271 00275 bool reactive(); 00276 00280 bool deactivate(); 00281 00286 bool execute(); 00287 00288 void loading(); 00289 00290 void unloading(); 00291 00307 StateInterface* requestNextState(bool stepping = false); 00308 00313 StateInterface* requestNextStateStep(); 00314 00319 bool requestFinalState(); 00320 00331 bool requestInitialState(); 00332 00341 StateInterface* nextState(); 00342 00346 std::vector<std::string> getStateList() const; 00347 00351 StateInterface* getState( const std::string & name ) const; 00352 00356 Status::StateMachineStatus getStatus() const; 00357 00361 std::string getStatusStr() const; 00362 00366 void addState( StateInterface* s ); 00367 00380 bool requestStateChange( StateInterface * s_n ); 00381 00395 bool executePending( bool stepping = false ); 00396 00414 void preconditionSet( StateInterface* state, ConditionInterface* cnd, int line); 00415 00435 void transitionSet( StateInterface* from, StateInterface* to, ConditionInterface* cnd, int priority, int line); 00436 00459 void transitionSet( StateInterface* from, StateInterface* to, 00460 ConditionInterface* cnd, boost::shared_ptr<ProgramInterface> transprog, 00461 int priority, int line); 00462 00482 bool createEventTransition( ServicePtr sp, 00483 const std::string& ename, std::vector<base::DataSourceBase::shared_ptr> args, 00484 StateInterface* from, StateInterface* to, 00485 ConditionInterface* guard, boost::shared_ptr<ProgramInterface> transprog, 00486 StateInterface* elseto = 0, boost::shared_ptr<ProgramInterface> elseprog = 00487 boost::shared_ptr<ProgramInterface>() ); 00491 void setInitialState( StateInterface* s ); 00492 00496 void setFinalState( StateInterface* s ); 00497 00502 StateInterface* currentState() const; 00503 00508 ProgramInterface* currentProgram() const; 00509 00513 StateInterface* getInitialState() const { 00514 return initstate; 00515 } 00516 00520 StateInterface* getFinalState() const { 00521 return finistate; 00522 } 00523 00531 void setInitCommand( base::ActionInterface* c) 00532 { 00533 initc = c; 00534 } 00535 00536 base::ActionInterface* getInitCommand() const 00537 { 00538 return initc; 00539 } 00540 00544 StateMachinePtr getParent() const 00545 { 00546 return _parent.lock(); 00547 } 00548 00549 void setParent(StateMachinePtr parent) 00550 { 00551 _parent = parent; 00552 } 00553 00557 const ChildList& getChildren() const 00558 { 00559 return _children; 00560 } 00561 00562 void addChild( StateMachinePtr child ) { 00563 _children.push_back( child ); 00564 } 00565 00570 const std::string& getName() const { 00571 return _name; 00572 } 00573 00578 int getLineNumber() const; 00579 00583 virtual std::string getText() const; 00584 00591 bool inTransition() const; 00592 00598 bool interruptible() const; 00599 00600 protected: 00606 TransitionMap stateMap; 00607 00612 PreConditionMap precondMap; 00613 00618 EventMap eventMap; 00619 00620 void changeState( StateInterface* s, ProgramInterface* tprog, bool stepping = false ); 00621 00622 void leaveState( StateInterface* s ); 00623 00624 void enterState( StateInterface* s ); 00625 00626 void runState( StateInterface* s ); 00627 00628 void handleState( StateInterface* s ); 00629 00630 bool executeProgram(ProgramInterface*& cp, bool stepping); 00631 00632 int checkConditions( StateInterface* state, bool stepping = false ); 00633 00634 void enableGlobalEvents(); 00635 void disableGlobalEvents(); 00636 void enableEvents( StateInterface* s ); 00637 void disableEvents( StateInterface* s ); 00638 private: 00639 00644 bool eventTransition( StateInterface* from, ConditionInterface* c, 00645 ProgramInterface* p, StateInterface* to, 00646 ProgramInterface* elsep, StateInterface* elseto ); 00647 00651 StateInterface* initstate; 00652 00656 StateInterface* finistate; 00657 00662 StateInterface* current; 00663 00667 StateInterface* next; 00668 00669 base::ActionInterface* initc; 00670 00671 ProgramInterface* currentProg; 00672 ProgramInterface* currentExit; 00673 ProgramInterface* currentHandle; 00674 ProgramInterface* currentEntry; 00675 ProgramInterface* currentRun; 00676 ProgramInterface* currentTrans; 00677 00678 TransList::iterator reqstep; 00679 TransList::iterator reqend; 00680 00681 std::pair<PreConditionMap::const_iterator,PreConditionMap::const_iterator> prec_it; 00682 bool checking_precond; 00683 bool mstep, mtrace; 00684 00685 int evaluating; 00686 00687 os::MutexRecursive execlock; 00688 }; 00689 }} 00690 00691 #endif