Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 #include <iostream>
00010
00011
00012 #include "SynchCout.h"
00013 #include "EventSystem.h"
00014 #include "TAO.h"
00015 using namespace decision_making;
00016
00017 EventQueue mainEventQueue;
00018
00019 #define CALL_REMOTE(NAME, CALLS, EVENTS) boost::bind(&callTask, #NAME, CALLS, EVENTS)
00020
00021 #include "DecisionMaking.h"
00022
00023 void callTask(std::string task_address, const CallContext& call_ctx, EventQueue& queue){
00024 cout<<" TASK("<<task_address<<":CALL) ";
00025 while(true)
00026 {
00027 Event e = queue.waitEvent();
00028 if(not e){
00029 cout<<" TASK("<<task_address<<":TERMINATED) ";
00030 return;
00031 }
00032 if( e=="/SUCCESS" or e=="/FAIL" or e=="/GO" ){
00033 Event new_event ( Event(""+e.event_name(), call_ctx) );
00034 cout<<" TASK("<<task_address<<":"<<new_event<<") ";
00035 queue.riseEvent(new_event);
00036 if(e=="/FAIL" and task_address=="RRR") return;
00037 }
00038 }
00039 }
00040
00041
00042
00043 #define X(...) __VA_ARGS__
00044
00045
00046 class NextSelectFirst:public decision_making::ProtocolNext{
00047 public:
00048 NextSelectFirst(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):ProtocolNext(res, call_context, events){}
00049 bool decide(){
00050 cout<<"[decide for next of "<<call_context->str()<<" ]";
00051 for(size_t i=0;i<options.size();i++){
00052 if(options[i].isReady){
00053 cout<<" set next state = "<<options[i].id <<": "<<options[i].name<<endl;
00054 result = options[i].id;
00055 return true;
00056 }
00057 }
00058 return false;
00059 }
00060 };
00061
00062 class AllocFirstReady:public decision_making::ProtocolAllocation{
00063 public:
00064 AllocFirstReady(int& res, decision_making::CallContext* call_context, decision_making::EventQueue* events):ProtocolAllocation(res, call_context, events){}
00065 bool decide(){
00066 cout<<"[decide for next of "<<call_context->str()<<" ]";
00067 for(size_t i=0;i<options.size();i++){
00068 if(options[i].isReady){
00069 cout<<" set next state = "<<options[i].id <<": "<<options[i].name<<endl;
00070 result = options[i].id;
00071 return true;
00072 }
00073 }
00074 return false;
00075 }
00076 };
00077
00078 #define X(...) __VAR_ARGS__
00079
00080 TAO(TEST2){
00081 TAO_PLANS{
00082 A,B
00083 }
00084 TAO_START_PLAN(A);
00085 TAO_BGN{
00086 TAO_PLAN(A){
00087 TAO_START_CONDITION(false);
00088 TAO_ALLOCATE_EMPTY
00089 TAO_STOP_CONDITION(true);
00090 TAO_NEXT_EMPTY
00091 }
00092 TAO_PLAN(B){
00093 TAO_START_CONDITION(true);
00094 TAO_ALLOCATE_EMPTY
00095 TAO_STOP_CONDITION(true);
00096 TAO_NEXT_EMPTY
00097 }
00098 }
00099 TAO_END
00100 }
00101
00102
00103 TAO(TEST3){
00104 TAO_PLANS{
00105 A,B
00106 }
00107 TAO_START_PLAN(A);
00108 TAO_BGN{
00109 TAO_PLAN(A){
00110 TAO_START_CONDITION(false);
00111 TAO_ALLOCATE_EMPTY
00112 TAO_STOP_CONDITION(true);
00113 TAO_NEXT_EMPTY
00114 }
00115 TAO_PLAN(B){
00116 TAO_START_CONDITION(true);
00117 TAO_ALLOCATE_EMPTY
00118 TAO_STOP_CONDITION(true);
00119 TAO_NEXT_EMPTY
00120 }
00121 }
00122 TAO_END
00123 }
00124
00125 TAO(TEST1)
00126 {
00127 TAO_PLANS{
00128 C,
00129 D,
00130 S
00131 }
00132 TAO_START_PLAN(C);
00133 TAO_BGN{
00134 TAO_PLAN( C ){
00135 TAO_START_CONDITION(true);
00136 TAO_ALLOCATE(AllocFirstReady){
00137 TAO_SUBPLAN(TEST2);
00138 TAO_SUBPLAN(TEST3);
00139 }
00140 TAO_CALL_TASK(C);
00141 TAO_CLEANUP_BGN
00142 {
00143 TAO_CALL_TASK(RRR);
00144 }
00145 TAO_CLEANUP_END
00146 TAO_STOP_CONDITION(event == TAO_EVENT(/FAIL))
00147 TAO_NEXT(NextSelectFirst){
00148 TAO_NEXT_PLAN(D);
00149 TAO_NEXT_PLAN(S);
00150 }
00151 }
00152 TAO_PLAN( D ){
00153 TAO_START_CONDITION(true);
00154 TAO_CALL_TASK(D);
00155 TAO_ALLOCATE_EMPTY
00156 TAO_STOP_CONDITION(true);
00157 TAO_NEXT_EMPTY
00158 }
00159 TAO_PLAN( S ){
00160 TAO_START_CONDITION(true);
00161 TAO_RESULT("SUCCESS", TaskResult::SUCCESS());
00162 TAO_ALLOCATE_EMPTY
00163 TAO_STOP_CONDITION(true);
00164 TAO_NEXT_EMPTY
00165 }
00166 }
00167 TAO_END
00168 }
00169
00170
00171 void TAO_TEST(){
00172 TaskResult res = TaoTEST1(NULL,&mainEventQueue);
00173 cout<<"TAO_TEST: "<<res<<endl;
00174 }
00175
00176 void TAO_EVENTS_GENERATOR(){
00177 Event spec[]={"SUCCESS","SUCCESS","SUCCESS","SUCCESS","SUCCESS","SUCCESS", "FAIL", "NOTHING"};
00178 int i=0;
00179 while(true){
00180 Event t = spec[i];
00181 if(t == "NOTHING"){ i=1; t=spec[0]; }else i++;
00182 cout << endl << t<<" -> ";
00183 mainEventQueue.riseEvent(t);
00184 boost::this_thread::sleep(boost::posix_time::seconds(1));
00185 }
00186 }
00187
00188
00189 int main() {
00190 boost::thread_group threads;
00191 threads.add_thread(new boost::thread(boost::bind(&TAO_TEST)));
00192 threads.add_thread(new boost::thread(boost::bind(&TAO_EVENTS_GENERATOR)));
00193
00194 threads.join_all();
00195 }
00196
00197 int main1() {
00198 TaskResult
00199 res = TaoTEST1_cond(NULL,&mainEventQueue, 0);
00200 cout<<"TAO_TEST: "<<res<<endl;
00201 res = TaoTEST1_cond(NULL,&mainEventQueue, 1);
00202 cout<<"TAO_TEST: "<<res<<endl;
00203 res = TaoTEST1_cond(NULL,&mainEventQueue, 2);
00204 cout<<"TAO_TEST: "<<res<<endl;
00205 return 0;
00206 }
00207
00208
00209
00210
00211
00212
00213
00214