00001 #include <iostream> 00002 00003 #include <boost/thread.hpp> 00004 #include <deque> 00005 #include <vector> 00006 #include <boost/thread/mutex.hpp> 00007 #include <boost/thread/condition.hpp> 00008 #include <boost/shared_ptr.hpp> 00009 00010 using namespace std; 00011 00012 #include "SynchCout.h" 00013 #include "EventSystem.h" 00014 00015 using namespace decision_making; 00016 00017 EventQueue mainEventQueue; 00018 00019 //=================== SYMULATION OF REAL SYSTEM ============================ 00020 00021 //enum EVENT_t{ 00022 // E_NOTHING, E_SUCCESS, E_FAIL 00023 //}; 00024 //std::ostream& operator<<(std::ostream& o, EVENT_t t){ 00025 // switch(t){ 00026 // case E_SUCCESS: return o<<"SUCC"; 00027 // case E_FAIL: return o<<"FAIL"; 00028 // default: break; 00029 // } 00030 // return o<<"*"; 00031 //} 00032 // 00033 //boost::mutex events_mutex; 00034 //boost::condition_variable on_new_event; 00035 //std::deque<EVENT_t> events; 00036 //bool events_system_stop = false; 00037 //void addEvent(EVENT_t e){ 00038 // boost::mutex::scoped_lock l(events_mutex); 00039 // events.push_back(e); 00040 // on_new_event.notify_one(); 00041 //} 00042 //EVENT_t waitEvent(){ 00043 // boost::mutex::scoped_lock l(events_mutex); 00044 // while(events_system_stop==false && events.empty()) on_new_event.wait(l); 00045 // if(events_system_stop) return E_NOTHING; 00046 // EVENT_t e = events.front(); 00047 // events.pop_front(); 00048 // return e; 00049 //} 00050 00051 void EVENTS_GENERATOR() { 00052 // Event spec[]={"SUCCESS", "FAIL", "SUCCESS", "FAIL", "SUCCESS", "FAIL", "SUCCESS", "FAIL", "GO", "NOTHING"}; 00053 Event spec[] = { "SUCCESS", "NOTHING" }; 00054 int i = 0; 00055 boost::this_thread::sleep(boost::posix_time::seconds(1)); 00056 while (true) { 00057 Event t = spec[i]; 00058 if (t == "NOTHING") { 00059 i = 1; 00060 t = spec[0]; 00061 } else 00062 i++; 00063 cout << endl << t << " -> "; 00064 mainEventQueue.riseEvent(t); 00065 boost::this_thread::sleep(boost::posix_time::seconds(1)); 00066 } 00067 } 00068 //void EVENTS_GENERATOR(){ 00069 // //EVENT_t spec[]={E_SUCCESS, E_FAIL, E_SUCCESS, E_FAIL, E_SUCCESS, E_FAIL, E_SUCCESS, E_FAIL, E_NOTHING}; 00070 // //EVENT_t spec[]={E_SUCCESS, E_FAIL, E_FAIL,E_SUCCESS,E_SUCCESS,E_SUCCESS,E_SUCCESS,E_SUCCESS,E_SUCCESS,E_SUCCESS,E_NOTHING}; 00071 // //EVENT_t spec[]={E_SUCCESS, E_NOTHING}; 00072 // EVENT_t spec[]={E_FAIL, E_NOTHING}; 00073 // int i=0; 00074 // boost::this_thread::sleep(boost::posix_time::seconds(1)); 00075 // while(true){ 00076 // EVENT_t t = spec[i]; 00077 // if(t == E_NOTHING){ i=1; t=spec[0]; }else i++; 00078 // std::cout<<endl<<t<<" -> "; 00079 // addEvent(t); 00080 // boost::this_thread::sleep(boost::posix_time::seconds(1)); 00081 // } 00082 //} 00083 00084 //==================== END OF SYMULATION OF REAL SYSTEM ====================== 00085 00086 #define X(x) x 00087 #define PAUSE boost::this_thread::sleep(boost::posix_time::seconds(1.3)); 00088 #define TIMES(N) for(size_t times=0;times<N;times++) 00089 00090 #include "BT.h" 00091 //#include "ROSTask.h" 00092 00093 //==================== CONNECTION TO REAL TASKS ============================== 00094 00095 //FROM BT 00096 TaskResult test_callTask(std::string task_address, const CallContext& call_ctx, EventQueue& queue) { 00097 cout << " TASK(" << task_address << ":CALL) "; 00098 Event e = mainEventQueue.waitEvent(); 00099 if (e == "SUCCESS") 00100 return TaskResult::SUCCESS(); 00101 return TaskResult::FAIL(); 00102 } 00103 00104 //FROM FSM 00105 //void callTask(std::string task_address, const CallContext& call_ctx, EventQueue* queue){ 00106 // cout<<" TASK("<<task_address<<":CALL) "; 00107 // while(true) 00108 // { 00109 // Event e = queue->waitEvent(); 00110 // if(not e){ 00111 // cout<<" TASK("<<task_address<<":TERMINATED) "; 00112 // return; 00113 // } 00114 // if( e=="/SUCCESS" or e=="/FAIL" or e=="/GO" ){ 00115 // Event new_event ( Event(""+e.event_name(), call_ctx) ); 00116 // cout<<" TASK("<<task_address<<":"<<new_event<<") "; 00117 // queue->riseEvent(new_event); 00118 // } 00119 // } 00120 //} 00121 00122 //#define CALL_REMOTE(NAME) test_callTask(#NAME, call_ctx, events) 00123 #define CALL_REMOTE(NAME, CALLS, EVENTS) boost::bind(&test_callTask, #NAME, CALLS, EVENTS) 00124 00125 //============================================================================= 00126 #include <DecisionMaking.h> 00127 00128 //BT_BGN(ST1_){ 00129 // PAR_BGN(P1){ 00130 // TASK_ROS(T1); 00131 // TASK_ROS(T2); 00132 // } 00133 // PAR_END(P1); 00134 //} 00135 //BT_END(ST1_); 00136 // 00137 // 00138 // 00139 //TaskResult BT_run_version_with_subtree_(){ 00140 // ROOT_BT_BGN(root){ 00141 // SEQ_BGN(S1){ 00142 // CALL_BT(ST1_); 00143 // TASK_ROS(T3); 00144 // TASK_ROS(T4); 00145 // } 00146 // SEQ_END(S1); 00147 // } 00148 // BT_END(root) main; 00149 // return main.run(); 00150 //} 00151 00152 //X( 00153 TaskResult BT_run_(){ 00154 BT_ROOT_BGN(root, mainEventQueue){ 00155 BT_PAR_BGN(P1){ 00156 00157 BT_DEC_WHILE_BGN(isSuccess()){ 00158 00159 BT_TASK_BGN(MY1){ 00160 cout<<"HELLO"<<endl; 00161 //createRosTaskCaller(selfPtrForRosTaskCaller); 00162 BT_TASK_RESULT( TaskResult::SUCCESS() ); 00163 } 00164 BT_TASK_END(MY1) 00165 00166 //boost::this_thread::sleep(boost::posix_time::seconds(5)); 00167 BT_SLEEP(5) 00168 }BT_DEC_WHILE_END 00169 00170 00171 BT_SET_TASK_RESULT_AFTER(TaskResult::FAIL(10,"EXIT"), 1) 00172 00173 // BT_SEQ_BGN(S1){ 00174 // BT_CALL_TASK(T1); 00175 // BT_CALL_TASK(T2); 00176 // } 00177 // BT_SEQ_END(S1); 00178 // BT_CALL_TASK(T3); 00179 // BT_CALL_TASK(T4); 00180 } 00181 BT_PAR_END(P1); 00182 } 00183 BT_END(root) main; 00184 TaskResult res = main.run(); 00185 cout<<"RES: "<<res<<endl; 00186 return res; 00187 } 00188 //) 00189 00190 //TaskResult BT_run_() { 00191 // cout<<"START"<<endl; 00192 // struct __BT_NODE_root_STRUCT: public BTNode { 00193 // BTContext _tmp_context; 00194 // BTContext& context; 00195 // __BT_NODE_root_STRUCT() : 00196 // BTNode(BT_SEQ, "root", decision_making::CallContext(), mainEventQueue), _tmp_context(), context(_tmp_context) { 00197 // } 00198 // __BT_NODE_root_STRUCT(BTContext& ctx, const decision_making::CallContext& calls, decision_making::EventQueue& events) : 00199 // BTNode(BT_SEQ, "root", calls, events), context(ctx) { 00200 // } 00201 // TaskResult run() { 00202 // cout<<"START: __BT_NODE_root_STRUCT"<<endl; 00203 // ; 00204 // BTNode* const & selfPtrForRosTaskCaller = this; 00205 // std::vector<boost::shared_ptr<BTNode> > __ALL_NODES; 00206 // { 00207 // { 00208 // struct __BT_NODE_P1_STRUCT; 00209 // typedef __BT_NODE_P1_STRUCT __LAST_BT_NODE_TYPE; 00210 // typedef boost::shared_ptr<BTNode> __LAST_BT_NODE_PTR; 00211 // struct __BT_NODE_P1_STRUCT: public BTNode { 00212 // typedef __BT_NODE_P1_STRUCT MY_NODE_TYPE; 00213 // BTContext& context; 00214 // std::string MY_NODE_NAME; 00215 // __BT_NODE_P1_STRUCT(BTNode* p, BTContext& ctx, const decision_making::CallContext& calls, decision_making::EventQueue& events) : 00216 // BTNode(BT_PAR, "P1", calls, events), context(ctx), MY_NODE_NAME("P1") { 00217 // p->tasks.push_back(this); 00218 // } 00219 // TaskResult run() { 00220 // cout<<"START: __BT_NODE_P1_STRUCT"<<endl; 00221 // 00222 // ; 00223 // BTNode* const & selfPtrForRosTaskCaller = this; 00224 // std::vector<boost::shared_ptr<BTNode> > __ALL_NODES; 00225 // { 00226 // 00227 // { 00228 // struct __BT_NODE___BT_DEC_NOT___COUNTER___STRUCT; 00229 // typedef __BT_NODE___BT_DEC_NOT___COUNTER___STRUCT __LAST_BT_NODE_TYPE; 00230 // typedef boost::shared_ptr<BTNode> __LAST_BT_NODE_PTR; 00231 // struct __BT_NODE___BT_DEC_NOT___COUNTER___STRUCT: public BTNode { 00232 // typedef __BT_NODE___BT_DEC_NOT___COUNTER___STRUCT MY_NODE_TYPE; 00233 // BTContext& context; 00234 // std::string MY_NODE_NAME; 00235 // __BT_NODE___BT_DEC_NOT___COUNTER___STRUCT(BTNode* p, BTContext& ctx, const decision_making::CallContext& calls, decision_making::EventQueue& events) : 00236 // BTNode(BT_TASK, "__BT_DEC_NOT___COUNTER__", calls, events), context(ctx), MY_NODE_NAME("__BT_DEC_NOT___COUNTER__") { 00237 // p->tasks.push_back(this); 00238 // } 00239 // TaskResult run() { 00240 // cout<<"START: __BT_NODE___BT_DEC_NOT___COUNTER___STRUCT"<<endl; 00241 // 00242 // ; 00243 // BTNode* const & selfPtrForRosTaskCaller = this; 00244 // std::vector<boost::shared_ptr<BTNode> > __ALL_NODES; 00245 // decision_making::TaskResult bt_node_return_value = decision_making::TaskResult::UNDEF(); 00246 // { 00247 // { 00248 // 00249 // { 00250 // struct __BT_NODE_MY1_STRUCT; 00251 // typedef __BT_NODE_MY1_STRUCT __LAST_BT_NODE_TYPE; typedef 00252 // boost::shared_ptr<BTNode> __LAST_BT_NODE_PTR; 00253 // struct __BT_NODE_MY1_STRUCT: public BTNode { 00254 // typedef __BT_NODE_MY1_STRUCT MY_NODE_TYPE; 00255 // BTContext& context; 00256 // std::string MY_NODE_NAME; 00257 // __BT_NODE_MY1_STRUCT(BTNode* p, BTContext& ctx, const decision_making::CallContext& calls, decision_making::EventQueue& events) : 00258 // BTNode(BT_TASK, "MY1", calls, events), context(ctx), MY_NODE_NAME("MY1") { 00259 // p->tasks.push_back(this); 00260 // } 00261 // TaskResult run() { 00262 // cout<<"START: __BT_NODE_MY1_STRUCT"<<endl; 00263 // 00264 // ; 00265 // BTNode* const & selfPtrForRosTaskCaller = this; 00266 // std::vector<boost::shared_ptr<BTNode> > __ALL_NODES; 00267 // decision_making::TaskResult bt_node_return_value = decision_making::TaskResult::UNDEF(); 00268 // { 00269 // Log() << "HELLO" << endl; 00270 // //createRosTaskCaller(selfPtrForRosTaskCaller); 00271 // bt_node_return_value = TaskResult::SUCCESS(); 00272 // } 00273 // this->run_all(); 00274 // ; 00275 // cout<<"RESULT: "<<bt_node_return_value<<" : "<< call_ctx.str()<<endl; 00276 // return bt_node_return_value; 00277 // } 00278 // }; 00279 // boost::shared_ptr<BTNode> __BT_NODE_MY1_INSTANCE((BTNode*) new __BT_NODE_MY1_STRUCT(this, context, call_ctx, events)); 00280 // __ALL_NODES.push_back(__BT_NODE_MY1_INSTANCE); 00281 // } 00282 // 00283 // } 00284 // } 00285 // this->run_all(); 00286 // ; 00287 // cout<<"RESULT: "<<bt_node_return_value<<" : "<< call_ctx.str()<<endl; 00288 // return bt_node_return_value; 00289 // } 00290 // }; 00291 // __LAST_BT_NODE_PTR __BT_NODE_11_INSTANCE((BTNode*)new __LAST_BT_NODE_TYPE(this, context, call_ctx, events)); 00292 // __ALL_NODES.push_back(__BT_NODE_11_INSTANCE); 00293 // }; 00294 // 00302 // } 00303 // this->run_all(); 00304 // ; 00305 // return bt_node_return_value; 00306 // } 00307 // }; 00308 // boost::shared_ptr<BTNode> __BT_NODE_P1_INSTANCE((BTNode*) new __BT_NODE_P1_STRUCT(this, context, call_ctx, events)); 00309 // __ALL_NODES.push_back(__BT_NODE_P1_INSTANCE); 00310 // }; 00311 // } 00312 // this->run_all(); 00313 // ; 00314 // return bt_node_return_value; 00315 // } 00316 // } main; 00317 // TaskResult res = main.run(); 00318 // Log() << "RES: " << res << endl; 00319 // return res; 00320 //} 00321 00400 // 00401 00402 00403 int main() { 00404 00405 boost::thread_group threads; 00406 threads.add_thread(new boost::thread(boost::bind(&BT_run_))); 00407 //threads.add_thread(new boost::thread(boost::bind(&BT_run_version_with_subtree_))); 00408 threads.add_thread(new boost::thread(boost::bind(&EVENTS_GENERATOR))); 00409 00410 threads.join_all(); 00411 00412 return 0; 00413 } 00414