00001
00002
00003
00004
00005
00006
00007
00008 #ifndef BT_H_
00009 #define BT_H_
00010
00011 #include <boost/thread.hpp>
00012 #include <deque>
00013 #include <vector>
00014 #include <boost/thread/mutex.hpp>
00015 #include <boost/thread/condition.hpp>
00016 #include <boost/shared_ptr.hpp>
00017 #include <boost/preprocessor/slot/counter.hpp>
00018
00019 #include "EventSystem.h"
00020 #include "TaskResult.h"
00021
00022 #ifndef DMDEBUG
00023 #define DMDEBUG(...)
00024 #endif
00025
00026 namespace decision_making{
00027
00028 struct BTContext{};
00029 struct BTNode{
00030 enum BTNodeTYPE{ BT_SEQ, BT_SEL, BT_PAR, BT_TASK, BT_PAR_SEQ, BT_PAR_SEL };
00031 BTNodeTYPE type;
00032 boost::thread_group threads;
00033 std::vector< BTNode* > tasks;
00034 boost::mutex m;
00035 bool terminated;
00036 bool finished;
00037 boost::condition_variable on_child_terminated;
00038 TaskResult bt_node_return_value;
00039 CallContext call_ctx;
00040 EventQueue events;
00041 string node_name;
00042 #define BTNodeContructorParams_DEF BTNodeTYPE t, string node_name, const CallContext& parent_call_ctx, EventQueue& events
00043 #define BTNodeContructorParams_USE parent_call_ctx, events
00044 BTNode(BTNodeContructorParams_DEF):
00045 type(t),
00046 terminated(false),
00047 finished(false),
00048 bt_node_return_value(TaskResult::UNDEF()),
00049 node_name(node_name),
00050 call_ctx(parent_call_ctx, node_name),
00051 events(&events, true)
00052 {}
00053 virtual ~BTNode(){}
00054 virtual TaskResult run(){return TaskResult::UNDEF();};
00055 void run_task(BTNode* t){
00056 TaskResult ret = run();
00057
00058 {boost::mutex::scoped_lock l(t->m);
00059 if(t->bt_node_return_value.isUndefined() || t->type!=BT_PAR){
00060 t->bt_node_return_value = ret;
00061 }
00062 t->on_child_terminated.notify_one();
00063 }
00064 };
00065 void run_thread(BTNode* BTNode){
00066 threads.add_thread(new boost::thread (boost::bind(&BTNode::run_task,BTNode, this)));
00067 }
00068 void join(){ threads.join_all(); }
00069 void run_all(){
00070 if(type==BT_PAR){
00071 for(size_t i=0;i<tasks.size();i++){
00072 tasks[i]->bt_node_return_value = bt_node_return_value;
00073 run_thread(tasks[i]);
00074 }
00075 {boost::mutex::scoped_lock l(m);
00076 on_child_terminated.wait(l);
00077 _us_terminate();
00078 }
00079 DMDEBUG( cout<<"[par: someone finished]"; )
00080 join();
00081 }else
00082 if(type==BT_SEQ){
00083 for(size_t i=0;i<tasks.size() && isTerminated()==false && not bt_node_return_value.isFail();i++){
00084 tasks[i]->bt_node_return_value = bt_node_return_value;
00085 bt_node_return_value = tasks[i]->run();
00086 }
00087 }else
00088 if(type==BT_SEL){
00089 for(size_t i=0;i<tasks.size() && isTerminated()==false && not bt_node_return_value.isSuccess();i++){
00090 tasks[i]->bt_node_return_value = bt_node_return_value;
00091 bt_node_return_value = tasks[i]->run();
00092 }
00093 }else
00094 if(type==BT_TASK){
00095
00096 }else
00097 if(type==BT_PAR_SEQ){
00098 size_t tasks_count = tasks.size();
00099 for(size_t i=0;i<tasks.size();i++){
00100 tasks[i]->bt_node_return_value = bt_node_return_value;
00101 run_thread(tasks[i]);
00102 }
00103 TaskResult _ret = TaskResult::UNDEF();
00104 {boost::mutex::scoped_lock l(m);
00105 on_child_terminated.wait(l);
00106 tasks_count--;
00107 if(bt_node_return_value.isFail() || tasks_count==0){
00108 _ret = bt_node_return_value;
00109 _us_terminate();
00110 }
00111 }
00112 join();
00113 bt_node_return_value = _ret;
00114 }else
00115 if(type==BT_PAR_SEL){
00116 size_t tasks_count = tasks.size();
00117 for(size_t i=0;i<tasks.size();i++){
00118 tasks[i]->bt_node_return_value = bt_node_return_value;
00119 run_thread(tasks[i]);
00120 }
00121 TaskResult _ret = TaskResult::UNDEF();
00122 {boost::mutex::scoped_lock l(m);
00123 on_child_terminated.wait(l);
00124 tasks_count--;
00125 if(bt_node_return_value.isSuccess() || tasks_count==0)
00126 _ret = bt_node_return_value;
00127 _us_terminate();
00128 }
00129 join();
00130 bt_node_return_value = _ret;
00131 }
00132 {boost::mutex::scoped_lock l(m);
00133 finished = true;
00134 }
00135 }
00136 void _us_terminate(){
00137 if(finished) return;
00138 terminated = true;
00139 events.close();
00140 for(size_t i=0;i<tasks.size();i++)tasks[i]->terminate();
00141 }
00142 void terminate(){
00143 boost::mutex::scoped_lock l(m);
00144 _us_terminate();
00145 }
00146 bool isTerminated(){
00147 boost::mutex::scoped_lock l(m);
00148 return terminated;
00149 }
00150 };
00151
00152 static EventQueue& __tmp_event_queue(){static EventQueue tmp; return tmp; };
00153
00154 typedef BTNode CurrentNodeType;
00155
00156 #define BT_TASK_RESULT(X) bt_node_return_value = X
00157 #define BT_NODE(NAME) __BT_NODE_##NAME##_INSTANCE
00158 #define BT_NODE_TYPE(NAME) __BT_NODE_##NAME##_STRUCT
00159 #define BT_NODE_PTR(NAME) boost::shared_ptr<BTNode>
00160 #define BT_NODE_NEW_PTR(NAME) BT_NODE_PTR(NAME)((BTNode*)new BT_NODE_TYPE(NAME)(this))
00161 #define BT_LAST_NODE __ALL_NODES.back()
00162
00163 #define BT_RUN_LAST_NODE BT_LAST_NODE->run()
00164 #define BT_RUN_NODE(NAME) BT_NODE(NODE)->run()
00165
00166 #define BT_RENAME_INNER_CONTEXT(NEW_NAME) typedef BTContext NEW_NAME##Type; NEW_NAME##Type& NEW_NAME=context;
00167 #define BT_NEW_INNER_CONTEXT(...) struct BTContext{__VA_ARGS__} context
00168
00169 #define BT_CONTEXT calls
00170 #define BT_INNER_CONTEXT context
00171
00172 #define BT_CALL_BT(NAME) \
00173 BT_NODE_PTR(NAME) BT_NODE(NAME)((BTNode*)new BT_NODE_TYPE(NAME)(this, context, call_ctx, events));\
00174 __ALL_NODES.push_back(BT_NODE(NAME));\
00175 CUR_NODE = BT_NODE(NAME);
00176
00177 #define __BT_CALL_BT_NONAME(NAME) \
00178 __LAST_BT_NODE_PTR BT_NODE(NAME)((BTNode*)new __LAST_BT_NODE_TYPE(this, context, call_ctx, events));\
00179 __ALL_NODES.push_back(BT_NODE(NAME));\
00180 CUR_NODE = BT_NODE(NAME);
00181
00182 #define __BT_CALL_BT_CALLER(NAME) \
00183 call_ctx.pop(); \
00184 BT_NODE_PTR(NAME) BT_NODE(NAME)((BTNode*)new BT_NODE_TYPE(NAME)(this, context, call_ctx, events));\
00185 __ALL_NODES.push_back(BT_NODE(NAME))
00186
00187
00188
00189 #define BT_HEADER(NAME) struct BT_NODE_TYPE(NAME);
00190
00191
00192
00193 #define BT_ROOT_BGN(NAME,EVENTS)\
00194 struct BT_NODE_TYPE(NAME):public BTNode\
00195 { \
00196 BTContext _tmp_context; BTContext& context;\
00197 BT_NODE_TYPE(NAME)():BTNode(BT_SEQ, #NAME, decision_making::CallContext(), EVENTS),_tmp_context(),context(_tmp_context){}\
00198 BT_NODE_TYPE(NAME)(BTContext& ctx, const decision_making::CallContext& calls, decision_making::EventQueue& events):BTNode(BT_SEQ, #NAME, calls, events),context(ctx){}\
00199 TaskResult run(){\
00200 DMDEBUG( cout<<" [BT:" #NAME " MAIN" "]{ "; ) \
00201 ON_BT_NODE_START(#NAME, "ROOT", call_ctx, events);\
00202 BTNode* const& selfPtrForRosTaskCaller=this;\
00203 std::vector< BT_NODE_PTR() > __ALL_NODES;
00204
00205 #define __BT_NODE_BGN(TYPE, NAME, STR)\
00206 struct BT_NODE_TYPE(NAME):public BTNode\
00207 { \
00208 typedef BT_NODE_TYPE(NAME) MY_NODE_TYPE;\
00209 BTContext& context;\
00210 std::string MY_NODE_NAME;\
00211 BT_NODE_TYPE(NAME)(BTNode* p, BTContext& ctx, const decision_making::CallContext& calls, decision_making::EventQueue& events):BTNode(TYPE, #NAME, calls, events),context(ctx),MY_NODE_NAME(#NAME){\
00212 p->tasks.push_back(this);\
00213 }\
00214 TaskResult run()\
00215 {\
00216 DMDEBUG( cout<<" [BT:" #NAME STR "]{ "; ) \
00217 ON_BT_NODE_START(#NAME, #TYPE, call_ctx, events);\
00218 BTNode* const& selfPtrForRosTaskCaller=this;\
00219 std::vector<BT_NODE_PTR()> __ALL_NODES;
00220
00221 #define __BT_END_NODE(NAME)\
00222 this->run_all();\
00223 DMDEBUG( cout<<" }[BT:" #NAME "] "; )\
00224 ON_BT_NODE_END(#NAME, call_ctx, events, bt_node_return_value);\
00225 return bt_node_return_value;\
00226 }\
00227 }
00228 #define __BT_END_NODE_NONAME\
00229 this->run_all();\
00230 DMDEBUG( cout<<" }[BT:" <<MY_NODE_NAME<< "] "; )\
00231 ON_BT_NODE_END(MY_NODE_NAME, call_ctx, events, bt_node_return_value);\
00232 return bt_node_return_value;\
00233 }\
00234 }
00235
00236 #define __BT_PREDEF(NAME) \
00237 BT_NODE_PTR(NAME) BT_NODE(NAME); \
00238 {\
00239 BT_NODE_PTR(NAME)& CUR_NODE=BT_NODE(NAME);\
00240 struct BT_NODE_TYPE(NAME);\
00241 typedef BT_NODE_TYPE(NAME) __LAST_BT_NODE_TYPE;\
00242 typedef BT_NODE_PTR(NAME) __LAST_BT_NODE_PTR;
00243
00244 #define __BT_POSTDEF \
00245 ;}
00246
00247 #define BT_BGN(NAME) __BT_NODE_BGN(BT_SEQ, NAME, "=>")
00248
00249
00250
00251
00252
00253 #define BT_PAR_BGN(NAME) __BT_PREDEF(NAME) __BT_NODE_BGN(BT_PAR, NAME, "||")
00254 #define BT_SEL_BGN(NAME) __BT_PREDEF(NAME) __BT_NODE_BGN(BT_SEL, NAME, "??")
00255 #define BT_SEQ_BGN(NAME) __BT_PREDEF(NAME) __BT_NODE_BGN(BT_SEQ, NAME, "->")
00256 #define BT_TASK_BGN(NAME) __BT_PREDEF(NAME) __BT_NODE_BGN(BT_TASK, NAME, "") decision_making::TaskResult bt_node_return_value = decision_making::TaskResult::UNDEF();
00257
00258 #define BT_END(NAME) __BT_END_NODE(NAME)
00259
00260 #define __BT_END_TASK(NAME) __BT_END_NODE(NAME); BT_CALL_BT(NAME)
00261 #define __BT_END_TASK_NONAME __BT_END_NODE_NONAME; __BT_CALL_BT_NONAME(__COUNTER__)
00262
00263
00264
00265
00266
00267
00268 #define BT_PAR_END(NAME) __BT_END_TASK(NAME) __BT_POSTDEF
00269 #define BT_SEL_END(NAME) __BT_END_TASK(NAME) __BT_POSTDEF
00270 #define BT_SEQ_END(NAME) __BT_END_TASK(NAME) __BT_POSTDEF
00271 #define BT_TASK_END(NAME) __BT_END_TASK(NAME) __BT_POSTDEF
00272 #define BT_TASK_END_NONAME __BT_END_TASK_NONAME __BT_POSTDEF
00273
00274
00275
00276 #define __BTDEFSUBEVENTQUEUE(NAME) decision_making::EventQueue events_##NAME(&events);
00277
00278
00279 #define __BTDEFSUBCTEXT(NAME) decision_making::CallContext& call_ctx_##NAME(call_ctx);
00280
00281 #define BT_CALL_TASK(NAME)\
00282 __BT_NODE_BGN(BT_TASK, NAME, "")\
00283 __BTDEFSUBEVENTQUEUE(NAME) __BTDEFSUBCTEXT(NAME)\
00284 bt_node_return_value = CALL_REMOTE(NAME,boost::ref(call_ctx_##NAME), boost::ref(events_##NAME))(); \
00285 __BT_END_TASK(NAME)
00286 #define BT_CALL_FSM(NAME)\
00287 __BT_NODE_BGN(BT_TASK, NAME, "")\
00288 call_ctx.pop();\
00289 bt_node_return_value = Fsm##NAME(&call_ctx, &events);\
00290 __BT_END_TASK(NAME)
00291
00292 #define BT_RAISE(EVENT) \
00293 DMDEBUG( cout<<" RAISE("<<node_name<<":"<<decision_making::Event(EVENT, call_ctx)<<") "; ) \
00294 events.raiseEvent(decision_making::Event(EVENT, call_ctx));
00295
00296
00297 #define BT_RISE(EVENT) BT_RAISE(EVENT)
00298
00299 #define ___BTMERGE_(a,b) a##b
00300 #define ___BTLABEL_(p,a) ___BTMERGE_(p, a)
00301 #define ___BTUNIQUE_NAME(p) ___BTLABEL_(p,__COUNTER__)
00302
00303
00304
00305
00306
00307 #define BT_DEC_NOT_BGN \
00308 BT_TASK_BGN(___BTUNIQUE_NAME(_lbl_BT_DEC_NOT_)){
00309
00310 #define BT_DEC_NOT_END \
00311 decision_making::TaskResult res1 = BT_LAST_NODE->run();\
00312 BT_TASK_RESULT( res1==decision_making::TaskResult::SUCCESS()?decision_making::TaskResult::FAIL("NOT"):decision_making::TaskResult::SUCCESS() );\
00313 }BT_TASK_END_NONAME;
00314
00315
00316
00317
00318 #define BT_DEC_SUCCESS_BGN \
00319 BT_TASK_BGN(___BTUNIQUE_NAME(_lbl_BT_DEC_SUCCESS_)){
00320
00321 #define BT_DEC_SUCCESS_END \
00322 decision_making::TaskResult res1 = BT_LAST_NODE->run();\
00323 BT_TASK_RESULT( decision_making::TaskResult::SUCCESS() );\
00324 }BT_TASK_END_NONAME;
00325
00326
00327
00328 #define BT_DEC_FAIL_BGN(ERROR_CODE) \
00329 BT_TASK_BGN(___BTUNIQUE_NAME(_lbl_BT_DEC_FAIL_)){ int _error_code = ERROR_CODE;
00330
00331 #define BT_DEC_FAIL_END \
00332 decision_making::TaskResult res1 = BT_LAST_NODE->run();\
00333 BT_TASK_RESULT( decision_making::TaskResult::FAIL(_error_code) );\
00334 }BT_TASK_END_NONAME;
00335
00336
00337
00338
00339 #define BT_DEC_WHILE_BGN(CONDITION) \
00340 BT_TASK_BGN(___BTUNIQUE_NAME(_lbl_BT_DEC_WHILE_)){ \
00341 struct CHECK_RESULT{\
00342 decision_making::TaskResult task_result;\
00343 bool check(){ return task_result.CONDITION ;}\
00344 } _CHECK_RESULT;\
00345 do{
00346
00347 #define BT_DEC_WHILE_END \
00348 if(not isTerminated())\
00349 _CHECK_RESULT.task_result = BT_LAST_NODE->run(); }while( _CHECK_RESULT.check() and not isTerminated() );\
00350 BT_TASK_RESULT( _CHECK_RESULT.task_result );\
00351 }BT_TASK_END_NONAME;
00352
00353
00354
00355
00356 #define __MAX(x,y) ((x)>(y)?(x):(y))
00357 #define __MIN(x,y) ((x)<(y)?(x):(y))
00358
00359 #define BT_SLEEP(time)\
00360 for(int i=0;i<__MAX(1,(float(time)/0.5F)) and not isTerminated();i++){ boost::this_thread::sleep(boost::posix_time::milliseconds(1000*__MIN(0.5F,time-0.5F*i))); }
00361
00362 #define BT_SET_TASK_RESULT(RESULT) \
00363 BT_TASK_BGN(___BTUNIQUE_NAME(_lbl_BT_SET_TASK_RESULT_)){ \
00364 BT_TASK_RESULT( RESULT );\
00365 }BT_TASK_END_NONAME;
00366
00367 #define BT_SET_TASK_RESULT_AFTER(RESULT, time) \
00368 BT_TASK_BGN(___BTUNIQUE_NAME(_lbl_BT_SET_TASK_RESULT_)){ \
00369 BT_SLEEP(time)\
00370 BT_TASK_RESULT( RESULT );\
00371 }BT_TASK_END_NONAME;
00372
00373
00374
00375 #if USE_SHORT_BT_SYNTAX
00376
00377 #define TASK(NAME,...)\
00378 TASK_BGN(NAME){\
00379 __VA_ARGS__ \
00380 }END_TASK(NAME)
00381
00382 #define PAR(NAME,...)\
00383 PAR_BGN(NAME){\
00384 __VA_ARGS__ \
00385 }END_TASK(NAME)
00386 #define SEQ(NAME,...)\
00387 SEQ_BGN(NAME){\
00388 __VA_ARGS__ \
00389 }END_TASK(NAME)
00390 #define SEL(NAME,...)\
00391 SEL_BGN(NAME){\
00392 __VA_ARGS__ \
00393 END_LIST;\
00394 }END_TASK(NAME)
00395
00396 #define TREE(NAME,...)\
00397 BT_BGN(NAME){\
00398 __VA_ARGS__ \
00399 }BT_END(NAME)
00400 #define ROOT(NAME,...)\
00401 ROOT_BT_BGN(NAME){\
00402 __VA_ARGS__ \
00403 }BT_END(NAME)
00404
00405 #define NOT(...) do{TASK(DECOR_NOT_,\
00406 __VA_ARGS__;\
00407 TaskResult res = LAST_NODE->run();\
00408 TASK_RESULT( res==E_SUCCESS?E_FAIL:E_SUCCESS );\
00409 );}while(0)
00410 #endif
00411
00412
00413
00414
00415 struct BTCaller{
00416 std::string task_address;
00417 decision_making::CallContext& call_ctx;
00418 decision_making::EventQueue& queue;
00419 BTCaller(std::string task_address, decision_making::CallContext& call_ctx, decision_making::EventQueue& queue):
00420 task_address(task_address), call_ctx(call_ctx), queue(queue)
00421 {}
00422 virtual ~BTCaller(){}
00423 virtual decision_making::TaskResult _bt_function() =0;
00424 boost::thread* getThread(){ return new boost::thread( boost::bind( &BTCaller::_bt_function, this )); }
00425 };
00426
00427 #define __BT_CREATE_BT_CALL_FUNCTION(BTNAME, P_call_ctx, P_events_queu)\
00428 struct _bt_function_struct##BTNAME: public decision_making::BTCaller{\
00429 _bt_function_structBT1(std::string task_address, decision_making::CallContext& call_ctx, decision_making::EventQueue& queue):decision_making::BTCaller(task_address,call_ctx,queue){}\
00430 decision_making::TaskResult _bt_function(){\
00431 BTContext _tmp_context;\
00432 BT_ROOT_BGN(bt_from_fsm, __tmp_event_queue()){\
00433 call_ctx.pop();\
00434 BT_PAR_BGN(TMP){\
00435 call_ctx.pop();\
00436 BT_TASK_BGN(STOP_CONDITION){\
00437 while(not isTerminated())\
00438 {\
00439 decision_making::Event e = events.waitEvent();\
00440 if(not e){\
00441 DMDEBUG( cout<<" (STOP_CONDITION:TERMINATED) "; )\
00442 BT_TASK_RESULT( decision_making::TaskResult::TERMINATED() );\
00443 break;\
00444 }\
00445 }\
00446 }\
00447 BT_TASK_END(STOP_CONDITION);\
00448 __BT_CALL_BT_CALLER(BTNAME);\
00449 }\
00450 BT_PAR_END(TMP);\
00451 }\
00452 BT_END(bt_from_fsm) bt_from_fsm(_tmp_context, call_ctx, queue);\
00453 decision_making::TaskResult res = bt_from_fsm.run();\
00454 DMDEBUG( cout<<"(fsm from function finished)"; )\
00455 return res;\
00456 }\
00457 } _bt_function_struct_instanceBT1(#BTNAME, P_call_ctx, P_events_queu);
00458
00459 #define __CALL_BT_FUNCTION(NAME, CALLS, EVENTS) _bt_function_struct_instance##BT1.getThread()
00460
00461
00462 }
00463
00464 #endif