00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
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;
00183 }
00184
00188 inline bool inFinalState() const {
00189 return finistate == current;
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
00484 bool createEventTransition( ServicePtr sp, ExecutionEngine* target_engine,
00485 const std::string& ename, std::vector<base::DataSourceBase::shared_ptr> args,
00486 StateInterface* from, StateInterface* to,
00487 ConditionInterface* guard, boost::shared_ptr<ProgramInterface> transprog,
00488 StateInterface* elseto = 0, boost::shared_ptr<ProgramInterface> elseprog =
00489 boost::shared_ptr<ProgramInterface>() );
00493 void setInitialState( StateInterface* s );
00494
00498 void setFinalState( StateInterface* s );
00499
00504 StateInterface* currentState() const;
00505
00510 ProgramInterface* currentProgram() const;
00511
00515 StateInterface* getInitialState() const {
00516 return initstate;
00517 }
00518
00522 StateInterface* getFinalState() const {
00523 return finistate;
00524 }
00525
00533 void setInitCommand( base::ActionInterface* c)
00534 {
00535 initc = c;
00536 }
00537
00538 base::ActionInterface* getInitCommand() const
00539 {
00540 return initc;
00541 }
00542
00546 StateMachinePtr getParent() const
00547 {
00548 return _parent.lock();
00549 }
00550
00551 void setParent(StateMachinePtr parent)
00552 {
00553 _parent = parent;
00554 }
00555
00559 const ChildList& getChildren() const
00560 {
00561 return _children;
00562 }
00563
00564 void addChild( StateMachinePtr child ) {
00565 _children.push_back( child );
00566 }
00567
00572 const std::string& getName() const {
00573 return _name;
00574 }
00575
00580 int getLineNumber() const;
00581
00585 virtual std::string getText() const;
00586
00593 bool inTransition() const;
00594
00600 bool interruptible() const;
00601
00602 protected:
00608 TransitionMap stateMap;
00609
00614 PreConditionMap precondMap;
00615
00620 EventMap eventMap;
00621
00622 void changeState( StateInterface* s, ProgramInterface* tprog, bool stepping = false );
00623
00624 void leaveState( StateInterface* s );
00625
00626 void enterState( StateInterface* s );
00627
00628 void runState( StateInterface* s );
00629
00630 void handleState( StateInterface* s );
00631
00632 bool executeProgram(ProgramInterface*& cp, bool stepping);
00633
00634 int checkConditions( StateInterface* state, bool stepping = false );
00635
00636 void enableGlobalEvents();
00637 void disableGlobalEvents();
00638 void enableEvents( StateInterface* s );
00639 void disableEvents( StateInterface* s );
00640 private:
00641
00646 bool eventTransition( StateInterface* from, ConditionInterface* c,
00647 ProgramInterface* p, StateInterface* to,
00648 ProgramInterface* elsep, StateInterface* elseto );
00649
00653 StateInterface* initstate;
00654
00658 StateInterface* finistate;
00659
00664 StateInterface* current;
00665
00669 StateInterface* next;
00670
00671 base::ActionInterface* initc;
00672
00673 ProgramInterface* currentProg;
00674 ProgramInterface* currentExit;
00675 ProgramInterface* currentHandle;
00676 ProgramInterface* currentEntry;
00677 ProgramInterface* currentRun;
00678 ProgramInterface* currentTrans;
00679
00680 TransList::iterator reqstep;
00681 TransList::iterator reqend;
00682
00683 std::pair<PreConditionMap::const_iterator,PreConditionMap::const_iterator> prec_it;
00684 bool checking_precond;
00685 bool mstep, mtrace;
00686
00687 int evaluating;
00688
00689 os::MutexRecursive execlock;
00690 };
00691 }}
00692
00693 #endif