00001
00002
00003
00004
00005
00006
00007
00008 #ifndef DECISION_MAKING_TAO_H_
00009 #define DECISION_MAKING_TAO_H_
00010
00011
00012 #include "EventSystem.h"
00013 #include "TaskResult.h"
00014
00015 #ifndef DMDEBUG
00016 #define DMDEBUG(...)
00017 #endif
00018
00019 #include <boost/function.hpp>
00020 #include <boost/bind.hpp>
00021
00022 namespace decision_making{
00023
00024 class ___ABS__ScoppedThreadsOnExit{
00025 public:
00026 virtual ~___ABS__ScoppedThreadsOnExit(){}
00027 virtual void exit()=0;
00028 virtual boost::thread_group& getThreads()=0;
00029 };
00030 struct ScoppedThreads{
00031 typedef boost::shared_ptr<EventQueue> EventQueuePtr;
00032 typedef boost::shared_ptr<CallContext> CallContextPtr;
00033 typedef boost::shared_ptr<___ABS__ScoppedThreadsOnExit> ScoppedThreadsOnExitPtr;
00034 boost::thread_group threads;
00035 vector<EventQueuePtr> events;
00036 vector<CallContextPtr> contexts;
00037 void add(boost::thread* thread){threads.add_thread(thread);};
00038 void add(EventQueuePtr event){ events.push_back(event); }
00039 void add(CallContextPtr event){ contexts.push_back(event); }
00040
00041 void stopEvents(){
00042 BOOST_FOREACH(EventQueuePtr e, events){
00043 e->close();
00044 }
00045 }
00046 struct Cleaner{
00047 ScoppedThreads& target;
00048 Cleaner(ScoppedThreads& target):target(target){}
00049 ~Cleaner(){
00050 target.runOnExit();
00051 target.stopEvents();
00052 target.threads.join_all();
00053 }
00054 };
00055
00056 vector<ScoppedThreadsOnExitPtr> on_exits;
00057 void add(ScoppedThreadsOnExitPtr exit){ on_exits.push_back(exit); }
00058
00059 void runOnExit(){
00060 BOOST_FOREACH(ScoppedThreadsOnExitPtr e, on_exits){
00061 e->exit();
00062 e->getThreads().join_all();
00063 }
00064 }
00065 };
00066 class ScoppedThreadsOnExit:public ___ABS__ScoppedThreadsOnExit{
00067 public:
00068 EventQueue* events_queue;
00069 CallContext& plan_call_ctx;
00070 ScoppedThreads SUBMACHINESTHREADS;
00071 ScoppedThreadsOnExit(CallContext& call_ctx, EventQueue* events_queue):
00072 events_queue(events_queue), plan_call_ctx(call_ctx)
00073 {}
00074 virtual ~ScoppedThreadsOnExit(){}
00075
00076 virtual boost::thread_group& getThreads(){ return SUBMACHINESTHREADS.threads; }
00077 };
00078
00079 #define _TAO_FUN_SIG boost::function<decision_making::TaskResult(int)>
00080 class TaoConditionCallback{public:
00081 _TAO_FUN_SIG cond;
00082 virtual ~TaoConditionCallback(){};
00083 };
00084
00085 class ProtocolNext{
00086 protected:
00087 int& result;
00088 decision_making::CallContext* call_context;
00089 decision_making::EventQueue* events;
00090 struct Option{
00091 int id;
00092 std::string name;
00093 bool isReady;
00094 Option(int id, std::string name, bool isReady):id(id),name(name),isReady(isReady){}
00095 };
00096 std::vector<Option> options;
00097 public:
00098 ProtocolNext(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):
00099 result(res), call_context(call_context), events(events){}
00100 virtual ~ProtocolNext(){ }
00101 void add(int id, std::string name, bool isReady){ options.push_back(Option(id, name, isReady)); }
00102 virtual bool decide()=0;
00103 void resetResult(){ result = -1; }
00104
00105 struct Cleanner{
00106 ProtocolNext& p;
00107 Cleanner(ProtocolNext& p):p(p){}
00108 ~Cleanner(){ if(not p.decide()) p.resetResult(); }
00109 };
00110 };
00111
00112 class ProtocolAllocation{
00113 public:
00114 typedef boost::function<decision_making::TaskResult(void)> Callback;
00115 protected:
00116 int& result;
00117 decision_making::CallContext* call_context;
00118 decision_making::EventQueue* events;
00119 int opt_counter;
00120 struct Option{
00121 Callback callback;
00122 int id;
00123 std::string name;
00124 bool isReady;
00125 Option(int id, std::string name, bool isReady, Callback cb):callback(cb),id(id),name(name),isReady(isReady){}
00126 };
00127 std::vector<Option> options;
00128 public:
00129 ProtocolAllocation(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):
00130 result(res), call_context(call_context), events(events), opt_counter(0){}
00131 virtual ~ProtocolAllocation(){ }
00132 void add(std::string name, bool isReady, Callback cb){ options.push_back(Option(opt_counter++, name, isReady, cb)); }
00133 virtual bool decide()=0;
00134 Callback getCallback()const{
00135 return options.at(result).callback;
00136 }
00137
00138 struct Cleanner{
00139 ProtocolAllocation& p;
00140 ScoppedThreads& SUBMACHINESTHREADS;
00141 decision_making::ScoppedThreads::EventQueuePtr events;
00142 Cleanner(ProtocolAllocation& p, ScoppedThreads& SUBMACHINESTHREADS, decision_making::ScoppedThreads::EventQueuePtr events):p(p),SUBMACHINESTHREADS(SUBMACHINESTHREADS), events(events){}
00143 ~Cleanner(){
00144
00145 if( p.decide() ){
00146 SUBMACHINESTHREADS.add(events);
00147 SUBMACHINESTHREADS.add(new boost::thread( p.getCallback() ));
00148 }
00149 }
00150 };
00151 };
00152
00153 class _NextPEMPTY:public decision_making::ProtocolNext{
00154 public:
00155 _NextPEMPTY(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):ProtocolNext(res, call_context, events){}
00156 bool decide(){
00157 result = 0;
00158 return false;
00159 }
00160 };
00161 class _AllocPEMPTY:public decision_making::ProtocolAllocation{
00162 public:
00163 _AllocPEMPTY(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):ProtocolAllocation(res, call_context, events){}
00164 bool decide(){
00165 result = 0;
00166 return false;
00167 }
00168 };
00169
00170
00171 #define TAO_HEADER(NAME) \
00172 decision_making::TaskResult Tao##NAME(const decision_making::CallContext*, decision_making::EventQueue*, std::string, bool jc, int tss, TaoConditionCallback*);\
00173 decision_making::TaskResult Tao##NAME##_cond(const decision_making::CallContext* p, decision_making::EventQueue* q, int tss);\
00174 decision_making::TaskResult Tao##NAME(const decision_making::CallContext* p, decision_making::EventQueue* q);
00175
00176 #define TAO(NAME) \
00177 TAO_HEADER(NAME)\
00178 decision_making::TaskResult Tao##NAME##_cond(const decision_making::CallContext* p, decision_making::EventQueue* q, int tss){\
00179 class Cond: public TaoConditionCallback{ } f;\
00180 return Tao##NAME(p,q,#NAME, true, tss, &f);\
00181 }\
00182 decision_making::TaskResult Tao##NAME(const decision_making::CallContext* p, decision_making::EventQueue* q){\
00183 class Cond: public TaoConditionCallback{ } f;\
00184 f.cond = boost::bind(&Tao##NAME##_cond, p, q, _1);\
00185 return Tao##NAME(p,q,#NAME, false, -1, &f);\
00186 }\
00187 decision_making::TaskResult Tao##NAME(\
00188 const decision_making::CallContext* parent_call_ctx,\
00189 decision_making::EventQueue* parent_event_queue,\
00190 std::string tao_name,\
00191 bool justCondition, int try_start_state, TaoConditionCallback* this_function)
00192
00193 #define TAO_PLANS enum TAOPLANS
00194
00195 #define __DEFCALLCONTEXT decision_making::CallContext call_ctx(parent_call_ctx?decision_making::CallContext(*parent_call_ctx, tao_name):decision_making::CallContext(tao_name));
00196 #define __DEFEVENTQUEUE decision_making::EventQueue* events_queue(parent_event_queue);
00197 #define __TAO_FINISHER \
00198 struct _tao_finisher_type{\
00199 bool run;\
00200 const decision_making::CallContext* parent_call_ctx;\
00201 decision_making::EventQueue* parent_event_queue;\
00202 _tao_finisher_type(bool run, const decision_making::CallContext* parent_call_ctx, decision_making::EventQueue* parent_event_queue):\
00203 run(run),parent_call_ctx(parent_call_ctx), parent_event_queue(parent_event_queue){}\
00204 ~_tao_finisher_type(){\
00205 if(!run or !parent_event_queue or !parent_call_ctx)return; \
00206 parent_event_queue->raiseEvent(decision_making::Event("SUBPLAN_FINISHED",*parent_call_ctx));\
00207 }\
00208 } _tao_finisher(!justCondition, parent_call_ctx, parent_event_queue);\
00209
00210 #define TAO_START_PLAN(STATE) \
00211 ; int state ( try_start_state<0? (int)STATE : try_start_state ); \
00212 decision_making::TaskResult tao_result = decision_making::TaskResult::TERMINATED();\
00213 __DEFCALLCONTEXT __DEFEVENTQUEUE __TAO_FINISHER\
00214 if(!justCondition){\
00215 DMDEBUG( cout<<" TAO("<<tao_name<<":START) "; )\
00216 ON_TAO_START(tao_name, call_ctx, *events_queue);\
00217 }
00218
00219 #define TAO_BGN \
00220 bool tao_stop = false; \
00221 while(not tao_stop and not events_queue->isTerminated() DM_SYSTEM_STOP){ \
00222 switch((TAOPLANS)state){ { {
00223
00224 #define TAO_END \
00225 }}break;\
00226 default: tao_stop=true; tao_result = decision_making::TaskResult::FAIL("SELECTED BEH DOES NOT EXIST") ; break;}} \
00227 if(!justCondition){\
00228 DMDEBUG( cout<<" TAO("<<tao_name<<":FINISH) "; ) \
00229 ON_TAO_END(tao_name, call_ctx, *events_queue, tao_result);\
00230 }\
00231 return tao_result;
00232
00233 #define __STARTOFSTATE(X) \
00234 std::string plan_name(#X);\
00235 decision_making::CallContext plan_call_ctx(call_ctx, plan_name);\
00236 if(!justCondition){\
00237 DMDEBUG( string outname("STT("+tao_name+":"+call_ctx.str()+"/"+#X+")");cout<<outname<<"{ "; )\
00238 ON_TAO_STATE_START(plan_name, call_ctx, *events_queue);\
00239 }
00240
00241 #define __ENDOFSTATE \
00242 DMDEBUG( struct _STATE_FINISHER_PRINT{std::string n; bool jc; _STATE_FINISHER_PRINT( std::string n, bool jc): n(n),jc(jc){}~_STATE_FINISHER_PRINT(){if(jc)return;DMDEBUG( cout<<"}"<<n<<" "; )} void r(){}}_ep(outname,justCondition);_ep.r(); )\
00243 struct _STATE_FINISHER{\
00244 std::string plan_name; decision_making::CallContext& ctx; decision_making::EventQueue& queue; bool jc;\
00245 _STATE_FINISHER(std::string plan_name, decision_making::CallContext& ctx, decision_making::EventQueue& queue, bool jc): plan_name(plan_name),ctx(ctx),queue(queue),jc(jc){}\
00246 ~_STATE_FINISHER(){\
00247 if(!jc){ ON_TAO_STATE_END(plan_name, ctx, queue); }\
00248 } void r(){}\
00249 }_e(plan_name, call_ctx, *events_queue, justCondition);_e.r();
00250
00251 #define TAO_CONTEXT plan_call_ctx
00252
00253 #define TAO_PLAN(X) \
00254 }}}break; \
00255 case X: { \
00256 decision_making::ScoppedThreads SUBMACHINESTHREADS; \
00257 __STARTOFSTATE(X) __ENDOFSTATE
00258
00259 #define TAO_GO_NEXT(STATE) \
00260 state = STATE; \
00261 break;
00262 #define TAO_ON_EVENT(EVENT, DO) \
00263 if(event==decision_making::Event(EVENT,plan_call_ctx)){ \
00264 DMDEBUG( cout<<" GOTO("<<tao_name<<":"<<decision_making::Event(EVENT,plan_call_ctx)<< "->" #DO ") "; ) \
00265 DO;\
00266 }
00267 #define TAO_EVENT(EVENT) decision_making::Event(#EVENT,plan_call_ctx)
00268
00269 #define TAO_ON_CONDITION(COND, DO) \
00270 if(COND){ \
00271 DMDEBUG( cout<<" GOTO("<<tao_name<<":"<<decision_making::Event(#COND,plan_call_ctx)<< "->" #DO ") "; ) \
00272 DO;\
00273 }
00274
00275 #define TAO_RAISE(EVENT) \
00276 DMDEBUG( cout<<" RAISE("<<tao_name<<":"<<decision_making::Event(EVENT, plan_call_ctx)<<") "; ) \
00277 events_queue->raiseEvent(decision_making::Event(EVENT, plan_call_ctx));
00278
00279
00280 #define TAO_RISE(EVENT) TAO_RAISE(EVENT)
00281
00282 #define TAO_EVENTS_DROP events_queue->drop_all();
00283
00284 #define __DEFSUBEVENTQUEUE(TASK) decision_making::ScoppedThreads::EventQueuePtr events_queu##TASK( new decision_making::EventQueue(events_queue) );
00285 #define __DEFSUBCTEXT(TASK) decision_making::ScoppedThreads::CallContextPtr call_ctx##TASK( new decision_making::CallContext(plan_call_ctx, #TASK) );
00286 #define __SHR_TO_REF(X) (*(X.get()))
00287 #define TAO_CALL_TASK(TASK) \
00288 __DEFSUBEVENTQUEUE(TASK) __DEFSUBCTEXT(TASK) \
00289 SUBMACHINESTHREADS.add(events_queu##TASK); \
00290 SUBMACHINESTHREADS.add(call_ctx##TASK); \
00291 SUBMACHINESTHREADS.add(\
00292 new boost::thread( CALL_REMOTE(TASK, boost::ref(__SHR_TO_REF(call_ctx##TASK)), boost::ref(__SHR_TO_REF(events_queu##TASK))) ));
00293
00294 #define TAO_CALL_FSM(NAME) \
00295 __DEFSUBEVENTQUEUE(NAME) \
00296 SUBMACHINESTHREADS.add(events_queu##NAME); \
00297 SUBMACHINESTHREADS.add(\
00298 new boost::thread(boost::bind(&Fsm##NAME, &call_ctx, events_queu##NAME.get()) ));
00299 #define TAO_CALL_TAO(NAME) \
00300 __DEFSUBEVENTQUEUE(NAME) \
00301 SUBMACHINESTHREADS.add(events_queu##NAME); \
00302 SUBMACHINESTHREADS.add(\
00303 new boost::thread(boost::bind(&Tao##NAME, &call_ctx, events_queu##NAME.get()) ));
00304
00305
00306 #define TAO_CALL_BT(NAME) \
00307 __DEFSUBEVENTQUEUE(NAME) __DEFSUBCTEXT(NAME) \
00308 SUBMACHINESTHREADS.add(events_queu##NAME); \
00309 SUBMACHINESTHREADS.add(call_ctx##NAME); \
00310 decision_making::EventQueue& __t_events_queu##NAME = __SHR_TO_REF(events_queu##NAME);\
00311 decision_making::CallContext& __t_call_ctx##NAME = __SHR_TO_REF(call_ctx##NAME);\
00312 __BT_CREATE_BT_CALL_FUNCTION(NAME, __t_call_ctx##NAME, __t_events_queu##NAME)\
00313 SUBMACHINESTHREADS.add(\
00314 __CALL_BT_FUNCTION(NAME, boost::ref(__t_call_ctx##NAME), boost::ref(__t_events_queu##NAME)) \
00315 );
00316
00317 #define TAO_CLEANUP_BGN \
00318 class __ON_STATE_EXIT_STRUCT:public decision_making::ScoppedThreadsOnExit{public:\
00319 __ON_STATE_EXIT_STRUCT(CallContext& call_ctx, EventQueue* events_queue):decision_making::ScoppedThreadsOnExit(call_ctx, events_queue){}\
00320 virtual void exit(){
00321
00322 #define TAO_CLEANUP_END \
00323 }\
00324 };\
00325 decision_making::ScoppedThreadsOnExit* __tmp___ON_STATE_EXIT_STRUCT = new __ON_STATE_EXIT_STRUCT(call_ctx, events_queue);\
00326 SUBMACHINESTHREADS.add(decision_making::ScoppedThreads::ScoppedThreadsOnExitPtr(__tmp___ON_STATE_EXIT_STRUCT));
00327
00328 #define TAO_RESULT(EVENT, RESULT) \
00329 tao_stop=true; \
00330 TAO_RAISE(EVENT); \
00331 tao_result = RESULT; \
00332 break;
00333
00334 #define TAO_START_CONDITION(CONDITION) \
00335 if(justCondition){\
00336 tao_stop=true; \
00337 bool _l_cond_satis = (CONDITION);\
00338 tao_result = _l_cond_satis ? decision_making::TaskResult::SUCCESS() : decision_making::TaskResult::FAIL(#CONDITION" is False") ; \
00339 return tao_result;\
00340 }
00341
00342 #define __CLEAN_THREAD_AND_EVENTS decision_making::ScoppedThreads::Cleaner SUBMACHINESTHREADSCLEANER(SUBMACHINESTHREADS);
00343 #define TAO_TRANSITIONS __CLEAN_THREAD_AND_EVENTS {Event event; while((event=events_queue->waitEvent())==true){
00344
00345 #define TAO_NEXT(ProtocolName) }} {{\
00346 ProtocolName protocol(state,&plan_call_ctx, events_queue); \
00347 decision_making::ProtocolNext::Cleanner cleanner(protocol);
00348
00349 #define TAO_NEXT_EMPTY TAO_NEXT(_NextPEMPTY)
00350
00351 #define TAO_NEXT_PLAN(BEHNAME) protocol.add((int)BEHNAME, #BEHNAME, this_function->cond(BEHNAME).isSuccess());
00352
00353
00354 #define TAO_ALLOCATE(ProtocolName) {\
00355 ProtocolName protocol(state,&plan_call_ctx, events_queue); \
00356 __DEFSUBEVENTQUEUE(ALLOC)\
00357 decision_making::ProtocolAllocation::Cleanner cleanner(protocol, SUBMACHINESTHREADS ,events_queu##ALLOC);
00358
00359 #define TAO_ALLOCATE_EMPTY TAO_ALLOCATE(_AllocPEMPTY)
00360
00361
00362 #define TAO_SUBPLAN(TAONAME) \
00363 DMDEBUG( cout<<"[ADD ROLE: "<<#TAONAME<<"]"; )\
00364 protocol.add(#TAONAME, Tao##TAONAME##_cond(&plan_call_ctx, events_queue, -1).isSuccess(), boost::bind(&Tao##TAONAME, &plan_call_ctx, events_queu##ALLOC.get()));
00365
00366 #define TAO_STOP_CONDITION(COND) \
00367 }\
00368 TAO_TRANSITIONS\
00369 {\
00370 if(event==TAO_EVENT(SUBPLAN_FINISHED)){ break; }\
00371 if(COND){ break ;}\
00372 }
00373
00374 #define TAO_STOP_CONDITION_AND_PRINT_EVENTS(COND) \
00375 }\
00376 TAO_TRANSITIONS\
00377 {\
00378 TAO_PRINT_EVENT\
00379 if(event==TAO_EVENT(SUBPLAN_FINISHED)){ break; }\
00380 if(COND){ break ;}\
00381 }
00382
00383 #define TAO_DROP_EVENTS events_queue->drop_all();
00384
00385 #define TAO_PRINT_EVENT if(not event.equals(decision_making::Event::SPIN_EVENT())){ cout<<" READ("<<tao_name<<":"<<event<<") "; }
00386
00387 }
00388
00389
00390 #endif